Visual Servoing Platform  version 3.5.1 under development (2022-12-01)
vpRotationMatrix.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Rotation matrix.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
45 #include <visp3/core/vpMath.h>
46 #include <visp3/core/vpMatrix.h>
47 
48 // Rotation classes
49 #include <visp3/core/vpRotationMatrix.h>
50 
51 // Exception
52 #include <visp3/core/vpException.h>
53 
54 // Debug trace
55 #include <math.h>
56 #include <visp3/core/vpDebug.h>
57 
64 {
65  for (unsigned int i = 0; i < 3; i++) {
66  for (unsigned int j = 0; j < 3; j++) {
67  if (i == j)
68  (*this)[i][j] = 1.0;
69  else
70  (*this)[i][j] = 0.0;
71  }
72  }
73 }
74 
85 {
86  for (unsigned int i = 0; i < 3; i++) {
87  for (unsigned int j = 0; j < 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)
121 vpRotationMatrix &vpRotationMatrix::operator=(const std::initializer_list<double> &list)
122 {
123  if (dsize != static_cast<unsigned int>(list.size())) {
125  "Cannot set a 3-by-3 rotation matrix from a %d-elements list of doubles."));
126  }
127 
128  std::copy(list.begin(), list.end(), data);
129 
130  if (!isARotationMatrix()) {
131  if (isARotationMatrix(1e-3)) {
132  orthogonalize();
133  } else {
134  throw(vpException(
136  "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
137  }
138  }
139 
140  return *this;
141 }
142 #endif
143 
161 {
162  if ((M.getCols() != 3) && (M.getRows() != 3)) {
163  throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
164  M.getRows(), M.getCols()));
165  }
166 
167  for (unsigned int i = 0; i < 3; i++) {
168  for (unsigned int j = 0; j < 3; j++) {
169  (*this)[i][j] = M[i][j];
170  }
171  }
172 
173  if (isARotationMatrix() == false) {
174  throw(vpException(vpException::fatalError, "Cannot set a rotation matrix "
175  "from a matrix that is not a "
176  "rotation matrix"));
177  }
178 
179  return *this;
180 }
181 
210 {
211  m_index = 0;
212  data[m_index] = val;
213  return *this;
214 }
215 
244 {
245  m_index++;
246  if (m_index >= size()) {
248  "Cannot set rotation matrix out of bounds. It has only %d elements while you try to initialize "
249  "with %d elements",
250  size(), m_index + 1));
251  }
252  data[m_index] = val;
253  return *this;
254 }
255 
260 {
262 
263  for (unsigned int i = 0; i < 3; i++) {
264  for (unsigned int j = 0; j < 3; j++) {
265  double s = 0;
266  for (unsigned int k = 0; k < 3; k++)
267  s += rowPtrs[i][k] * R.rowPtrs[k][j];
268  p[i][j] = s;
269  }
270  }
271  return p;
272 }
288 {
289  if (M.getRows() != 3 || M.getCols() != 3) {
290  throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
291  M.getRows(), M.getCols()));
292  }
293  vpMatrix p(3, 3);
294 
295  for (unsigned int i = 0; i < 3; i++) {
296  for (unsigned int j = 0; j < 3; j++) {
297  double s = 0;
298  for (unsigned int k = 0; k < 3; k++)
299  s += (*this)[i][k] * M[k][j];
300  p[i][j] = s;
301  }
302  }
303  return p;
304 }
305 
336 {
337  if (v.getRows() != 3) {
339  "Cannot multiply a (3x3) rotation matrix by a %d "
340  "dimension column vector",
341  v.getRows()));
342  }
343  vpColVector v_out(3);
344 
345  for (unsigned int j = 0; j < colNum; j++) {
346  double vj = v[j]; // optimization em 5/12/2006
347  for (unsigned int i = 0; i < rowNum; i++) {
348  v_out[i] += rowPtrs[i][j] * vj;
349  }
350  }
351 
352  return v_out;
353 }
354 
360 {
362 
363  for (unsigned int j = 0; j < 3; j++)
364  p[j] = 0;
365 
366  for (unsigned int j = 0; j < 3; j++) {
367  for (unsigned int i = 0; i < 3; i++) {
368  p[i] += rowPtrs[i][j] * tv[j];
369  }
370  }
371 
372  return p;
373 }
374 
380 {
382 
383  for (unsigned int i = 0; i < rowNum; i++)
384  for (unsigned int j = 0; j < colNum; j++)
385  R[i][j] = rowPtrs[i][j] * x;
386 
387  return R;
388 }
389 
395 {
396  for (unsigned int i = 0; i < rowNum; i++)
397  for (unsigned int j = 0; j < colNum; j++)
398  rowPtrs[i][j] *= x;
399 
400  return *this;
401 }
402 
403 /*********************************************************************/
404 
411 bool vpRotationMatrix::isARotationMatrix(double threshold) const
412 {
413  bool isRotation = true;
414 
415  if (getCols() != 3 || getRows() != 3) {
416  return false;
417  }
418 
419  // test R^TR = Id ;
420  vpRotationMatrix RtR = (*this).t() * (*this);
421  for (unsigned int i = 0; i < 3; i++) {
422  for (unsigned int j = 0; j < 3; j++) {
423  if (i == j) {
424  if (fabs(RtR[i][j] - 1) > threshold) {
425  isRotation = false;
426  }
427  } else {
428  if (fabs(RtR[i][j]) > threshold) {
429  isRotation = false;
430  }
431  }
432  }
433  }
434  // test if it is a basis
435  // test || Ci || = 1
436  for (unsigned int i = 0; i < 3; i++) {
437  if ((sqrt(vpMath::sqr(RtR[0][i]) + vpMath::sqr(RtR[1][i]) + vpMath::sqr(RtR[2][i])) - 1) > threshold) {
438  isRotation = false;
439  }
440  }
441 
442  // test || Ri || = 1
443  for (unsigned int i = 0; i < 3; i++) {
444  if ((sqrt(vpMath::sqr(RtR[i][0]) + vpMath::sqr(RtR[i][1]) + vpMath::sqr(RtR[i][2])) - 1) > threshold) {
445  isRotation = false;
446  }
447  }
448 
449  // test if the basis is orthogonal
450  return isRotation;
451 }
452 
456 vpRotationMatrix::vpRotationMatrix() : vpArray2D<double>(3, 3), m_index(0) { eye(); }
457 
462 vpRotationMatrix::vpRotationMatrix(const vpRotationMatrix &M) : vpArray2D<double>(3, 3), m_index(0) { (*this) = M; }
463 
467 vpRotationMatrix::vpRotationMatrix(const vpHomogeneousMatrix &M) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(M); }
468 
473 vpRotationMatrix::vpRotationMatrix(const vpThetaUVector &tu) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(tu); }
474 
478 vpRotationMatrix::vpRotationMatrix(const vpPoseVector &p) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(p); }
479 
484 vpRotationMatrix::vpRotationMatrix(const vpRzyzVector &euler) : vpArray2D<double>(3, 3), m_index(0)
485 {
486  buildFrom(euler);
487 }
488 
493 vpRotationMatrix::vpRotationMatrix(const vpRxyzVector &Rxyz) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(Rxyz); }
494 
499 vpRotationMatrix::vpRotationMatrix(const vpRzyxVector &Rzyx) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(Rzyx); }
500 
504 vpRotationMatrix::vpRotationMatrix(const vpMatrix &R) : vpArray2D<double>(3, 3), m_index(0) { *this = R; }
505 
510 vpRotationMatrix::vpRotationMatrix(double tux, double tuy, double tuz) : vpArray2D<double>(3, 3), m_index(0)
511 {
512  buildFrom(tux, tuy, tuz);
513 }
514 
518 vpRotationMatrix::vpRotationMatrix(const vpQuaternionVector &q) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(q); }
519 
520 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
544 vpRotationMatrix::vpRotationMatrix(const std::initializer_list<double> &list)
545  : vpArray2D<double>(3, 3, list), m_index(0)
546 {
547  if (!isARotationMatrix()) {
548  if (isARotationMatrix(1e-3)) {
549  orthogonalize();
550  } else {
551  throw(vpException(
553  "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
554  }
555  }
556 }
557 #endif
558 
566 {
567  vpRotationMatrix Rt;
568 
569  for (unsigned int i = 0; i < 3; i++)
570  for (unsigned int j = 0; j < 3; j++)
571  Rt[j][i] = (*this)[i][j];
572 
573  return Rt;
574 }
575 
583 {
584  vpRotationMatrix Ri = (*this).t();
585 
586  return Ri;
587 }
588 
607 
613 {
614  vpThetaUVector tu(*this);
615 
616  for (unsigned int i = 0; i < 3; i++)
617  std::cout << tu[i] << " ";
618 
619  std::cout << std::endl;
620 }
621 
632 {
633  double theta, si, co, sinc, mcosc;
635 
636  theta = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
637  si = sin(theta);
638  co = cos(theta);
639  sinc = vpMath::sinc(si, theta);
640  mcosc = vpMath::mcosc(co, theta);
641 
642  R[0][0] = co + mcosc * v[0] * v[0];
643  R[0][1] = -sinc * v[2] + mcosc * v[0] * v[1];
644  R[0][2] = sinc * v[1] + mcosc * v[0] * v[2];
645  R[1][0] = sinc * v[2] + mcosc * v[1] * v[0];
646  R[1][1] = co + mcosc * v[1] * v[1];
647  R[1][2] = -sinc * v[0] + mcosc * v[1] * v[2];
648  R[2][0] = -sinc * v[1] + mcosc * v[2] * v[0];
649  R[2][1] = sinc * v[0] + mcosc * v[2] * v[1];
650  R[2][2] = co + mcosc * v[2] * v[2];
651 
652  for (unsigned int i = 0; i < 3; i++)
653  for (unsigned int j = 0; j < 3; j++)
654  (*this)[i][j] = R[i][j];
655 
656  return *this;
657 }
658 
663 {
664  for (unsigned int i = 0; i < 3; i++)
665  for (unsigned int j = 0; j < 3; j++)
666  (*this)[i][j] = M[i][j];
667 
668  return *this;
669 }
670 
677 {
678  vpThetaUVector tu(p);
679  return buildFrom(tu);
680 }
681 
690 {
691  double c0, c1, c2, s0, s1, s2;
692 
693  c0 = cos(v[0]);
694  c1 = cos(v[1]);
695  c2 = cos(v[2]);
696  s0 = sin(v[0]);
697  s1 = sin(v[1]);
698  s2 = sin(v[2]);
699 
700  (*this)[0][0] = c0 * c1 * c2 - s0 * s2;
701  (*this)[0][1] = -c0 * c1 * s2 - s0 * c2;
702  (*this)[0][2] = c0 * s1;
703  (*this)[1][0] = s0 * c1 * c2 + c0 * s2;
704  (*this)[1][1] = -s0 * c1 * s2 + c0 * c2;
705  (*this)[1][2] = s0 * s1;
706  (*this)[2][0] = -s1 * c2;
707  (*this)[2][1] = s1 * s2;
708  (*this)[2][2] = c1;
709 
710  return (*this);
711 }
712 
722 {
723  double c0, c1, c2, s0, s1, s2;
724 
725  c0 = cos(v[0]);
726  c1 = cos(v[1]);
727  c2 = cos(v[2]);
728  s0 = sin(v[0]);
729  s1 = sin(v[1]);
730  s2 = sin(v[2]);
731 
732  (*this)[0][0] = c1 * c2;
733  (*this)[0][1] = -c1 * s2;
734  (*this)[0][2] = s1;
735  (*this)[1][0] = c0 * s2 + s0 * s1 * c2;
736  (*this)[1][1] = c0 * c2 - s0 * s1 * s2;
737  (*this)[1][2] = -s0 * c1;
738  (*this)[2][0] = -c0 * s1 * c2 + s0 * s2;
739  (*this)[2][1] = c0 * s1 * s2 + c2 * s0;
740  (*this)[2][2] = c0 * c1;
741 
742  return (*this);
743 }
744 
752 {
753  double c0, c1, c2, s0, s1, s2;
754 
755  c0 = cos(v[0]);
756  c1 = cos(v[1]);
757  c2 = cos(v[2]);
758  s0 = sin(v[0]);
759  s1 = sin(v[1]);
760  s2 = sin(v[2]);
761 
762  (*this)[0][0] = c0 * c1;
763  (*this)[0][1] = c0 * s1 * s2 - s0 * c2;
764  (*this)[0][2] = c0 * s1 * c2 + s0 * s2;
765 
766  (*this)[1][0] = s0 * c1;
767  (*this)[1][1] = s0 * s1 * s2 + c0 * c2;
768  (*this)[1][2] = s0 * s1 * c2 - c0 * s2;
769 
770  (*this)[2][0] = -s1;
771  (*this)[2][1] = c1 * s2;
772  (*this)[2][2] = c1 * c2;
773 
774  return (*this);
775 }
776 
781 vpRotationMatrix vpRotationMatrix::buildFrom(double tux, double tuy, double tuz)
782 {
783  vpThetaUVector tu(tux, tuy, tuz);
784  buildFrom(tu);
785  return *this;
786 }
787 
792 {
793  double a = q.w();
794  double b = q.x();
795  double c = q.y();
796  double d = q.z();
797  (*this)[0][0] = a * a + b * b - c * c - d * d;
798  (*this)[0][1] = 2 * b * c - 2 * a * d;
799  (*this)[0][2] = 2 * a * c + 2 * b * d;
800 
801  (*this)[1][0] = 2 * a * d + 2 * b * c;
802  (*this)[1][1] = a * a - b * b + c * c - d * d;
803  (*this)[1][2] = 2 * c * d - 2 * a * b;
804 
805  (*this)[2][0] = 2 * b * d - 2 * a * c;
806  (*this)[2][1] = 2 * a * b + 2 * c * d;
807  (*this)[2][2] = a * a - b * b - c * c + d * d;
808  return *this;
809 }
810 
814 vpRotationMatrix operator*(const double &x, const vpRotationMatrix &R)
815 {
817 
818  unsigned int Rrow = R.getRows();
819  unsigned int Rcol = R.getCols();
820 
821  for (unsigned int i = 0; i < Rrow; i++)
822  for (unsigned int j = 0; j < Rcol; j++)
823  C[i][j] = R[i][j] * x;
824 
825  return C;
826 }
827 
833 {
834  vpThetaUVector tu;
835  tu.buildFrom(*this);
836  return tu;
837 }
838 
867 {
868  if (j >= getCols())
869  throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the homogeneous matrix"));
870  unsigned int nb_rows = getRows();
871  vpColVector c(nb_rows);
872  for (unsigned int i = 0; i < nb_rows; i++)
873  c[i] = (*this)[i][j];
874  return c;
875 }
876 
886 vpRotationMatrix vpRotationMatrix::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
887 {
888  vpMatrix meanR(3, 3);
890  for (size_t i = 0; i < vec_M.size(); i++) {
891  R = vec_M[i].getRotationMatrix();
892  meanR += (vpMatrix)R;
893  }
894  meanR /= static_cast<double>(vec_M.size());
895 
896  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
897  vpMatrix M, U, V;
898  vpColVector sv;
899  meanR.pseudoInverse(M, sv, 1e-6, U, V);
900  double det = sv[0] * sv[1] * sv[2];
901  if (det > 0) {
902  meanR = U * V.t();
903  } else {
904  vpMatrix D(3, 3);
905  D = 0.0;
906  D[0][0] = D[1][1] = 1.0;
907  D[2][2] = -1;
908  meanR = U * D * V.t();
909  }
910 
911  R = meanR;
912  return R;
913 }
914 
923 vpRotationMatrix vpRotationMatrix::mean(const std::vector<vpRotationMatrix> &vec_R)
924 {
925  vpMatrix meanR(3, 3);
927  for (size_t i = 0; i < vec_R.size(); i++) {
928  meanR += (vpMatrix)vec_R[i];
929  }
930  meanR /= static_cast<double>(vec_R.size());
931 
932  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
933  vpMatrix M, U, V;
934  vpColVector sv;
935  meanR.pseudoInverse(M, sv, 1e-6, U, V);
936  double det = sv[0] * sv[1] * sv[2];
937  if (det > 0) {
938  meanR = U * V.t();
939  } else {
940  vpMatrix D(3, 3);
941  D = 0.0;
942  D[0][0] = D[1][1] = 1.0;
943  D[2][2] = -1;
944  meanR = U * D * V.t();
945  }
946 
947  R = meanR;
948  return R;
949 }
950 
955 {
956  vpMatrix U = *this;
957  vpColVector w;
958  vpMatrix V;
959  U.svd(w, V);
960  vpMatrix Vt = V.t();
961  vpMatrix R = U * Vt;
962 
963  double det = R.det();
964  if (det < 0) {
965  Vt[2][0] *= -1;
966  Vt[2][1] *= -1;
967  Vt[2][2] *= -1;
968 
969  R = U * Vt;
970  }
971 
972  data[0] = R[0][0];
973  data[1] = R[0][1];
974  data[2] = R[0][2];
975  data[3] = R[1][0];
976  data[4] = R[1][1];
977  data[5] = R[1][2];
978  data[6] = R[2][0];
979  data[7] = R[2][1];
980  data[8] = R[2][2];
981 }
982 
983 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
984 
993 
994 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:132
unsigned int getCols() const
Definition: vpArray2D.h:281
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:139
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:141
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:293
unsigned int getRows() const
Definition: vpArray2D.h:291
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
Implementation of an homogeneous matrix and operations on such kind of matrices.
VISP_EXPORT vpImagePoint operator*(const vpImagePoint &ip1, double scale)
static double sinc(double x)
Definition: vpMath.cpp:211
static double sqr(double x)
Definition: vpMath.h:123
static double mcosc(double cosx, double x)
Definition: vpMath.cpp:178
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2024
vpMatrix t() const
Definition: vpMatrix.cpp:465
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2235
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:6464
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
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.
vp_deprecated void setIdentity()
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 t() const
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
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:184
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyxVector.h:186
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:183
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.