Visual Servoing Platform  version 3.6.1 under development (2025-01-14)
vpRotationMatrix.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Rotation matrix.
32  */
33 
40 #include <visp3/core/vpMath.h>
41 #include <visp3/core/vpMatrix.h>
42 
43 // Rotation classes
44 #include <visp3/core/vpRotationMatrix.h>
45 
46 // Exception
47 #include <visp3/core/vpException.h>
48 
49 // Debug trace
50 #include <math.h>
51 
52 BEGIN_VISP_NAMESPACE
53 const unsigned int vpRotationMatrix::constr_val_3 = 3;
60 {
61  const unsigned int val_3 = 3;
62  for (unsigned int i = 0; i < val_3; ++i) {
63  for (unsigned int j = 0; j < val_3; ++j) {
64  if (i == j) {
65  (*this)[i][j] = 1.0;
66  }
67  else {
68  (*this)[i][j] = 0.0;
69  }
70  }
71  }
72 }
73 
84 {
85  const unsigned int val_3 = 3;
86  for (unsigned int i = 0; i < val_3; ++i) {
87  for (unsigned int j = 0; j < val_3; ++j) {
88  rowPtrs[i][j] = R.rowPtrs[i][j];
89  }
90  }
91 
92  return *this;
93 }
94 
95 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
123 vpRotationMatrix &vpRotationMatrix::operator=(const std::initializer_list<double> &list)
124 {
125  if (dsize != static_cast<unsigned int>(list.size())) {
127  "Cannot set a 3-by-3 rotation matrix from a %d-elements list of doubles."));
128  }
129 
130  std::copy(list.begin(), list.end(), data);
131 
132  if (!isARotationMatrix()) {
133  if (isARotationMatrix(1e-3)) {
134  orthogonalize();
135  }
136  else {
137  throw(vpException(
139  "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
140  }
141  }
142 
143  return *this;
144 }
145 #endif
146 
164 {
165  const unsigned int val_3 = 3;
166  if ((M.getCols() != val_3) && (M.getRows() != val_3)) {
167  throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
168  M.getRows(), M.getCols()));
169  }
170 
171  for (unsigned int i = 0; i < val_3; ++i) {
172  for (unsigned int j = 0; j < val_3; ++j) {
173  (*this)[i][j] = M[i][j];
174  }
175  }
176 
177  if (isARotationMatrix() == false) {
178  throw(vpException(vpException::fatalError, "Cannot set a rotation matrix "
179  "from a matrix that is not a "
180  "rotation matrix"));
181  }
182 
183  return *this;
184 }
185 
218 {
219  m_index = 0;
220  data[m_index] = val;
221  return *this;
222 }
223 
256 {
257  ++m_index;
258  if (m_index >= size()) {
260  "Cannot set rotation matrix out of bounds. It has only %d elements while you try to initialize "
261  "with %d elements",
262  size(), m_index + 1));
263  }
264  data[m_index] = val;
265  return *this;
266 }
267 
272 {
274 
275  const unsigned int val_3 = 3;
276  for (unsigned int i = 0; i < val_3; ++i) {
277  for (unsigned int j = 0; j < val_3; ++j) {
278  double s = 0;
279  for (unsigned int k = 0; k < val_3; ++k) {
280  s += rowPtrs[i][k] * R.rowPtrs[k][j];
281  }
282  p[i][j] = s;
283  }
284  }
285  return p;
286 }
287 
306 {
307  const unsigned int val_3 = 3;
308  if ((M.getRows() != val_3) || (M.getCols() != val_3)) {
309  throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
310  M.getRows(), M.getCols()));
311  }
312  vpMatrix p(3, 3);
313 
314  for (unsigned int i = 0; i < val_3; ++i) {
315  for (unsigned int j = 0; j < val_3; ++j) {
316  double s = 0;
317  for (unsigned int k = 0; k < val_3; ++k) {
318  s += (*this)[i][k] * M[k][j];
319  }
320  p[i][j] = s;
321  }
322  }
323  return p;
324 }
325 
341 {
342  return (vpHomogeneousMatrix(*this * M.getTranslationVector(), *this * M.getRotationMatrix()));
343 }
344 
379 {
380  const unsigned int rows_size = 3;
381  if (v.getRows() != rows_size) {
383  "Cannot multiply a (3x3) rotation matrix by a %d "
384  "dimension column vector",
385  v.getRows()));
386  }
387  vpColVector v_out(3);
388 
389  for (unsigned int j = 0; j < colNum; ++j) {
390  double vj = v[j]; // optimization em 5/12/2006
391  for (unsigned int i = 0; i < rowNum; ++i) {
392  v_out[i] += rowPtrs[i][j] * vj;
393  }
394  }
395 
396  return v_out;
397 }
398 
404 {
406  const unsigned int val_3 = 3;
407 
408  for (unsigned int j = 0; j < val_3; ++j) {
409  p[j] = 0;
410  }
411 
412  for (unsigned int j = 0; j < val_3; ++j) {
413  for (unsigned int i = 0; i < val_3; ++i) {
414  p[i] += rowPtrs[i][j] * tv[j];
415  }
416  }
417 
418  return p;
419 }
420 
426 {
428 
429  for (unsigned int i = 0; i < rowNum; ++i) {
430  for (unsigned int j = 0; j < colNum; ++j) {
431  R[i][j] = rowPtrs[i][j] * x;
432  }
433  }
434 
435  return R;
436 }
437 
443 {
444  for (unsigned int i = 0; i < rowNum; ++i) {
445  for (unsigned int j = 0; j < colNum; ++j) {
446  rowPtrs[i][j] *= x;
447  }
448  }
449 
450  return *this;
451 }
452 
453 /*********************************************************************/
454 
461 bool vpRotationMatrix::isARotationMatrix(double threshold) const
462 {
463  bool isRotation = true;
464  const unsigned int val_3 = 3;
465 
466  if ((getCols() != val_3) || (getRows() != val_3)) {
467  return false;
468  }
469 
470  // --comment: test R^TR = Id
471  vpRotationMatrix RtR = (*this).t() * (*this);
472  for (unsigned int i = 0; i < val_3; ++i) {
473  for (unsigned int j = 0; j < val_3; ++j) {
474  if (i == j) {
475  if (fabs(RtR[i][j] - 1) > threshold) {
476  isRotation = false;
477  }
478  }
479  else {
480  if (fabs(RtR[i][j]) > threshold) {
481  isRotation = false;
482  }
483  }
484  }
485  }
486  // test if it is a basis
487  // test || Ci || = 1
488  const unsigned int index_2 = 2;
489  for (unsigned int i = 0; i < val_3; ++i) {
490  if ((sqrt(vpMath::sqr(RtR[0][i]) + vpMath::sqr(RtR[1][i]) + vpMath::sqr(RtR[index_2][i])) - 1) > threshold) {
491  isRotation = false;
492  }
493  }
494 
495  // test || Ri || = 1
496  for (unsigned int i = 0; i < val_3; ++i) {
497  if ((sqrt(vpMath::sqr(RtR[i][0]) + vpMath::sqr(RtR[i][1]) + vpMath::sqr(RtR[i][index_2])) - 1) > threshold) {
498  isRotation = false;
499  }
500  }
501 
502  // test if the basis is orthogonal
503  return isRotation;
504 }
505 
509 vpRotationMatrix::vpRotationMatrix() : vpArray2D<double>(constr_val_3, constr_val_3), m_index(0) { eye(); }
510 
515 vpRotationMatrix::vpRotationMatrix(const vpRotationMatrix &M) : vpArray2D<double>(constr_val_3, constr_val_3), m_index(0) { (*this) = M; }
516 
520 vpRotationMatrix::vpRotationMatrix(const vpHomogeneousMatrix &M) : vpArray2D<double>(constr_val_3, constr_val_3), m_index(0) { buildFrom(M); }
521 
526 vpRotationMatrix::vpRotationMatrix(const vpThetaUVector &tu) : vpArray2D<double>(constr_val_3, constr_val_3), m_index(0) { buildFrom(tu); }
527 
531 vpRotationMatrix::vpRotationMatrix(const vpPoseVector &p) : vpArray2D<double>(constr_val_3, constr_val_3), m_index(0) { buildFrom(p); }
532 
537 vpRotationMatrix::vpRotationMatrix(const vpRzyzVector &euler) : vpArray2D<double>(constr_val_3, constr_val_3), m_index(0)
538 {
539  buildFrom(euler);
540 }
541 
546 vpRotationMatrix::vpRotationMatrix(const vpRxyzVector &Rxyz) : vpArray2D<double>(constr_val_3, constr_val_3), m_index(0) { buildFrom(Rxyz); }
547 
552 vpRotationMatrix::vpRotationMatrix(const vpRzyxVector &Rzyx) : vpArray2D<double>(constr_val_3, constr_val_3), m_index(0) { buildFrom(Rzyx); }
553 
557 vpRotationMatrix::vpRotationMatrix(const vpMatrix &R) : vpArray2D<double>(constr_val_3, constr_val_3), m_index(0) { *this = R; }
558 
563 vpRotationMatrix::vpRotationMatrix(double tux, double tuy, double tuz) : vpArray2D<double>(constr_val_3, constr_val_3), m_index(0)
564 {
565  buildFrom(tux, tuy, tuz);
566 }
567 
571 vpRotationMatrix::vpRotationMatrix(const vpQuaternionVector &q) : vpArray2D<double>(constr_val_3, constr_val_3), m_index(0) { buildFrom(q); }
572 
573 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
599 vpRotationMatrix::vpRotationMatrix(const std::initializer_list<double> &list)
600  : vpArray2D<double>(3, 3, list), m_index(0)
601 {
602  if (!isARotationMatrix()) {
603  if (isARotationMatrix(1e-3)) {
604  orthogonalize();
605  }
606  else {
607  throw(vpException(
609  "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
610  }
611  }
612 }
613 #endif
614 
622 {
623  vpRotationMatrix Rt;
624 
625  const unsigned int val_3 = 3;
626  for (unsigned int i = 0; i < val_3; ++i) {
627  for (unsigned int j = 0; j < val_3; ++j) {
628  Rt[j][i] = (*this)[i][j];
629  }
630  }
631 
632  return Rt;
633 }
634 
642 {
643  vpRotationMatrix Ri = (*this).t();
644 
645  return Ri;
646 }
647 
670 
676 {
677  vpThetaUVector tu(*this);
678 
679  const unsigned int val_3 = 3;
680  for (unsigned int i = 0; i < val_3; ++i) {
681  std::cout << tu[i] << " ";
682  }
683 
684  std::cout << std::endl;
685 }
686 
697 {
698  double theta, si, co, sinc, mcosc;
700 
701  const unsigned int index_0 = 0;
702  const unsigned int index_1 = 1;
703  const unsigned int index_2 = 2;
704  theta = sqrt((v[index_0] * v[index_0]) + (v[index_1] * v[index_1]) + (v[index_2] * v[index_2]));
705  si = sin(theta);
706  co = cos(theta);
707  sinc = vpMath::sinc(si, theta);
708  mcosc = vpMath::mcosc(co, theta);
709 
710  R[index_0][index_0] = co + (mcosc * v[index_0] * v[index_0]);
711  R[index_0][index_1] = (-sinc * v[index_2]) + (mcosc * v[index_0] * v[index_1]);
712  R[index_0][index_2] = (sinc * v[index_1]) + (mcosc * v[index_0] * v[index_2]);
713  R[index_1][index_0] = (sinc * v[index_2]) + (mcosc * v[index_1] * v[index_0]);
714  R[index_1][index_1] = co + (mcosc * v[index_1] * v[index_1]);
715  R[index_1][index_2] = (-sinc * v[index_0]) + (mcosc * v[index_1] * v[index_2]);
716  R[index_2][index_0] = (-sinc * v[index_1]) + (mcosc * v[index_2] * v[index_0]);
717  R[index_2][index_1] = (sinc * v[index_0]) + (mcosc * v[index_2] * v[index_1]);
718  R[index_2][index_2] = co + (mcosc * v[index_2] * v[index_2]);
719 
720  const unsigned int val_3 = 3;
721  for (unsigned int i = 0; i < val_3; ++i) {
722  for (unsigned int j = 0; j < val_3; ++j) {
723  (*this)[i][j] = R[i][j];
724  }
725  }
726 
727  return *this;
728 }
729 
734 {
735  const unsigned int val_3 = 3;
736  for (unsigned int i = 0; i < val_3; ++i) {
737  for (unsigned int j = 0; j < val_3; ++j) {
738  (*this)[i][j] = M[i][j];
739  }
740  }
741 
742  return *this;
743 }
744 
751 {
752  vpThetaUVector tu(p);
753  return buildFrom(tu);
754 }
755 
764 {
765  double c0, c1, c2, s0, s1, s2;
766  const unsigned int index_0 = 0;
767  const unsigned int index_1 = 1;
768  const unsigned int index_2 = 2;
769 
770  c0 = cos(v[index_0]);
771  c1 = cos(v[index_1]);
772  c2 = cos(v[index_2]);
773  s0 = sin(v[index_0]);
774  s1 = sin(v[index_1]);
775  s2 = sin(v[index_2]);
776 
777  (*this)[index_0][index_0] = (c0 * c1 * c2) - (s0 * s2);
778  (*this)[index_0][index_1] = (-c0 * c1 * s2) - (s0 * c2);
779  (*this)[index_0][index_2] = c0 * s1;
780  (*this)[index_1][index_0] = (s0 * c1 * c2) + (c0 * s2);
781  (*this)[index_1][index_1] = (-s0 * c1 * s2) + (c0 * c2);
782  (*this)[index_1][index_2] = s0 * s1;
783  (*this)[index_2][index_0] = -s1 * c2;
784  (*this)[index_2][index_1] = s1 * s2;
785  (*this)[index_2][index_2] = c1;
786 
787  return *this;
788 }
789 
799 {
800  double c0, c1, c2, s0, s1, s2;
801  const unsigned int index_0 = 0;
802  const unsigned int index_1 = 1;
803  const unsigned int index_2 = 2;
804 
805  c0 = cos(v[index_0]);
806  c1 = cos(v[index_1]);
807  c2 = cos(v[index_2]);
808  s0 = sin(v[index_0]);
809  s1 = sin(v[index_1]);
810  s2 = sin(v[index_2]);
811 
812  (*this)[index_0][index_0] = c1 * c2;
813  (*this)[index_0][index_1] = -c1 * s2;
814  (*this)[index_0][index_2] = s1;
815  (*this)[index_1][index_0] = (c0 * s2) + (s0 * s1 * c2);
816  (*this)[index_1][index_1] = (c0 * c2) - (s0 * s1 * s2);
817  (*this)[index_1][index_2] = -s0 * c1;
818  (*this)[index_2][index_0] = (-c0 * s1 * c2) + (s0 * s2);
819  (*this)[index_2][index_1] = (c0 * s1 * s2) + (c2 * s0);
820  (*this)[index_2][index_2] = c0 * c1;
821 
822  return *this;
823 }
824 
832 {
833  double c0, c1, c2, s0, s1, s2;
834  const unsigned int index_0 = 0;
835  const unsigned int index_1 = 1;
836  const unsigned int index_2 = 2;
837 
838  c0 = cos(v[index_0]);
839  c1 = cos(v[index_1]);
840  c2 = cos(v[index_2]);
841  s0 = sin(v[index_0]);
842  s1 = sin(v[index_1]);
843  s2 = sin(v[index_2]);
844 
845  (*this)[index_0][index_0] = c0 * c1;
846  (*this)[index_0][index_1] = (c0 * s1 * s2) - (s0 * c2);
847  (*this)[index_0][index_2] = (c0 * s1 * c2) + (s0 * s2);
848 
849  (*this)[index_1][index_0] = s0 * c1;
850  (*this)[index_1][index_1] = (s0 * s1 * s2) + (c0 * c2);
851  (*this)[index_1][index_2] = (s0 * s1 * c2) - (c0 * s2);
852 
853  (*this)[index_2][index_0] = -s1;
854  (*this)[index_2][index_1] = c1 * s2;
855  (*this)[index_2][index_2] = c1 * c2;
856 
857  return *this;
858 }
859 
864 vpRotationMatrix &vpRotationMatrix::buildFrom(const double &tux, const double &tuy, const double &tuz)
865 {
866  vpThetaUVector tu(tux, tuy, tuz);
867  buildFrom(tu);
868  return *this;
869 }
870 
875 {
876  double a = q.w();
877  double b = q.x();
878  double c = q.y();
879  double d = q.z();
880  const unsigned int index_0 = 0;
881  const unsigned int index_1 = 1;
882  const unsigned int index_2 = 2;
883  (*this)[index_0][index_0] = (((a * a) + (b * b)) - (c * c)) - (d * d);
884  (*this)[index_0][index_1] = (2.0 * b * c) - (2.0 * a * d);
885  (*this)[index_0][index_2] = (2.0 * a * c) + (2.0 * b * d);
886 
887  (*this)[index_1][index_0] = (2.0 * a * d) + (2.0 * b * c);
888  (*this)[index_1][index_1] = (((a * a) - (b * b)) + (c * c)) - (d * d);
889  (*this)[index_1][index_2] = (2.0 * c * d) - (2.0 * a * b);
890 
891  (*this)[index_2][index_0] = (2.0 * b * d) - (2.0 * a * c);
892  (*this)[index_2][index_1] = (2.0 * a * b) + (2.0 * c * d);
893  (*this)[index_2][index_2] = ((a * a) - (b * b) - (c * c)) + (d * d);
894  return *this;
895 }
896 
902 {
903  vpThetaUVector tu;
904  tu.buildFrom(*this);
905  return tu;
906 }
907 
940 {
941  if (j >= getCols()) {
942  throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the homogeneous matrix"));
943  }
944  unsigned int nb_rows = getRows();
945  vpColVector c(nb_rows);
946  for (unsigned int i = 0; i < nb_rows; ++i) {
947  c[i] = (*this)[i][j];
948  }
949  return c;
950 }
951 
961 vpRotationMatrix vpRotationMatrix::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
962 {
963  vpMatrix meanR(3, 3);
965  size_t vec_m_size = vec_M.size();
966  for (size_t i = 0; i < vec_m_size; ++i) {
967  R = vec_M[i].getRotationMatrix();
968  meanR += static_cast<vpMatrix>(R);
969  }
970  meanR /= static_cast<double>(vec_M.size());
971 
972  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
973  vpMatrix M, U, V;
974  vpColVector sv;
975  meanR.pseudoInverse(M, sv, 1e-6, U, V);
976  const unsigned int index_0 = 0;
977  const unsigned int index_1 = 1;
978  const unsigned int index_2 = 2;
979  double det = sv[index_0] * sv[index_1] * sv[index_2];
980  if (det > 0) {
981  meanR = U * V.t();
982  }
983  else {
984  vpMatrix D(3, 3);
985  D = 0.0;
986  D[index_0][index_0] = 1.0;
987  D[index_1][index_1] = 1.0;
988  D[index_2][index_2] = -1;
989  meanR = U * D * V.t();
990  }
991 
992  R = meanR;
993  return R;
994 }
995 
1004 vpRotationMatrix vpRotationMatrix::mean(const std::vector<vpRotationMatrix> &vec_R)
1005 {
1006  vpMatrix meanR(3, 3);
1007  vpRotationMatrix R;
1008  size_t vec_r_size = vec_R.size();
1009  for (size_t i = 0; i < vec_r_size; ++i) {
1010  meanR += static_cast<vpMatrix>(vec_R[i]);
1011  }
1012  meanR /= static_cast<double>(vec_R.size());
1013 
1014  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
1015  vpMatrix M, U, V;
1016  vpColVector sv;
1017  meanR.pseudoInverse(M, sv, 1e-6, U, V);
1018  const unsigned int index_0 = 0;
1019  const unsigned int index_1 = 1;
1020  const unsigned int index_2 = 2;
1021  double det = sv[index_0] * sv[index_1] * sv[index_2];
1022  if (det > 0) {
1023  meanR = U * V.t();
1024  }
1025  else {
1026  vpMatrix D(3, 3);
1027  D = 0.0;
1028  D[index_0][index_0] = 1.0;
1029  D[index_1][index_1] = 1.0;
1030  D[index_2][index_2] = -1;
1031  meanR = U * D * V.t();
1032  }
1033 
1034  R = meanR;
1035  return R;
1036 }
1037 
1042 {
1043  vpMatrix U(*this);
1044  vpColVector w;
1045  vpMatrix V;
1046  U.svd(w, V);
1047  vpMatrix Vt = V.t();
1048  vpMatrix R = U * Vt;
1049  const unsigned int index_0 = 0;
1050  const unsigned int index_1 = 1;
1051  const unsigned int index_2 = 2;
1052  const unsigned int index_3 = 3;
1053  const unsigned int index_4 = 4;
1054  const unsigned int index_5 = 5;
1055  const unsigned int index_6 = 6;
1056  const unsigned int index_7 = 7;
1057  const unsigned int index_8 = 8;
1058 
1059  double det = R.det();
1060  if (det < 0) {
1061  Vt[index_2][index_0] *= -1;
1062  Vt[index_2][index_1] *= -1;
1063  Vt[index_2][index_2] *= -1;
1064 
1065  R = U * Vt;
1066  }
1067 
1068  data[index_0] = R[index_0][index_0];
1069  data[index_1] = R[index_0][index_1];
1070  data[index_2] = R[index_0][index_2];
1071  data[index_3] = R[index_1][index_0];
1072  data[index_4] = R[index_1][index_1];
1073  data[index_5] = R[index_1][index_2];
1074  data[index_6] = R[index_2][index_0];
1075  data[index_7] = R[index_2][index_1];
1076  data[index_8] = R[index_2][index_2];
1077 }
1078 
1079 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1080 
1088 void vpRotationMatrix::setIdentity() { eye(); }
1089 
1090 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1091 
1095 vpRotationMatrix operator*(const double &x, const vpRotationMatrix &R)
1096 {
1097  vpRotationMatrix C;
1098 
1099  unsigned int Rrow = R.getRows();
1100  unsigned int Rcol = R.getCols();
1101 
1102  for (unsigned int i = 0; i < Rrow; ++i) {
1103  for (unsigned int j = 0; j < Rcol; ++j) {
1104  C[i][j] = R[i][j] * x;
1105  }
1106  }
1107 
1108  return C;
1109 }
1110 END_VISP_NAMESPACE
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:145
unsigned int getCols() const
Definition: vpArray2D.h:337
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:148
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:1105
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:1101
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:1107
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
unsigned int getRows() const
Definition: vpArray2D.h:347
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:1103
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
vpColVector operator*(const double &x, const vpColVector &v)
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
@ fatalError
Fatal error.
Definition: vpException.h:72
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
vpTranslationVector getTranslationVector() const
static double sinc(double x)
Definition: vpMath.cpp:268
static double sqr(double x)
Definition: vpMath.h:203
static double mcosc(double cosx, double x)
Definition: vpMath.cpp:233
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
void svd(vpColVector &w, vpMatrix &V)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:1637
vpMatrix t() const
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:203
Implementation of a rotation vector as quaternion angle minimal representation.
const double & z() const
Returns the z-component of the quaternion.
const double & x() const
Returns the x-component of the quaternion.
const double & y() const
Returns the y-component of the quaternion.
const double & w() const
Returns the w-component of the quaternion.
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix & operator*=(double x)
bool isARotationMatrix(double threshold=1e-6) const
vpRotationMatrix & operator,(double val)
unsigned int m_index
vpColVector getCol(unsigned int j) const
vpThetaUVector getThetaUVector()
vpRotationMatrix & operator<<(double val)
vpRotationMatrix & buildFrom(const vpHomogeneousMatrix &M)
vpRotationMatrix t() const
vpRotationMatrix inverse() const
vpTranslationVector operator*(const vpTranslationVector &tv) const
static vpRotationMatrix mean(const std::vector< vpHomogeneousMatrix > &vec_M)
vpRotationMatrix & operator=(const vpRotationMatrix &R)
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyxVector.h:184
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:182
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector & buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.