Visual Servoing Platform  version 3.6.1 under development (2024-10-15)
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
59 {
60  const unsigned int val_3 = 3;
61  for (unsigned int i = 0; i < val_3; ++i) {
62  for (unsigned int j = 0; j < val_3; ++j) {
63  if (i == j) {
64  (*this)[i][j] = 1.0;
65  }
66  else {
67  (*this)[i][j] = 0.0;
68  }
69  }
70  }
71 }
72 
83 {
84  const unsigned int val_3 = 3;
85  for (unsigned int i = 0; i < val_3; ++i) {
86  for (unsigned int j = 0; j < val_3; ++j) {
87  rowPtrs[i][j] = R.rowPtrs[i][j];
88  }
89  }
90 
91  return *this;
92 }
93 
94 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
122 vpRotationMatrix &vpRotationMatrix::operator=(const std::initializer_list<double> &list)
123 {
124  if (dsize != static_cast<unsigned int>(list.size())) {
126  "Cannot set a 3-by-3 rotation matrix from a %d-elements list of doubles."));
127  }
128 
129  std::copy(list.begin(), list.end(), data);
130 
131  if (!isARotationMatrix()) {
132  if (isARotationMatrix(1e-3)) {
133  orthogonalize();
134  }
135  else {
136  throw(vpException(
138  "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
139  }
140  }
141 
142  return *this;
143 }
144 #endif
145 
163 {
164  if ((M.getCols() != 3) && (M.getRows() != 3)) {
165  throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
166  M.getRows(), M.getCols()));
167  }
168 
169  const unsigned int val_3 = 3;
170  for (unsigned int i = 0; i < val_3; ++i) {
171  for (unsigned int j = 0; j < val_3; ++j) {
172  (*this)[i][j] = M[i][j];
173  }
174  }
175 
176  if (isARotationMatrix() == false) {
177  throw(vpException(vpException::fatalError, "Cannot set a rotation matrix "
178  "from a matrix that is not a "
179  "rotation matrix"));
180  }
181 
182  return *this;
183 }
184 
217 {
218  m_index = 0;
219  data[m_index] = val;
220  return *this;
221 }
222 
255 {
256  ++m_index;
257  if (m_index >= size()) {
259  "Cannot set rotation matrix out of bounds. It has only %d elements while you try to initialize "
260  "with %d elements",
261  size(), m_index + 1));
262  }
263  data[m_index] = val;
264  return *this;
265 }
266 
271 {
273 
274  const unsigned int val_3 = 3;
275  for (unsigned int i = 0; i < val_3; ++i) {
276  for (unsigned int j = 0; j < val_3; ++j) {
277  double s = 0;
278  for (unsigned int k = 0; k < 3; ++k) {
279  s += rowPtrs[i][k] * R.rowPtrs[k][j];
280  }
281  p[i][j] = s;
282  }
283  }
284  return p;
285 }
286 
305 {
306  if ((M.getRows() != 3) || (M.getCols() != 3)) {
307  throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
308  M.getRows(), M.getCols()));
309  }
310  vpMatrix p(3, 3);
311 
312  const unsigned int val_3 = 3;
313  for (unsigned int i = 0; i < val_3; ++i) {
314  for (unsigned int j = 0; j < val_3; ++j) {
315  double s = 0;
316  for (unsigned int k = 0; k < 3; ++k) {
317  s += (*this)[i][k] * M[k][j];
318  }
319  p[i][j] = s;
320  }
321  }
322  return p;
323 }
324 
340 {
341  return (vpHomogeneousMatrix(*this * M.getTranslationVector(), *this * M.getRotationMatrix()));
342 }
343 
378 {
379  const unsigned int rows_size = 3;
380  if (v.getRows() != rows_size) {
382  "Cannot multiply a (3x3) rotation matrix by a %d "
383  "dimension column vector",
384  v.getRows()));
385  }
386  vpColVector v_out(3);
387 
388  for (unsigned int j = 0; j < colNum; ++j) {
389  double vj = v[j]; // optimization em 5/12/2006
390  for (unsigned int i = 0; i < rowNum; ++i) {
391  v_out[i] += rowPtrs[i][j] * vj;
392  }
393  }
394 
395  return v_out;
396 }
397 
403 {
405  const unsigned int val_3 = 3;
406 
407  for (unsigned int j = 0; j < val_3; ++j) {
408  p[j] = 0;
409  }
410 
411  for (unsigned int j = 0; j < val_3; ++j) {
412  for (unsigned int i = 0; i < val_3; ++i) {
413  p[i] += rowPtrs[i][j] * tv[j];
414  }
415  }
416 
417  return p;
418 }
419 
425 {
427 
428  for (unsigned int i = 0; i < rowNum; ++i) {
429  for (unsigned int j = 0; j < colNum; ++j) {
430  R[i][j] = rowPtrs[i][j] * x;
431  }
432  }
433 
434  return R;
435 }
436 
442 {
443  for (unsigned int i = 0; i < rowNum; ++i) {
444  for (unsigned int j = 0; j < colNum; ++j) {
445  rowPtrs[i][j] *= x;
446  }
447  }
448 
449  return *this;
450 }
451 
452 /*********************************************************************/
453 
460 bool vpRotationMatrix::isARotationMatrix(double threshold) const
461 {
462  bool isRotation = true;
463 
464  if ((getCols() != 3) || (getRows() != 3)) {
465  return false;
466  }
467 
468  // --comment: test R^TR = Id
469  vpRotationMatrix RtR = (*this).t() * (*this);
470  const unsigned int val_3 = 3;
471  for (unsigned int i = 0; i < val_3; ++i) {
472  for (unsigned int j = 0; j < val_3; ++j) {
473  if (i == j) {
474  if (fabs(RtR[i][j] - 1) > threshold) {
475  isRotation = false;
476  }
477  }
478  else {
479  if (fabs(RtR[i][j]) > threshold) {
480  isRotation = false;
481  }
482  }
483  }
484  }
485  // test if it is a basis
486  // test || Ci || = 1
487  const unsigned int index_2 = 2;
488  for (unsigned int i = 0; i < val_3; ++i) {
489  if ((sqrt(vpMath::sqr(RtR[0][i]) + vpMath::sqr(RtR[1][i]) + vpMath::sqr(RtR[index_2][i])) - 1) > threshold) {
490  isRotation = false;
491  }
492  }
493 
494  // test || Ri || = 1
495  for (unsigned int i = 0; i < val_3; ++i) {
496  if ((sqrt(vpMath::sqr(RtR[i][0]) + vpMath::sqr(RtR[i][1]) + vpMath::sqr(RtR[i][index_2])) - 1) > threshold) {
497  isRotation = false;
498  }
499  }
500 
501  // test if the basis is orthogonal
502  return isRotation;
503 }
504 
508 vpRotationMatrix::vpRotationMatrix() : vpArray2D<double>(3, 3), m_index(0) { eye(); }
509 
514 vpRotationMatrix::vpRotationMatrix(const vpRotationMatrix &M) : vpArray2D<double>(3, 3), m_index(0) { (*this) = M; }
515 
519 vpRotationMatrix::vpRotationMatrix(const vpHomogeneousMatrix &M) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(M); }
520 
525 vpRotationMatrix::vpRotationMatrix(const vpThetaUVector &tu) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(tu); }
526 
530 vpRotationMatrix::vpRotationMatrix(const vpPoseVector &p) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(p); }
531 
536 vpRotationMatrix::vpRotationMatrix(const vpRzyzVector &euler) : vpArray2D<double>(3, 3), m_index(0)
537 {
538  buildFrom(euler);
539 }
540 
545 vpRotationMatrix::vpRotationMatrix(const vpRxyzVector &Rxyz) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(Rxyz); }
546 
551 vpRotationMatrix::vpRotationMatrix(const vpRzyxVector &Rzyx) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(Rzyx); }
552 
556 vpRotationMatrix::vpRotationMatrix(const vpMatrix &R) : vpArray2D<double>(3, 3), m_index(0) { *this = R; }
557 
562 vpRotationMatrix::vpRotationMatrix(double tux, double tuy, double tuz) : vpArray2D<double>(3, 3), m_index(0)
563 {
564  buildFrom(tux, tuy, tuz);
565 }
566 
570 vpRotationMatrix::vpRotationMatrix(const vpQuaternionVector &q) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(q); }
571 
572 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
598 vpRotationMatrix::vpRotationMatrix(const std::initializer_list<double> &list)
599  : vpArray2D<double>(3, 3, list), m_index(0)
600 {
601  if (!isARotationMatrix()) {
602  if (isARotationMatrix(1e-3)) {
603  orthogonalize();
604  }
605  else {
606  throw(vpException(
608  "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
609  }
610  }
611 }
612 #endif
613 
621 {
622  vpRotationMatrix Rt;
623 
624  const unsigned int val_3 = 3;
625  for (unsigned int i = 0; i < val_3; ++i) {
626  for (unsigned int j = 0; j < val_3; ++j) {
627  Rt[j][i] = (*this)[i][j];
628  }
629  }
630 
631  return Rt;
632 }
633 
641 {
642  vpRotationMatrix Ri = (*this).t();
643 
644  return Ri;
645 }
646 
669 
675 {
676  vpThetaUVector tu(*this);
677 
678  const unsigned int val_3 = 3;
679  for (unsigned int i = 0; i < val_3; ++i) {
680  std::cout << tu[i] << " ";
681  }
682 
683  std::cout << std::endl;
684 }
685 
696 {
697  double theta, si, co, sinc, mcosc;
699 
700  const unsigned int index_0 = 0;
701  const unsigned int index_1 = 1;
702  const unsigned int index_2 = 2;
703  theta = sqrt((v[index_0] * v[index_0]) + (v[index_1] * v[index_1]) + (v[index_2] * v[index_2]));
704  si = sin(theta);
705  co = cos(theta);
706  sinc = vpMath::sinc(si, theta);
707  mcosc = vpMath::mcosc(co, theta);
708 
709  R[index_0][index_0] = co + (mcosc * v[index_0] * v[index_0]);
710  R[index_0][index_1] = (-sinc * v[index_2]) + (mcosc * v[index_0] * v[index_1]);
711  R[index_0][index_2] = (sinc * v[index_1]) + (mcosc * v[index_0] * v[index_2]);
712  R[index_1][index_0] = (sinc * v[index_2]) + (mcosc * v[index_1] * v[index_0]);
713  R[index_1][index_1] = co + (mcosc * v[index_1] * v[index_1]);
714  R[index_1][index_2] = (-sinc * v[index_0]) + (mcosc * v[index_1] * v[index_2]);
715  R[index_2][index_0] = (-sinc * v[index_1]) + (mcosc * v[index_2] * v[index_0]);
716  R[index_2][index_1] = (sinc * v[index_0]) + (mcosc * v[index_2] * v[index_1]);
717  R[index_2][index_2] = co + (mcosc * v[index_2] * v[index_2]);
718 
719  const unsigned int val_3 = 3;
720  for (unsigned int i = 0; i < val_3; ++i) {
721  for (unsigned int j = 0; j < val_3; ++j) {
722  (*this)[i][j] = R[i][j];
723  }
724  }
725 
726  return *this;
727 }
728 
733 {
734  const unsigned int val_3 = 3;
735  for (unsigned int i = 0; i < val_3; ++i) {
736  for (unsigned int j = 0; j < val_3; ++j) {
737  (*this)[i][j] = M[i][j];
738  }
739  }
740 
741  return *this;
742 }
743 
750 {
751  vpThetaUVector tu(p);
752  return buildFrom(tu);
753 }
754 
763 {
764  double c0, c1, c2, s0, s1, s2;
765  const unsigned int index_0 = 0;
766  const unsigned int index_1 = 1;
767  const unsigned int index_2 = 2;
768 
769  c0 = cos(v[index_0]);
770  c1 = cos(v[index_1]);
771  c2 = cos(v[index_2]);
772  s0 = sin(v[index_0]);
773  s1 = sin(v[index_1]);
774  s2 = sin(v[index_2]);
775 
776  (*this)[index_0][index_0] = (c0 * c1 * c2) - (s0 * s2);
777  (*this)[index_0][index_1] = (-c0 * c1 * s2) - (s0 * c2);
778  (*this)[index_0][index_2] = c0 * s1;
779  (*this)[index_1][index_0] = (s0 * c1 * c2) + (c0 * s2);
780  (*this)[index_1][index_1] = (-s0 * c1 * s2) + (c0 * c2);
781  (*this)[index_1][index_2] = s0 * s1;
782  (*this)[index_2][index_0] = -s1 * c2;
783  (*this)[index_2][index_1] = s1 * s2;
784  (*this)[index_2][index_2] = c1;
785 
786  return *this;
787 }
788 
798 {
799  double c0, c1, c2, s0, s1, s2;
800  const unsigned int index_0 = 0;
801  const unsigned int index_1 = 1;
802  const unsigned int index_2 = 2;
803 
804  c0 = cos(v[index_0]);
805  c1 = cos(v[index_1]);
806  c2 = cos(v[index_2]);
807  s0 = sin(v[index_0]);
808  s1 = sin(v[index_1]);
809  s2 = sin(v[index_2]);
810 
811  (*this)[index_0][index_0] = c1 * c2;
812  (*this)[index_0][index_1] = -c1 * s2;
813  (*this)[index_0][index_2] = s1;
814  (*this)[index_1][index_0] = (c0 * s2) + (s0 * s1 * c2);
815  (*this)[index_1][index_1] = (c0 * c2) - (s0 * s1 * s2);
816  (*this)[index_1][index_2] = -s0 * c1;
817  (*this)[index_2][index_0] = (-c0 * s1 * c2) + (s0 * s2);
818  (*this)[index_2][index_1] = (c0 * s1 * s2) + (c2 * s0);
819  (*this)[index_2][index_2] = c0 * c1;
820 
821  return *this;
822 }
823 
831 {
832  double c0, c1, c2, s0, s1, s2;
833  const unsigned int index_0 = 0;
834  const unsigned int index_1 = 1;
835  const unsigned int index_2 = 2;
836 
837  c0 = cos(v[index_0]);
838  c1 = cos(v[index_1]);
839  c2 = cos(v[index_2]);
840  s0 = sin(v[index_0]);
841  s1 = sin(v[index_1]);
842  s2 = sin(v[index_2]);
843 
844  (*this)[index_0][index_0] = c0 * c1;
845  (*this)[index_0][index_1] = (c0 * s1 * s2) - (s0 * c2);
846  (*this)[index_0][index_2] = (c0 * s1 * c2) + (s0 * s2);
847 
848  (*this)[index_1][index_0] = s0 * c1;
849  (*this)[index_1][index_1] = (s0 * s1 * s2) + (c0 * c2);
850  (*this)[index_1][index_2] = (s0 * s1 * c2) - (c0 * s2);
851 
852  (*this)[index_2][index_0] = -s1;
853  (*this)[index_2][index_1] = c1 * s2;
854  (*this)[index_2][index_2] = c1 * c2;
855 
856  return *this;
857 }
858 
863 vpRotationMatrix &vpRotationMatrix::buildFrom(const double &tux, const double &tuy, const double &tuz)
864 {
865  vpThetaUVector tu(tux, tuy, tuz);
866  buildFrom(tu);
867  return *this;
868 }
869 
874 {
875  double a = q.w();
876  double b = q.x();
877  double c = q.y();
878  double d = q.z();
879  const unsigned int index_0 = 0;
880  const unsigned int index_1 = 1;
881  const unsigned int index_2 = 2;
882  (*this)[index_0][index_0] = (((a * a) + (b * b)) - (c * c)) - (d * d);
883  (*this)[index_0][index_1] = (2 * b * c) - (2 * a * d);
884  (*this)[index_0][index_2] = (2 * a * c) + (2 * b * d);
885 
886  (*this)[index_1][index_0] = (2 * a * d) + (2 * b * c);
887  (*this)[index_1][index_1] = (((a * a) - (b * b)) + (c * c)) - (d * d);
888  (*this)[index_1][index_2] = (2 * c * d) - (2 * a * b);
889 
890  (*this)[index_2][index_0] = (2 * b * d) - (2 * a * c);
891  (*this)[index_2][index_1] = (2 * a * b) + (2 * c * d);
892  (*this)[index_2][index_2] = ((a * a) - (b * b) - (c * c)) + (d * d);
893  return *this;
894 }
895 
901 {
902  vpThetaUVector tu;
903  tu.buildFrom(*this);
904  return tu;
905 }
906 
939 {
940  if (j >= getCols()) {
941  throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the homogeneous matrix"));
942  }
943  unsigned int nb_rows = getRows();
944  vpColVector c(nb_rows);
945  for (unsigned int i = 0; i < nb_rows; ++i) {
946  c[i] = (*this)[i][j];
947  }
948  return c;
949 }
950 
960 vpRotationMatrix vpRotationMatrix::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
961 {
962  vpMatrix meanR(3, 3);
964  size_t vec_m_size = vec_M.size();
965  for (size_t i = 0; i < vec_m_size; ++i) {
966  R = vec_M[i].getRotationMatrix();
967  meanR += (vpMatrix)R;
968  }
969  meanR /= static_cast<double>(vec_M.size());
970 
971  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
972  vpMatrix M, U, V;
973  vpColVector sv;
974  meanR.pseudoInverse(M, sv, 1e-6, U, V);
975  const unsigned int index_0 = 0;
976  const unsigned int index_1 = 1;
977  const unsigned int index_2 = 2;
978  double det = sv[index_0] * sv[index_1] * sv[index_2];
979  if (det > 0) {
980  meanR = U * V.t();
981  }
982  else {
983  vpMatrix D(3, 3);
984  D = 0.0;
985  D[index_0][index_0] = 1.0;
986  D[index_1][index_1] = 1.0;
987  D[index_2][index_2] = -1;
988  meanR = U * D * V.t();
989  }
990 
991  R = meanR;
992  return R;
993 }
994 
1003 vpRotationMatrix vpRotationMatrix::mean(const std::vector<vpRotationMatrix> &vec_R)
1004 {
1005  vpMatrix meanR(3, 3);
1006  vpRotationMatrix R;
1007  size_t vec_r_size = vec_R.size();
1008  for (size_t i = 0; i < vec_r_size; ++i) {
1009  meanR += (vpMatrix)vec_R[i];
1010  }
1011  meanR /= static_cast<double>(vec_R.size());
1012 
1013  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
1014  vpMatrix M, U, V;
1015  vpColVector sv;
1016  meanR.pseudoInverse(M, sv, 1e-6, U, V);
1017  const unsigned int index_0 = 0;
1018  const unsigned int index_1 = 1;
1019  const unsigned int index_2 = 2;
1020  double det = sv[index_0] * sv[index_1] * sv[index_2];
1021  if (det > 0) {
1022  meanR = U * V.t();
1023  }
1024  else {
1025  vpMatrix D(3, 3);
1026  D = 0.0;
1027  D[index_0][index_0] = 1.0;
1028  D[index_1][index_1] = 1.0;
1029  D[index_2][index_2] = -1;
1030  meanR = U * D * V.t();
1031  }
1032 
1033  R = meanR;
1034  return R;
1035 }
1036 
1041 {
1042  vpMatrix U(*this);
1043  vpColVector w;
1044  vpMatrix V;
1045  U.svd(w, V);
1046  vpMatrix Vt = V.t();
1047  vpMatrix R = U * Vt;
1048  const unsigned int index_0 = 0;
1049  const unsigned int index_1 = 1;
1050  const unsigned int index_2 = 2;
1051  const unsigned int index_3 = 3;
1052  const unsigned int index_4 = 4;
1053  const unsigned int index_5 = 5;
1054  const unsigned int index_6 = 6;
1055  const unsigned int index_7 = 7;
1056  const unsigned int index_8 = 8;
1057 
1058  double det = R.det();
1059  if (det < 0) {
1060  Vt[index_2][index_0] *= -1;
1061  Vt[index_2][index_1] *= -1;
1062  Vt[index_2][index_2] *= -1;
1063 
1064  R = U * Vt;
1065  }
1066 
1067  data[index_0] = R[index_0][index_0];
1068  data[index_1] = R[index_0][index_1];
1069  data[index_2] = R[index_0][index_2];
1070  data[index_3] = R[index_1][index_0];
1071  data[index_4] = R[index_1][index_1];
1072  data[index_5] = R[index_1][index_2];
1073  data[index_6] = R[index_2][index_0];
1074  data[index_7] = R[index_2][index_1];
1075  data[index_8] = R[index_2][index_2];
1076 }
1077 
1078 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1079 
1087 void vpRotationMatrix::setIdentity() { eye(); }
1088 
1089 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1090 
1094 vpRotationMatrix operator*(const double &x, const vpRotationMatrix &R)
1095 {
1096  vpRotationMatrix C;
1097 
1098  unsigned int Rrow = R.getRows();
1099  unsigned int Rcol = R.getCols();
1100 
1101  for (unsigned int i = 0; i < Rrow; ++i) {
1102  for (unsigned int j = 0; j < Rcol; ++j) {
1103  C[i][j] = R[i][j] * x;
1104  }
1105  }
1106 
1107  return C;
1108 }
1109 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:1102
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:1098
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:1104
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:1100
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:1641
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.