Visual Servoing Platform  version 3.2.1 under development (2019-05-26)
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 const double vpRotationMatrix::threshold = 1e-6;
58 
65 {
66  for (unsigned int i = 0; i < 3; i++) {
67  for (unsigned int j = 0; j < 3; j++) {
68  if (i == j)
69  (*this)[i][j] = 1.0;
70  else
71  (*this)[i][j] = 0.0;
72  }
73  }
74 }
75 
86 {
87  for (unsigned int i = 0; i < 3; i++) {
88  for (unsigned int j = 0; j < 3; j++) {
89  rowPtrs[i][j] = R.rowPtrs[i][j];
90  }
91  }
92 
93  return *this;
94 }
95 
96 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
97 
122 vpRotationMatrix& vpRotationMatrix::operator=(const std::initializer_list<double> &list)
123 {
124  if (dsize != static_cast<unsigned int>(list.size())) {
125  throw(vpException(vpException::dimensionError, "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  throw(vpException(vpException::fatalError, "Rotation matrix initialization fails since it's elements doesn't represent a rotation matrix"));
132  }
133 
134  return *this;
135 }
136 #endif
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()) {
241  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));
242  }
243  data[m_index] = val;
244  return *this;
245 }
246 
251 {
253 
254  for (unsigned int i = 0; i < 3; i++) {
255  for (unsigned int j = 0; j < 3; j++) {
256  double s = 0;
257  for (unsigned int k = 0; k < 3; k++)
258  s += rowPtrs[i][k] * R.rowPtrs[k][j];
259  p[i][j] = s;
260  }
261  }
262  return p;
263 }
279 {
280  if (M.getRows() != 3 || M.getCols() != 3) {
281  throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
282  M.getRows(), M.getCols()));
283  }
284  vpMatrix p(3, 3);
285 
286  for (unsigned int i = 0; i < 3; i++) {
287  for (unsigned int j = 0; j < 3; j++) {
288  double s = 0;
289  for (unsigned int k = 0; k < 3; k++)
290  s += (*this)[i][k] * M[k][j];
291  p[i][j] = s;
292  }
293  }
294  return p;
295 }
296 
327 {
328  if (v.getRows() != 3) {
330  "Cannot multiply a (3x3) rotation matrix by a %d "
331  "dimension column vector",
332  v.getRows()));
333  }
334  vpColVector v_out(3);
335 
336  for (unsigned int j = 0; j < colNum; j++) {
337  double vj = v[j]; // optimization em 5/12/2006
338  for (unsigned int i = 0; i < rowNum; i++) {
339  v_out[i] += rowPtrs[i][j] * vj;
340  }
341  }
342 
343  return v_out;
344 }
345 
351 {
353 
354  for (unsigned int j = 0; j < 3; j++)
355  p[j] = 0;
356 
357  for (unsigned int j = 0; j < 3; j++) {
358  for (unsigned int i = 0; i < 3; i++) {
359  p[i] += rowPtrs[i][j] * tv[j];
360  }
361  }
362 
363  return p;
364 }
365 
371 {
373 
374  for (unsigned int i = 0; i < rowNum; i++)
375  for (unsigned int j = 0; j < colNum; j++)
376  R[i][j] = rowPtrs[i][j] * x;
377 
378  return R;
379 }
380 
386 {
387  for (unsigned int i = 0; i < rowNum; i++)
388  for (unsigned int j = 0; j < colNum; j++)
389  rowPtrs[i][j] *= x;
390 
391  return *this;
392 }
393 
394 /*********************************************************************/
395 
400 {
401  unsigned int i, j;
402  bool isRotation = true;
403 
404  if (getCols() != 3 || getRows() != 3) {
405  return false;
406  }
407 
408  // test R^TR = Id ;
409  vpRotationMatrix RtR = (*this).t() * (*this);
410  for (i = 0; i < 3; i++) {
411  for (j = 0; j < 3; j++) {
412  if (i == j) {
413  if (fabs(RtR[i][j] - 1) > threshold)
414  isRotation = false;
415  } else {
416  if (fabs(RtR[i][j]) > threshold)
417  isRotation = false;
418  }
419  }
420  }
421  // test if it is a basis
422  // test || Ci || = 1
423  for (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  // test || Ri || = 1
429  for (i = 0; i < 3; i++) {
430  if ((sqrt(vpMath::sqr(RtR[i][0]) + vpMath::sqr(RtR[i][1]) + vpMath::sqr(RtR[i][2])) - 1) > threshold)
431  isRotation = false;
432  }
433 
434  // test if the basis is orthogonal
435  return isRotation;
436 }
437 
442 
447 vpRotationMatrix::vpRotationMatrix(const vpRotationMatrix &M) : vpArray2D<double>(3, 3), m_index(0) { (*this) = M; }
452 
458 
463 
468 vpRotationMatrix::vpRotationMatrix(const vpRzyzVector &euler) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(euler); }
469 
474 vpRotationMatrix::vpRotationMatrix(const vpRxyzVector &Rxyz) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(Rxyz); }
475 
480 vpRotationMatrix::vpRotationMatrix(const vpRzyxVector &Rzyx) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(Rzyx); }
481 
485 vpRotationMatrix::vpRotationMatrix(const vpMatrix &R) : vpArray2D<double>(3, 3), m_index(0) { *this = R; }
486 
491 vpRotationMatrix::vpRotationMatrix(const double tux, const double tuy, const double tuz) : vpArray2D<double>(3, 3), m_index(0)
492 {
493  buildFrom(tux, tuy, tuz);
494 }
495 
500 
501 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
502 
525 vpRotationMatrix::vpRotationMatrix(const std::initializer_list<double> &list) : vpArray2D<double>(3, 3, list), m_index(0)
526 {
527  if (! isARotationMatrix() ) {
528  throw(vpException(vpException::fatalError, "Rotation matrix initialization fails since it's elements doesn't represent a rotation matrix"));
529  }
530 }
531 #endif
532 
540 {
541  vpRotationMatrix Rt;
542 
543  unsigned int i, j;
544  for (i = 0; i < 3; i++)
545  for (j = 0; j < 3; j++)
546  Rt[j][i] = (*this)[i][j];
547 
548  return Rt;
549 }
550 
558 {
559  vpRotationMatrix Ri = (*this).t();
560 
561  return Ri;
562 }
563 
582 
588 {
589  vpThetaUVector tu(*this);
590 
591  for (unsigned int i = 0; i < 3; i++)
592  std::cout << tu[i] << " ";
593 
594  std::cout << std::endl;
595 }
596 
607 {
608  unsigned int i, j;
609  double theta, si, co, sinc, mcosc;
611 
612  theta = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
613  si = sin(theta);
614  co = cos(theta);
615  sinc = vpMath::sinc(si, theta);
616  mcosc = vpMath::mcosc(co, theta);
617 
618  R[0][0] = co + mcosc * v[0] * v[0];
619  R[0][1] = -sinc * v[2] + mcosc * v[0] * v[1];
620  R[0][2] = sinc * v[1] + mcosc * v[0] * v[2];
621  R[1][0] = sinc * v[2] + mcosc * v[1] * v[0];
622  R[1][1] = co + mcosc * v[1] * v[1];
623  R[1][2] = -sinc * v[0] + mcosc * v[1] * v[2];
624  R[2][0] = -sinc * v[1] + mcosc * v[2] * v[0];
625  R[2][1] = sinc * v[0] + mcosc * v[2] * v[1];
626  R[2][2] = co + mcosc * v[2] * v[2];
627 
628  for (i = 0; i < 3; i++)
629  for (j = 0; j < 3; j++)
630  (*this)[i][j] = R[i][j];
631 
632  return *this;
633 }
634 
639 {
640  for (unsigned int i = 0; i < 3; i++)
641  for (unsigned int j = 0; j < 3; j++)
642  (*this)[i][j] = M[i][j];
643 
644  return *this;
645 }
646 
653 {
654  vpThetaUVector tu(p);
655  return buildFrom(tu);
656 }
657 
666 {
667  double c0, c1, c2, s0, s1, s2;
668 
669  c0 = cos(v[0]);
670  c1 = cos(v[1]);
671  c2 = cos(v[2]);
672  s0 = sin(v[0]);
673  s1 = sin(v[1]);
674  s2 = sin(v[2]);
675 
676  (*this)[0][0] = c0 * c1 * c2 - s0 * s2;
677  (*this)[0][1] = -c0 * c1 * s2 - s0 * c2;
678  (*this)[0][2] = c0 * s1;
679  (*this)[1][0] = s0 * c1 * c2 + c0 * s2;
680  (*this)[1][1] = -s0 * c1 * s2 + c0 * c2;
681  (*this)[1][2] = s0 * s1;
682  (*this)[2][0] = -s1 * c2;
683  (*this)[2][1] = s1 * s2;
684  (*this)[2][2] = c1;
685 
686  return (*this);
687 }
688 
698 {
699  double c0, c1, c2, s0, s1, s2;
700 
701  c0 = cos(v[0]);
702  c1 = cos(v[1]);
703  c2 = cos(v[2]);
704  s0 = sin(v[0]);
705  s1 = sin(v[1]);
706  s2 = sin(v[2]);
707 
708  (*this)[0][0] = c1 * c2;
709  (*this)[0][1] = -c1 * s2;
710  (*this)[0][2] = s1;
711  (*this)[1][0] = c0 * s2 + s0 * s1 * c2;
712  (*this)[1][1] = c0 * c2 - s0 * s1 * s2;
713  (*this)[1][2] = -s0 * c1;
714  (*this)[2][0] = -c0 * s1 * c2 + s0 * s2;
715  (*this)[2][1] = c0 * s1 * s2 + c2 * s0;
716  (*this)[2][2] = c0 * c1;
717 
718  return (*this);
719 }
720 
728 {
729  double c0, c1, c2, s0, s1, s2;
730 
731  c0 = cos(v[0]);
732  c1 = cos(v[1]);
733  c2 = cos(v[2]);
734  s0 = sin(v[0]);
735  s1 = sin(v[1]);
736  s2 = sin(v[2]);
737 
738  (*this)[0][0] = c0 * c1;
739  (*this)[0][1] = c0 * s1 * s2 - s0 * c2;
740  (*this)[0][2] = c0 * s1 * c2 + s0 * s2;
741 
742  (*this)[1][0] = s0 * c1;
743  (*this)[1][1] = s0 * s1 * s2 + c0 * c2;
744  (*this)[1][2] = s0 * s1 * c2 - c0 * s2;
745 
746  (*this)[2][0] = -s1;
747  (*this)[2][1] = c1 * s2;
748  (*this)[2][2] = c1 * c2;
749 
750  return (*this);
751 }
752 
757 vpRotationMatrix vpRotationMatrix::buildFrom(const double tux, const double tuy, const double tuz)
758 {
759  vpThetaUVector tu(tux, tuy, tuz);
760  buildFrom(tu);
761  return *this;
762 }
763 
768 {
769  double a = q.w();
770  double b = q.x();
771  double c = q.y();
772  double d = q.z();
773  (*this)[0][0] = a * a + b * b - c * c - d * d;
774  (*this)[0][1] = 2 * b * c - 2 * a * d;
775  (*this)[0][2] = 2 * a * c + 2 * b * d;
776 
777  (*this)[1][0] = 2 * a * d + 2 * b * c;
778  (*this)[1][1] = a * a - b * b + c * c - d * d;
779  (*this)[1][2] = 2 * c * d - 2 * a * b;
780 
781  (*this)[2][0] = 2 * b * d - 2 * a * c;
782  (*this)[2][1] = 2 * a * b + 2 * c * d;
783  (*this)[2][2] = a * a - b * b - c * c + d * d;
784  return *this;
785 }
786 
790 vpRotationMatrix operator*(const double &x, const vpRotationMatrix &R)
791 {
793 
794  unsigned int Rrow = R.getRows();
795  unsigned int Rcol = R.getCols();
796 
797  for (unsigned int i = 0; i < Rrow; i++)
798  for (unsigned int j = 0; j < Rcol; j++)
799  C[i][j] = R[i][j] * x;
800 
801  return C;
802 }
803 
809 {
810  vpThetaUVector tu;
811  tu.buildFrom(*this);
812  return tu;
813 }
814 
842 vpColVector vpRotationMatrix::getCol(const unsigned int j) const
843 {
844  if (j >= getCols())
845  throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the homogeneous matrix"));
846  unsigned int nb_rows = getRows();
847  vpColVector c(nb_rows);
848  for (unsigned int i = 0; i < nb_rows; i++)
849  c[i] = (*this)[i][j];
850  return c;
851 }
852 
861 vpRotationMatrix vpRotationMatrix::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
862 {
863  vpMatrix meanR(3, 3);
865  for (size_t i = 0; i < vec_M.size(); i++) {
866  R = vec_M[i].getRotationMatrix();
867  meanR += (vpMatrix) R;
868  }
869  meanR /= static_cast<double>(vec_M.size());
870 
871  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
872  vpMatrix M, U, V;
873  vpColVector sv;
874  meanR.pseudoInverse(M, sv, 1e-6, U, V);
875  double det = sv[0]*sv[1]*sv[2];
876  if (det > 0) {
877  meanR = U * V.t();
878  }
879  else {
880  vpMatrix D(3,3);
881  D = 0.0;
882  D[0][0] = D[1][1] = 1.0; D[2][2] = -1;
883  meanR = U * D * V.t();
884  }
885 
886  R = meanR;
887  return R;
888 }
889 
898 vpRotationMatrix vpRotationMatrix::mean(const std::vector<vpRotationMatrix> &vec_R)
899 {
900  vpMatrix meanR(3, 3);
902  for (size_t i = 0; i < vec_R.size(); i++) {
903  meanR += (vpMatrix) vec_R[i];
904  }
905  meanR /= static_cast<double>(vec_R.size());
906 
907  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
908  vpMatrix M, U, V;
909  vpColVector sv;
910  meanR.pseudoInverse(M, sv, 1e-6, U, V);
911  double det = sv[0]*sv[1]*sv[2];
912  if (det > 0) {
913  meanR = U * V.t();
914  }
915  else {
916  vpMatrix D(3,3);
917  D = 0.0;
918  D[0][0] = D[1][1] = 1.0; D[2][2] = -1;
919  meanR = U * D * V.t();
920  }
921 
922  R = meanR;
923  return R;
924 }
925 
926 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
927 
936 
937 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpRotationMatrix inverse() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix & operator,(double val)
error that can be emited by ViSP classes.
Definition: vpException.h:71
double y() const
Returns y-component of the quaternion.
vpColVector getCol(const unsigned int j) const
vpRotationMatrix t() const
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
unsigned int getCols() const
Definition: vpArray2D.h:279
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyxVector.h:185
vp_deprecated void setIdentity()
vpRotationMatrix & operator=(const vpRotationMatrix &R)
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
static double sinc(double x)
Definition: vpMath.cpp:170
bool isARotationMatrix() const
Implementation of a rotation matrix and operations on such kind of matrices.
double w() const
Returns w-component of the quaternion.
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
vpRotationMatrix & operator*=(const double x)
static vpRotationMatrix mean(const std::vector< vpHomogeneousMatrix > &vec_M)
static double mcosc(double cosx, double x)
Definition: vpMath.cpp:135
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
vpRotationMatrix & operator<<(double val)
static double sqr(double x)
Definition: vpMath.h:114
double z() const
Returns z-component of the quaternion.
double x() const
Returns x-component of the quaternion.
Implementation of a rotation vector as quaternion angle minimal representation.
unsigned int getRows() const
Definition: vpArray2D.h:289
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
vpTranslationVector operator*(const vpTranslationVector &tv) const
vpMatrix t() const
Definition: vpMatrix.cpp:375
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
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2021
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