Visual Servoing Platform  version 3.4.1 under development (2021-06-22)
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)
96 
121 vpRotationMatrix& vpRotationMatrix::operator=(const std::initializer_list<double> &list)
122 {
123  if (dsize != static_cast<unsigned int>(list.size())) {
124  throw(vpException(vpException::dimensionError, "Cannot set a 3-by-3 rotation matrix from a %d-elements list of doubles."));
125  }
126 
127  std::copy(list.begin(), list.end(), data);
128 
129  if (! isARotationMatrix() ) {
130  throw(vpException(vpException::fatalError, "Rotation matrix initialization fails since it's elements doesn't represent a rotation matrix"));
131  }
132 
133  return *this;
134 }
135 #endif
136 
154 {
155  if ((M.getCols() != 3) && (M.getRows() != 3)) {
156  throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
157  M.getRows(), M.getCols()));
158  }
159 
160  for (unsigned int i = 0; i < 3; i++) {
161  for (unsigned int j = 0; j < 3; j++) {
162  (*this)[i][j] = M[i][j];
163  }
164  }
165 
166  if (isARotationMatrix() == false) {
167  throw(vpException(vpException::fatalError, "Cannot set a rotation matrix "
168  "from a matrix that is not a "
169  "rotation matrix"));
170  }
171 
172  return *this;
173 }
174 
203 {
204  m_index = 0;
205  data[m_index] = val;
206  return *this;
207 }
208 
237 {
238  m_index ++;
239  if (m_index >= size()) {
240  throw(vpException(vpException::dimensionError, "Cannot set rotation matrix out of bounds. It has only %d elements while you try to initialize with %d elements", size(), m_index+1));
241  }
242  data[m_index] = val;
243  return *this;
244 }
245 
250 {
252 
253  for (unsigned int i = 0; i < 3; i++) {
254  for (unsigned int j = 0; j < 3; j++) {
255  double s = 0;
256  for (unsigned int k = 0; k < 3; k++)
257  s += rowPtrs[i][k] * R.rowPtrs[k][j];
258  p[i][j] = s;
259  }
260  }
261  return p;
262 }
278 {
279  if (M.getRows() != 3 || M.getCols() != 3) {
280  throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
281  M.getRows(), M.getCols()));
282  }
283  vpMatrix p(3, 3);
284 
285  for (unsigned int i = 0; i < 3; i++) {
286  for (unsigned int j = 0; j < 3; j++) {
287  double s = 0;
288  for (unsigned int k = 0; k < 3; k++)
289  s += (*this)[i][k] * M[k][j];
290  p[i][j] = s;
291  }
292  }
293  return p;
294 }
295 
326 {
327  if (v.getRows() != 3) {
329  "Cannot multiply a (3x3) rotation matrix by a %d "
330  "dimension column vector",
331  v.getRows()));
332  }
333  vpColVector v_out(3);
334 
335  for (unsigned int j = 0; j < colNum; j++) {
336  double vj = v[j]; // optimization em 5/12/2006
337  for (unsigned int i = 0; i < rowNum; i++) {
338  v_out[i] += rowPtrs[i][j] * vj;
339  }
340  }
341 
342  return v_out;
343 }
344 
350 {
352 
353  for (unsigned int j = 0; j < 3; j++)
354  p[j] = 0;
355 
356  for (unsigned int j = 0; j < 3; j++) {
357  for (unsigned int i = 0; i < 3; i++) {
358  p[i] += rowPtrs[i][j] * tv[j];
359  }
360  }
361 
362  return p;
363 }
364 
370 {
372 
373  for (unsigned int i = 0; i < rowNum; i++)
374  for (unsigned int j = 0; j < colNum; j++)
375  R[i][j] = rowPtrs[i][j] * x;
376 
377  return R;
378 }
379 
385 {
386  for (unsigned int i = 0; i < rowNum; i++)
387  for (unsigned int j = 0; j < colNum; j++)
388  rowPtrs[i][j] *= x;
389 
390  return *this;
391 }
392 
393 /*********************************************************************/
394 
398 bool vpRotationMatrix::isARotationMatrix(double threshold) const
399 {
400  bool isRotation = true;
401 
402  if (getCols() != 3 || getRows() != 3) {
403  return false;
404  }
405 
406  // test R^TR = Id ;
407  vpRotationMatrix RtR = (*this).t() * (*this);
408  for (unsigned int i = 0; i < 3; i++) {
409  for (unsigned int j = 0; j < 3; j++) {
410  if (i == j) {
411  if (fabs(RtR[i][j] - 1) > threshold) {
412  isRotation = false;
413  }
414  } else {
415  if (fabs(RtR[i][j]) > threshold) {
416  isRotation = false;
417  }
418  }
419  }
420  }
421  // test if it is a basis
422  // test || Ci || = 1
423  for (unsigned int i = 0; i < 3; i++) {
424  if ((sqrt(vpMath::sqr(RtR[0][i]) + vpMath::sqr(RtR[1][i]) + vpMath::sqr(RtR[2][i])) - 1) > threshold) {
425  isRotation = false;
426  }
427  }
428 
429  // test || Ri || = 1
430  for (unsigned int i = 0; i < 3; i++) {
431  if ((sqrt(vpMath::sqr(RtR[i][0]) + vpMath::sqr(RtR[i][1]) + vpMath::sqr(RtR[i][2])) - 1) > threshold) {
432  isRotation = false;
433  }
434  }
435 
436  // test if the basis is orthogonal
437  return isRotation;
438 }
439 
444 
449 vpRotationMatrix::vpRotationMatrix(const vpRotationMatrix &M) : vpArray2D<double>(3, 3), m_index(0) { (*this) = M; }
450 
455 
461 
466 
471 vpRotationMatrix::vpRotationMatrix(const vpRzyzVector &euler) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(euler); }
472 
477 vpRotationMatrix::vpRotationMatrix(const vpRxyzVector &Rxyz) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(Rxyz); }
478 
483 vpRotationMatrix::vpRotationMatrix(const vpRzyxVector &Rzyx) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(Rzyx); }
484 
488 vpRotationMatrix::vpRotationMatrix(const vpMatrix &R) : vpArray2D<double>(3, 3), m_index(0) { *this = R; }
489 
494 vpRotationMatrix::vpRotationMatrix(double tux, double tuy, double tuz) : vpArray2D<double>(3, 3), m_index(0)
495 {
496  buildFrom(tux, tuy, tuz);
497 }
498 
503 
504 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
505 
528 vpRotationMatrix::vpRotationMatrix(const std::initializer_list<double> &list) : vpArray2D<double>(3, 3, list), m_index(0)
529 {
530  if (! isARotationMatrix() ) {
531  throw(vpException(vpException::fatalError, "Rotation matrix initialization fails since it's elements doesn't represent a rotation matrix"));
532  }
533 }
534 #endif
535 
543 {
544  vpRotationMatrix Rt;
545 
546  for (unsigned int i = 0; i < 3; i++)
547  for (unsigned int j = 0; j < 3; j++)
548  Rt[j][i] = (*this)[i][j];
549 
550  return Rt;
551 }
552 
560 {
561  vpRotationMatrix Ri = (*this).t();
562 
563  return Ri;
564 }
565 
584 
590 {
591  vpThetaUVector tu(*this);
592 
593  for (unsigned int i = 0; i < 3; i++)
594  std::cout << tu[i] << " ";
595 
596  std::cout << std::endl;
597 }
598 
609 {
610  double theta, si, co, sinc, mcosc;
612 
613  theta = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
614  si = sin(theta);
615  co = cos(theta);
616  sinc = vpMath::sinc(si, theta);
617  mcosc = vpMath::mcosc(co, theta);
618 
619  R[0][0] = co + mcosc * v[0] * v[0];
620  R[0][1] = -sinc * v[2] + mcosc * v[0] * v[1];
621  R[0][2] = sinc * v[1] + mcosc * v[0] * v[2];
622  R[1][0] = sinc * v[2] + mcosc * v[1] * v[0];
623  R[1][1] = co + mcosc * v[1] * v[1];
624  R[1][2] = -sinc * v[0] + mcosc * v[1] * v[2];
625  R[2][0] = -sinc * v[1] + mcosc * v[2] * v[0];
626  R[2][1] = sinc * v[0] + mcosc * v[2] * v[1];
627  R[2][2] = co + mcosc * v[2] * v[2];
628 
629  for (unsigned int i = 0; i < 3; i++)
630  for (unsigned int j = 0; j < 3; j++)
631  (*this)[i][j] = R[i][j];
632 
633  return *this;
634 }
635 
640 {
641  for (unsigned int i = 0; i < 3; i++)
642  for (unsigned int j = 0; j < 3; j++)
643  (*this)[i][j] = M[i][j];
644 
645  return *this;
646 }
647 
654 {
655  vpThetaUVector tu(p);
656  return buildFrom(tu);
657 }
658 
667 {
668  double c0, c1, c2, s0, s1, s2;
669 
670  c0 = cos(v[0]);
671  c1 = cos(v[1]);
672  c2 = cos(v[2]);
673  s0 = sin(v[0]);
674  s1 = sin(v[1]);
675  s2 = sin(v[2]);
676 
677  (*this)[0][0] = c0 * c1 * c2 - s0 * s2;
678  (*this)[0][1] = -c0 * c1 * s2 - s0 * c2;
679  (*this)[0][2] = c0 * s1;
680  (*this)[1][0] = s0 * c1 * c2 + c0 * s2;
681  (*this)[1][1] = -s0 * c1 * s2 + c0 * c2;
682  (*this)[1][2] = s0 * s1;
683  (*this)[2][0] = -s1 * c2;
684  (*this)[2][1] = s1 * s2;
685  (*this)[2][2] = c1;
686 
687  return (*this);
688 }
689 
699 {
700  double c0, c1, c2, s0, s1, s2;
701 
702  c0 = cos(v[0]);
703  c1 = cos(v[1]);
704  c2 = cos(v[2]);
705  s0 = sin(v[0]);
706  s1 = sin(v[1]);
707  s2 = sin(v[2]);
708 
709  (*this)[0][0] = c1 * c2;
710  (*this)[0][1] = -c1 * s2;
711  (*this)[0][2] = s1;
712  (*this)[1][0] = c0 * s2 + s0 * s1 * c2;
713  (*this)[1][1] = c0 * c2 - s0 * s1 * s2;
714  (*this)[1][2] = -s0 * c1;
715  (*this)[2][0] = -c0 * s1 * c2 + s0 * s2;
716  (*this)[2][1] = c0 * s1 * s2 + c2 * s0;
717  (*this)[2][2] = c0 * c1;
718 
719  return (*this);
720 }
721 
729 {
730  double c0, c1, c2, s0, s1, s2;
731 
732  c0 = cos(v[0]);
733  c1 = cos(v[1]);
734  c2 = cos(v[2]);
735  s0 = sin(v[0]);
736  s1 = sin(v[1]);
737  s2 = sin(v[2]);
738 
739  (*this)[0][0] = c0 * c1;
740  (*this)[0][1] = c0 * s1 * s2 - s0 * c2;
741  (*this)[0][2] = c0 * s1 * c2 + s0 * s2;
742 
743  (*this)[1][0] = s0 * c1;
744  (*this)[1][1] = s0 * s1 * s2 + c0 * c2;
745  (*this)[1][2] = s0 * s1 * c2 - c0 * s2;
746 
747  (*this)[2][0] = -s1;
748  (*this)[2][1] = c1 * s2;
749  (*this)[2][2] = c1 * c2;
750 
751  return (*this);
752 }
753 
758 vpRotationMatrix vpRotationMatrix::buildFrom(double tux, double tuy, double tuz)
759 {
760  vpThetaUVector tu(tux, tuy, tuz);
761  buildFrom(tu);
762  return *this;
763 }
764 
769 {
770  double a = q.w();
771  double b = q.x();
772  double c = q.y();
773  double d = q.z();
774  (*this)[0][0] = a * a + b * b - c * c - d * d;
775  (*this)[0][1] = 2 * b * c - 2 * a * d;
776  (*this)[0][2] = 2 * a * c + 2 * b * d;
777 
778  (*this)[1][0] = 2 * a * d + 2 * b * c;
779  (*this)[1][1] = a * a - b * b + c * c - d * d;
780  (*this)[1][2] = 2 * c * d - 2 * a * b;
781 
782  (*this)[2][0] = 2 * b * d - 2 * a * c;
783  (*this)[2][1] = 2 * a * b + 2 * c * d;
784  (*this)[2][2] = a * a - b * b - c * c + d * d;
785  return *this;
786 }
787 
791 vpRotationMatrix operator*(const double &x, const vpRotationMatrix &R)
792 {
794 
795  unsigned int Rrow = R.getRows();
796  unsigned int Rcol = R.getCols();
797 
798  for (unsigned int i = 0; i < Rrow; i++)
799  for (unsigned int j = 0; j < Rcol; j++)
800  C[i][j] = R[i][j] * x;
801 
802  return C;
803 }
804 
810 {
811  vpThetaUVector tu;
812  tu.buildFrom(*this);
813  return tu;
814 }
815 
844 {
845  if (j >= getCols())
846  throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the homogeneous matrix"));
847  unsigned int nb_rows = getRows();
848  vpColVector c(nb_rows);
849  for (unsigned int i = 0; i < nb_rows; i++)
850  c[i] = (*this)[i][j];
851  return c;
852 }
853 
862 vpRotationMatrix vpRotationMatrix::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
863 {
864  vpMatrix meanR(3, 3);
866  for (size_t i = 0; i < vec_M.size(); i++) {
867  R = vec_M[i].getRotationMatrix();
868  meanR += (vpMatrix) R;
869  }
870  meanR /= static_cast<double>(vec_M.size());
871 
872  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
873  vpMatrix M, U, V;
874  vpColVector sv;
875  meanR.pseudoInverse(M, sv, 1e-6, U, V);
876  double det = sv[0]*sv[1]*sv[2];
877  if (det > 0) {
878  meanR = U * V.t();
879  }
880  else {
881  vpMatrix D(3,3);
882  D = 0.0;
883  D[0][0] = D[1][1] = 1.0; D[2][2] = -1;
884  meanR = U * D * V.t();
885  }
886 
887  R = meanR;
888  return R;
889 }
890 
899 vpRotationMatrix vpRotationMatrix::mean(const std::vector<vpRotationMatrix> &vec_R)
900 {
901  vpMatrix meanR(3, 3);
903  for (size_t i = 0; i < vec_R.size(); i++) {
904  meanR += (vpMatrix) vec_R[i];
905  }
906  meanR /= static_cast<double>(vec_R.size());
907 
908  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
909  vpMatrix M, U, V;
910  vpColVector sv;
911  meanR.pseudoInverse(M, sv, 1e-6, U, V);
912  double det = sv[0]*sv[1]*sv[2];
913  if (det > 0) {
914  meanR = U * V.t();
915  }
916  else {
917  vpMatrix D(3,3);
918  D = 0.0;
919  D[0][0] = D[1][1] = 1.0; D[2][2] = -1;
920  meanR = U * D * V.t();
921  }
922 
923  R = meanR;
924  return R;
925 }
926 
927 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
928 
937 
938 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
bool isARotationMatrix(double threshold=1e-6) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
double z() const
Returns z-component of the quaternion.
vpRotationMatrix & operator,(double val)
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getRows() const
Definition: vpArray2D.h:289
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
Implementation of a generic 2D array used as base class for matrices and vectors. ...
Definition: vpArray2D.h:131
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyxVector.h:185
vp_deprecated void setIdentity()
vpRotationMatrix inverse() const
vpRotationMatrix & operator=(const vpRotationMatrix &R)
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
vpRotationMatrix t() const
static double sinc(double x)
Definition: vpMath.cpp:169
Implementation of a rotation matrix and operations on such kind of matrices.
unsigned int getCols() const
Definition: vpArray2D.h:279
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
vpMatrix t() const
Definition: vpMatrix.cpp:464
static vpRotationMatrix mean(const std::vector< vpHomogeneousMatrix > &vec_M)
static double mcosc(double cosx, double x)
Definition: vpMath.cpp:136
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
vpRotationMatrix & operator<<(double val)
static double sqr(double x)
Definition: vpMath.h:116
vpColVector getCol(unsigned int j) const
Implementation of a rotation vector as quaternion angle minimal representation.
double y() const
Returns y-component of the quaternion.
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
double x() const
Returns x-component of the quaternion.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
double w() const
Returns w-component of the quaternion.
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:141
vpThetaUVector getThetaUVector()
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:182
unsigned int m_index
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:139
vpRotationMatrix & operator*=(double x)
vpTranslationVector operator*(const vpTranslationVector &tv) const