Visual Servoing Platform  version 3.6.1 under development (2023-12-07)
vpRotationMatrix.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
42 #include <visp3/core/vpMath.h>
43 #include <visp3/core/vpMatrix.h>
44 
45 // Rotation classes
46 #include <visp3/core/vpRotationMatrix.h>
47 
48 // Exception
49 #include <visp3/core/vpException.h>
50 
51 // Debug trace
52 #include <math.h>
53 #include <visp3/core/vpDebug.h>
54 
61 {
62  for (unsigned int i = 0; i < 3; i++) {
63  for (unsigned int j = 0; j < 3; j++) {
64  if (i == j)
65  (*this)[i][j] = 1.0;
66  else
67  (*this)[i][j] = 0.0;
68  }
69  }
70 }
71 
82 {
83  for (unsigned int i = 0; i < 3; i++) {
84  for (unsigned int j = 0; j < 3; j++) {
85  rowPtrs[i][j] = R.rowPtrs[i][j];
86  }
87  }
88 
89  return *this;
90 }
91 
115 vpRotationMatrix &vpRotationMatrix::operator=(const std::initializer_list<double> &list)
116 {
117  if (dsize != static_cast<unsigned int>(list.size())) {
119  "Cannot set a 3-by-3 rotation matrix from a %d-elements list of doubles."));
120  }
121 
122  std::copy(list.begin(), list.end(), data);
123 
124  if (!isARotationMatrix()) {
125  if (isARotationMatrix(1e-3)) {
126  orthogonalize();
127  }
128  else {
129  throw(vpException(
131  "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
132  }
133  }
134 
135  return *this;
136 }
137 
155 {
156  if ((M.getCols() != 3) && (M.getRows() != 3)) {
157  throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
158  M.getRows(), M.getCols()));
159  }
160 
161  for (unsigned int i = 0; i < 3; i++) {
162  for (unsigned int j = 0; j < 3; j++) {
163  (*this)[i][j] = M[i][j];
164  }
165  }
166 
167  if (isARotationMatrix() == false) {
168  throw(vpException(vpException::fatalError, "Cannot set a rotation matrix "
169  "from a matrix that is not a "
170  "rotation matrix"));
171  }
172 
173  return *this;
174 }
175 
204 {
205  m_index = 0;
206  data[m_index] = val;
207  return *this;
208 }
209 
238 {
239  m_index++;
240  if (m_index >= size()) {
242  "Cannot set rotation matrix out of bounds. It has only %d elements while you try to initialize "
243  "with %d elements",
244  size(), m_index + 1));
245  }
246  data[m_index] = val;
247  return *this;
248 }
249 
254 {
256 
257  for (unsigned int i = 0; i < 3; i++) {
258  for (unsigned int j = 0; j < 3; j++) {
259  double s = 0;
260  for (unsigned int k = 0; k < 3; k++)
261  s += rowPtrs[i][k] * R.rowPtrs[k][j];
262  p[i][j] = s;
263  }
264  }
265  return p;
266 }
267 
286 {
287  if (M.getRows() != 3 || M.getCols() != 3) {
288  throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
289  M.getRows(), M.getCols()));
290  }
291  vpMatrix p(3, 3);
292 
293  for (unsigned int i = 0; i < 3; i++) {
294  for (unsigned int j = 0; j < 3; j++) {
295  double s = 0;
296  for (unsigned int k = 0; k < 3; k++)
297  s += (*this)[i][k] * M[k][j];
298  p[i][j] = s;
299  }
300  }
301  return p;
302 }
303 
319 {
320  return (vpHomogeneousMatrix(*this * M.getTranslationVector(), *this * M.getRotationMatrix()));
321 }
322 
353 {
354  if (v.getRows() != 3) {
356  "Cannot multiply a (3x3) rotation matrix by a %d "
357  "dimension column vector",
358  v.getRows()));
359  }
360  vpColVector v_out(3);
361 
362  for (unsigned int j = 0; j < colNum; j++) {
363  double vj = v[j]; // optimization em 5/12/2006
364  for (unsigned int i = 0; i < rowNum; i++) {
365  v_out[i] += rowPtrs[i][j] * vj;
366  }
367  }
368 
369  return v_out;
370 }
371 
377 {
379 
380  for (unsigned int j = 0; j < 3; j++)
381  p[j] = 0;
382 
383  for (unsigned int j = 0; j < 3; j++) {
384  for (unsigned int i = 0; i < 3; i++) {
385  p[i] += rowPtrs[i][j] * tv[j];
386  }
387  }
388 
389  return p;
390 }
391 
397 {
399 
400  for (unsigned int i = 0; i < rowNum; i++)
401  for (unsigned int j = 0; j < colNum; j++)
402  R[i][j] = rowPtrs[i][j] * x;
403 
404  return R;
405 }
406 
412 {
413  for (unsigned int i = 0; i < rowNum; i++)
414  for (unsigned int j = 0; j < colNum; j++)
415  rowPtrs[i][j] *= x;
416 
417  return *this;
418 }
419 
420 /*********************************************************************/
421 
428 bool vpRotationMatrix::isARotationMatrix(double threshold) const
429 {
430  bool isRotation = true;
431 
432  if (getCols() != 3 || getRows() != 3) {
433  return false;
434  }
435 
436  // test R^TR = Id ;
437  vpRotationMatrix RtR = (*this).t() * (*this);
438  for (unsigned int i = 0; i < 3; i++) {
439  for (unsigned int j = 0; j < 3; j++) {
440  if (i == j) {
441  if (fabs(RtR[i][j] - 1) > threshold) {
442  isRotation = false;
443  }
444  }
445  else {
446  if (fabs(RtR[i][j]) > threshold) {
447  isRotation = false;
448  }
449  }
450  }
451  }
452  // test if it is a basis
453  // test || Ci || = 1
454  for (unsigned int i = 0; i < 3; i++) {
455  if ((sqrt(vpMath::sqr(RtR[0][i]) + vpMath::sqr(RtR[1][i]) + vpMath::sqr(RtR[2][i])) - 1) > threshold) {
456  isRotation = false;
457  }
458  }
459 
460  // test || Ri || = 1
461  for (unsigned int i = 0; i < 3; i++) {
462  if ((sqrt(vpMath::sqr(RtR[i][0]) + vpMath::sqr(RtR[i][1]) + vpMath::sqr(RtR[i][2])) - 1) > threshold) {
463  isRotation = false;
464  }
465  }
466 
467  // test if the basis is orthogonal
468  return isRotation;
469 }
470 
474 vpRotationMatrix::vpRotationMatrix() : vpArray2D<double>(3, 3), m_index(0) { eye(); }
475 
480 vpRotationMatrix::vpRotationMatrix(const vpRotationMatrix &M) : vpArray2D<double>(3, 3), m_index(0) { (*this) = M; }
481 
485 vpRotationMatrix::vpRotationMatrix(const vpHomogeneousMatrix &M) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(M); }
486 
491 vpRotationMatrix::vpRotationMatrix(const vpThetaUVector &tu) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(tu); }
492 
496 vpRotationMatrix::vpRotationMatrix(const vpPoseVector &p) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(p); }
497 
502 vpRotationMatrix::vpRotationMatrix(const vpRzyzVector &euler) : vpArray2D<double>(3, 3), m_index(0)
503 {
504  buildFrom(euler);
505 }
506 
511 vpRotationMatrix::vpRotationMatrix(const vpRxyzVector &Rxyz) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(Rxyz); }
512 
517 vpRotationMatrix::vpRotationMatrix(const vpRzyxVector &Rzyx) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(Rzyx); }
518 
522 vpRotationMatrix::vpRotationMatrix(const vpMatrix &R) : vpArray2D<double>(3, 3), m_index(0) { *this = R; }
523 
528 vpRotationMatrix::vpRotationMatrix(double tux, double tuy, double tuz) : vpArray2D<double>(3, 3), m_index(0)
529 {
530  buildFrom(tux, tuy, tuz);
531 }
532 
536 vpRotationMatrix::vpRotationMatrix(const vpQuaternionVector &q) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(q); }
537 
559 vpRotationMatrix::vpRotationMatrix(const std::initializer_list<double> &list)
560  : vpArray2D<double>(3, 3, list), m_index(0)
561 {
562  if (!isARotationMatrix()) {
563  if (isARotationMatrix(1e-3)) {
564  orthogonalize();
565  }
566  else {
567  throw(vpException(
569  "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
570  }
571  }
572 }
573 
581 {
582  vpRotationMatrix Rt;
583 
584  for (unsigned int i = 0; i < 3; i++)
585  for (unsigned int j = 0; j < 3; j++)
586  Rt[j][i] = (*this)[i][j];
587 
588  return Rt;
589 }
590 
598 {
599  vpRotationMatrix Ri = (*this).t();
600 
601  return Ri;
602 }
603 
622 
628 {
629  vpThetaUVector tu(*this);
630 
631  for (unsigned int i = 0; i < 3; i++)
632  std::cout << tu[i] << " ";
633 
634  std::cout << std::endl;
635 }
636 
647 {
648  double theta, si, co, sinc, mcosc;
650 
651  theta = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
652  si = sin(theta);
653  co = cos(theta);
654  sinc = vpMath::sinc(si, theta);
655  mcosc = vpMath::mcosc(co, theta);
656 
657  R[0][0] = co + mcosc * v[0] * v[0];
658  R[0][1] = -sinc * v[2] + mcosc * v[0] * v[1];
659  R[0][2] = sinc * v[1] + mcosc * v[0] * v[2];
660  R[1][0] = sinc * v[2] + mcosc * v[1] * v[0];
661  R[1][1] = co + mcosc * v[1] * v[1];
662  R[1][2] = -sinc * v[0] + mcosc * v[1] * v[2];
663  R[2][0] = -sinc * v[1] + mcosc * v[2] * v[0];
664  R[2][1] = sinc * v[0] + mcosc * v[2] * v[1];
665  R[2][2] = co + mcosc * v[2] * v[2];
666 
667  for (unsigned int i = 0; i < 3; i++)
668  for (unsigned int j = 0; j < 3; j++)
669  (*this)[i][j] = R[i][j];
670 
671  return *this;
672 }
673 
678 {
679  for (unsigned int i = 0; i < 3; i++)
680  for (unsigned int j = 0; j < 3; j++)
681  (*this)[i][j] = M[i][j];
682 
683  return *this;
684 }
685 
692 {
693  vpThetaUVector tu(p);
694  return buildFrom(tu);
695 }
696 
705 {
706  double c0, c1, c2, s0, s1, s2;
707 
708  c0 = cos(v[0]);
709  c1 = cos(v[1]);
710  c2 = cos(v[2]);
711  s0 = sin(v[0]);
712  s1 = sin(v[1]);
713  s2 = sin(v[2]);
714 
715  (*this)[0][0] = c0 * c1 * c2 - s0 * s2;
716  (*this)[0][1] = -c0 * c1 * s2 - s0 * c2;
717  (*this)[0][2] = c0 * s1;
718  (*this)[1][0] = s0 * c1 * c2 + c0 * s2;
719  (*this)[1][1] = -s0 * c1 * s2 + c0 * c2;
720  (*this)[1][2] = s0 * s1;
721  (*this)[2][0] = -s1 * c2;
722  (*this)[2][1] = s1 * s2;
723  (*this)[2][2] = c1;
724 
725  return (*this);
726 }
727 
737 {
738  double c0, c1, c2, s0, s1, s2;
739 
740  c0 = cos(v[0]);
741  c1 = cos(v[1]);
742  c2 = cos(v[2]);
743  s0 = sin(v[0]);
744  s1 = sin(v[1]);
745  s2 = sin(v[2]);
746 
747  (*this)[0][0] = c1 * c2;
748  (*this)[0][1] = -c1 * s2;
749  (*this)[0][2] = s1;
750  (*this)[1][0] = c0 * s2 + s0 * s1 * c2;
751  (*this)[1][1] = c0 * c2 - s0 * s1 * s2;
752  (*this)[1][2] = -s0 * c1;
753  (*this)[2][0] = -c0 * s1 * c2 + s0 * s2;
754  (*this)[2][1] = c0 * s1 * s2 + c2 * s0;
755  (*this)[2][2] = c0 * c1;
756 
757  return (*this);
758 }
759 
767 {
768  double c0, c1, c2, s0, s1, s2;
769 
770  c0 = cos(v[0]);
771  c1 = cos(v[1]);
772  c2 = cos(v[2]);
773  s0 = sin(v[0]);
774  s1 = sin(v[1]);
775  s2 = sin(v[2]);
776 
777  (*this)[0][0] = c0 * c1;
778  (*this)[0][1] = c0 * s1 * s2 - s0 * c2;
779  (*this)[0][2] = c0 * s1 * c2 + s0 * s2;
780 
781  (*this)[1][0] = s0 * c1;
782  (*this)[1][1] = s0 * s1 * s2 + c0 * c2;
783  (*this)[1][2] = s0 * s1 * c2 - c0 * s2;
784 
785  (*this)[2][0] = -s1;
786  (*this)[2][1] = c1 * s2;
787  (*this)[2][2] = c1 * c2;
788 
789  return (*this);
790 }
791 
796 vpRotationMatrix vpRotationMatrix::buildFrom(double tux, double tuy, double tuz)
797 {
798  vpThetaUVector tu(tux, tuy, tuz);
799  buildFrom(tu);
800  return *this;
801 }
802 
807 {
808  double a = q.w();
809  double b = q.x();
810  double c = q.y();
811  double d = q.z();
812  (*this)[0][0] = a * a + b * b - c * c - d * d;
813  (*this)[0][1] = 2 * b * c - 2 * a * d;
814  (*this)[0][2] = 2 * a * c + 2 * b * d;
815 
816  (*this)[1][0] = 2 * a * d + 2 * b * c;
817  (*this)[1][1] = a * a - b * b + c * c - d * d;
818  (*this)[1][2] = 2 * c * d - 2 * a * b;
819 
820  (*this)[2][0] = 2 * b * d - 2 * a * c;
821  (*this)[2][1] = 2 * a * b + 2 * c * d;
822  (*this)[2][2] = a * a - b * b - c * c + d * d;
823  return *this;
824 }
825 
829 vpRotationMatrix operator*(const double &x, const vpRotationMatrix &R)
830 {
832 
833  unsigned int Rrow = R.getRows();
834  unsigned int Rcol = R.getCols();
835 
836  for (unsigned int i = 0; i < Rrow; i++)
837  for (unsigned int j = 0; j < Rcol; j++)
838  C[i][j] = R[i][j] * x;
839 
840  return C;
841 }
842 
848 {
849  vpThetaUVector tu;
850  tu.buildFrom(*this);
851  return tu;
852 }
853 
882 {
883  if (j >= getCols())
884  throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the homogeneous matrix"));
885  unsigned int nb_rows = getRows();
886  vpColVector c(nb_rows);
887  for (unsigned int i = 0; i < nb_rows; i++)
888  c[i] = (*this)[i][j];
889  return c;
890 }
891 
901 vpRotationMatrix vpRotationMatrix::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
902 {
903  vpMatrix meanR(3, 3);
905  for (size_t i = 0; i < vec_M.size(); i++) {
906  R = vec_M[i].getRotationMatrix();
907  meanR += (vpMatrix)R;
908  }
909  meanR /= static_cast<double>(vec_M.size());
910 
911  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
912  vpMatrix M, U, V;
913  vpColVector sv;
914  meanR.pseudoInverse(M, sv, 1e-6, U, V);
915  double det = sv[0] * sv[1] * sv[2];
916  if (det > 0) {
917  meanR = U * V.t();
918  }
919  else {
920  vpMatrix D(3, 3);
921  D = 0.0;
922  D[0][0] = D[1][1] = 1.0;
923  D[2][2] = -1;
924  meanR = U * D * V.t();
925  }
926 
927  R = meanR;
928  return R;
929 }
930 
939 vpRotationMatrix vpRotationMatrix::mean(const std::vector<vpRotationMatrix> &vec_R)
940 {
941  vpMatrix meanR(3, 3);
943  for (size_t i = 0; i < vec_R.size(); i++) {
944  meanR += (vpMatrix)vec_R[i];
945  }
946  meanR /= static_cast<double>(vec_R.size());
947 
948  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
949  vpMatrix M, U, V;
950  vpColVector sv;
951  meanR.pseudoInverse(M, sv, 1e-6, U, V);
952  double det = sv[0] * sv[1] * sv[2];
953  if (det > 0) {
954  meanR = U * V.t();
955  }
956  else {
957  vpMatrix D(3, 3);
958  D = 0.0;
959  D[0][0] = D[1][1] = 1.0;
960  D[2][2] = -1;
961  meanR = U * D * V.t();
962  }
963 
964  R = meanR;
965  return R;
966 }
967 
972 {
973  vpMatrix U = *this;
974  vpColVector w;
975  vpMatrix V;
976  U.svd(w, V);
977  vpMatrix Vt = V.t();
978  vpMatrix R = U * Vt;
979 
980  double det = R.det();
981  if (det < 0) {
982  Vt[2][0] *= -1;
983  Vt[2][1] *= -1;
984  Vt[2][2] *= -1;
985 
986  R = U * Vt;
987  }
988 
989  data[0] = R[0][0];
990  data[1] = R[0][1];
991  data[2] = R[0][2];
992  data[3] = R[1][0];
993  data[4] = R[1][1];
994  data[5] = R[1][2];
995  data[6] = R[2][0];
996  data[7] = R[2][1];
997  data[8] = R[2][2];
998 }
999 
1000 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1001 
1010 
1011 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:125
unsigned int getCols() const
Definition: vpArray2D.h:257
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:138
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:132
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:128
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:134
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:269
unsigned int getRows() const
Definition: vpArray2D.h:267
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:130
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
vpColVector operator*(const double &x, const vpColVector &v)
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ dimensionError
Bad dimension.
Definition: vpException.h:83
@ fatalError
Fatal error.
Definition: vpException.h:84
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:264
static double sqr(double x)
Definition: vpMath.h:201
static double mcosc(double cosx, double x)
Definition: vpMath.cpp:233
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2016
vpMatrix t() const
Definition: vpMatrix.cpp:452
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2227
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:6471
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:189
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:176
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyxVector.h:177
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:175
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.