Visual Servoing Platform  version 3.2.0 under development (2019-01-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 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 
113 {
114  if ((M.getCols() != 3) && (M.getRows() != 3)) {
115  throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
116  M.getRows(), M.getCols()));
117  }
118 
119  for (unsigned int i = 0; i < 3; i++) {
120  for (unsigned int j = 0; j < 3; j++) {
121  (*this)[i][j] = M[i][j];
122  }
123  }
124 
125  if (isARotationMatrix() == false) {
126  throw(vpException(vpException::fatalError, "Cannot set a rotation matrix "
127  "from a matrix that is not a "
128  "rotation matrix"));
129  }
130 
131  return *this;
132 }
133 
138 {
140 
141  for (unsigned int i = 0; i < 3; i++) {
142  for (unsigned int j = 0; j < 3; j++) {
143  double s = 0;
144  for (unsigned int k = 0; k < 3; k++)
145  s += rowPtrs[i][k] * R.rowPtrs[k][j];
146  p[i][j] = s;
147  }
148  }
149  return p;
150 }
166 {
167  if (M.getRows() != 3 || M.getCols() != 3) {
168  throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
169  M.getRows(), M.getCols()));
170  }
171  vpMatrix p(3, 3);
172 
173  for (unsigned int i = 0; i < 3; i++) {
174  for (unsigned int j = 0; j < 3; j++) {
175  double s = 0;
176  for (unsigned int k = 0; k < 3; k++)
177  s += (*this)[i][k] * M[k][j];
178  p[i][j] = s;
179  }
180  }
181  return p;
182 }
183 
214 {
215  if (v.getRows() != 3) {
217  "Cannot multiply a (3x3) rotation matrix by a %d "
218  "dimension column vector",
219  v.getRows()));
220  }
221  vpColVector v_out(3);
222 
223  for (unsigned int j = 0; j < colNum; j++) {
224  double vj = v[j]; // optimization em 5/12/2006
225  for (unsigned int i = 0; i < rowNum; i++) {
226  v_out[i] += rowPtrs[i][j] * vj;
227  }
228  }
229 
230  return v_out;
231 }
232 
238 {
240 
241  for (unsigned int j = 0; j < 3; j++)
242  p[j] = 0;
243 
244  for (unsigned int j = 0; j < 3; j++) {
245  for (unsigned int i = 0; i < 3; i++) {
246  p[i] += rowPtrs[i][j] * tv[j];
247  }
248  }
249 
250  return p;
251 }
252 
258 {
260 
261  for (unsigned int i = 0; i < rowNum; i++)
262  for (unsigned int j = 0; j < colNum; j++)
263  R[i][j] = rowPtrs[i][j] * x;
264 
265  return R;
266 }
267 
273 {
274  for (unsigned int i = 0; i < rowNum; i++)
275  for (unsigned int j = 0; j < colNum; j++)
276  rowPtrs[i][j] *= x;
277 
278  return *this;
279 }
280 
281 /*********************************************************************/
282 
287 {
288  unsigned int i, j;
289  bool isRotation = true;
290 
291  // test R^TR = Id ;
292  vpRotationMatrix RtR = (*this).t() * (*this);
293  for (i = 0; i < 3; i++) {
294  for (j = 0; j < 3; j++) {
295  if (i == j) {
296  if (fabs(RtR[i][j] - 1) > threshold)
297  isRotation = false;
298  } else {
299  if (fabs(RtR[i][j]) > threshold)
300  isRotation = false;
301  }
302  }
303  }
304  // test if it is a basis
305  // test || Ci || = 1
306  for (i = 0; i < 3; i++) {
307  if ((sqrt(vpMath::sqr(RtR[0][i]) + vpMath::sqr(RtR[1][i]) + vpMath::sqr(RtR[2][i])) - 1) > threshold)
308  isRotation = false;
309  }
310 
311  // test || Ri || = 1
312  for (i = 0; i < 3; i++) {
313  if ((sqrt(vpMath::sqr(RtR[i][0]) + vpMath::sqr(RtR[i][1]) + vpMath::sqr(RtR[i][2])) - 1) > threshold)
314  isRotation = false;
315  }
316 
317  // test if the basis is orthogonal
318  return isRotation;
319 }
320 
325 
330 vpRotationMatrix::vpRotationMatrix(const vpRotationMatrix &M) : vpArray2D<double>(3, 3) { (*this) = M; }
335 
341 
346 
351 vpRotationMatrix::vpRotationMatrix(const vpRzyzVector &euler) : vpArray2D<double>(3, 3) { buildFrom(euler); }
352 
358 
364 
368 vpRotationMatrix::vpRotationMatrix(const vpMatrix &R) : vpArray2D<double>(3, 3) { *this = R; }
369 
374 vpRotationMatrix::vpRotationMatrix(const double tux, const double tuy, const double tuz) : vpArray2D<double>(3, 3)
375 {
376  buildFrom(tux, tuy, tuz);
377 }
378 
383 
391 {
392  vpRotationMatrix Rt;
393 
394  unsigned int i, j;
395  for (i = 0; i < 3; i++)
396  for (j = 0; j < 3; j++)
397  Rt[j][i] = (*this)[i][j];
398 
399  return Rt;
400 }
401 
409 {
410  vpRotationMatrix Ri = (*this).t();
411 
412  return Ri;
413 }
414 
433 
439 {
440  vpThetaUVector tu(*this);
441 
442  for (unsigned int i = 0; i < 3; i++)
443  std::cout << tu[i] << " ";
444 
445  std::cout << std::endl;
446 }
447 
458 {
459  unsigned int i, j;
460  double theta, si, co, sinc, mcosc;
462 
463  theta = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
464  si = sin(theta);
465  co = cos(theta);
466  sinc = vpMath::sinc(si, theta);
467  mcosc = vpMath::mcosc(co, theta);
468 
469  R[0][0] = co + mcosc * v[0] * v[0];
470  R[0][1] = -sinc * v[2] + mcosc * v[0] * v[1];
471  R[0][2] = sinc * v[1] + mcosc * v[0] * v[2];
472  R[1][0] = sinc * v[2] + mcosc * v[1] * v[0];
473  R[1][1] = co + mcosc * v[1] * v[1];
474  R[1][2] = -sinc * v[0] + mcosc * v[1] * v[2];
475  R[2][0] = -sinc * v[1] + mcosc * v[2] * v[0];
476  R[2][1] = sinc * v[0] + mcosc * v[2] * v[1];
477  R[2][2] = co + mcosc * v[2] * v[2];
478 
479  for (i = 0; i < 3; i++)
480  for (j = 0; j < 3; j++)
481  (*this)[i][j] = R[i][j];
482 
483  return *this;
484 }
485 
490 {
491  for (unsigned int i = 0; i < 3; i++)
492  for (unsigned int j = 0; j < 3; j++)
493  (*this)[i][j] = M[i][j];
494 
495  return *this;
496 }
497 
504 {
505  vpThetaUVector tu(p);
506  return buildFrom(tu);
507 }
508 
517 {
518  double c0, c1, c2, s0, s1, s2;
519 
520  c0 = cos(v[0]);
521  c1 = cos(v[1]);
522  c2 = cos(v[2]);
523  s0 = sin(v[0]);
524  s1 = sin(v[1]);
525  s2 = sin(v[2]);
526 
527  (*this)[0][0] = c0 * c1 * c2 - s0 * s2;
528  (*this)[0][1] = -c0 * c1 * s2 - s0 * c2;
529  (*this)[0][2] = c0 * s1;
530  (*this)[1][0] = s0 * c1 * c2 + c0 * s2;
531  (*this)[1][1] = -s0 * c1 * s2 + c0 * c2;
532  (*this)[1][2] = s0 * s1;
533  (*this)[2][0] = -s1 * c2;
534  (*this)[2][1] = s1 * s2;
535  (*this)[2][2] = c1;
536 
537  return (*this);
538 }
539 
549 {
550  double c0, c1, c2, s0, s1, s2;
551 
552  c0 = cos(v[0]);
553  c1 = cos(v[1]);
554  c2 = cos(v[2]);
555  s0 = sin(v[0]);
556  s1 = sin(v[1]);
557  s2 = sin(v[2]);
558 
559  (*this)[0][0] = c1 * c2;
560  (*this)[0][1] = -c1 * s2;
561  (*this)[0][2] = s1;
562  (*this)[1][0] = c0 * s2 + s0 * s1 * c2;
563  (*this)[1][1] = c0 * c2 - s0 * s1 * s2;
564  (*this)[1][2] = -s0 * c1;
565  (*this)[2][0] = -c0 * s1 * c2 + s0 * s2;
566  (*this)[2][1] = c0 * s1 * s2 + c2 * s0;
567  (*this)[2][2] = c0 * c1;
568 
569  return (*this);
570 }
571 
579 {
580  double c0, c1, c2, s0, s1, s2;
581 
582  c0 = cos(v[0]);
583  c1 = cos(v[1]);
584  c2 = cos(v[2]);
585  s0 = sin(v[0]);
586  s1 = sin(v[1]);
587  s2 = sin(v[2]);
588 
589  (*this)[0][0] = c0 * c1;
590  (*this)[0][1] = c0 * s1 * s2 - s0 * c2;
591  (*this)[0][2] = c0 * s1 * c2 + s0 * s2;
592 
593  (*this)[1][0] = s0 * c1;
594  (*this)[1][1] = s0 * s1 * s2 + c0 * c2;
595  (*this)[1][2] = s0 * s1 * c2 - c0 * s2;
596 
597  (*this)[2][0] = -s1;
598  (*this)[2][1] = c1 * s2;
599  (*this)[2][2] = c1 * c2;
600 
601  return (*this);
602 }
603 
608 vpRotationMatrix vpRotationMatrix::buildFrom(const double tux, const double tuy, const double tuz)
609 {
610  vpThetaUVector tu(tux, tuy, tuz);
611  buildFrom(tu);
612  return *this;
613 }
614 
619 {
620  double a = q.w();
621  double b = q.x();
622  double c = q.y();
623  double d = q.z();
624  (*this)[0][0] = a * a + b * b - c * c - d * d;
625  (*this)[0][1] = 2 * b * c - 2 * a * d;
626  (*this)[0][2] = 2 * a * c + 2 * b * d;
627 
628  (*this)[1][0] = 2 * a * d + 2 * b * c;
629  (*this)[1][1] = a * a - b * b + c * c - d * d;
630  (*this)[1][2] = 2 * c * d - 2 * a * b;
631 
632  (*this)[2][0] = 2 * b * d - 2 * a * c;
633  (*this)[2][1] = 2 * a * b + 2 * c * d;
634  (*this)[2][2] = a * a - b * b - c * c + d * d;
635  return *this;
636 }
637 
641 vpRotationMatrix operator*(const double &x, const vpRotationMatrix &R)
642 {
644 
645  unsigned int Rrow = R.getRows();
646  unsigned int Rcol = R.getCols();
647 
648  for (unsigned int i = 0; i < Rrow; i++)
649  for (unsigned int j = 0; j < Rcol; j++)
650  C[i][j] = R[i][j] * x;
651 
652  return C;
653 }
654 
660 {
661  vpThetaUVector tu;
662  tu.buildFrom(*this);
663  return tu;
664 }
665 
693 vpColVector vpRotationMatrix::getCol(const unsigned int j) const
694 {
695  if (j >= getCols())
696  throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the homogeneous matrix"));
697  unsigned int nb_rows = getRows();
698  vpColVector c(nb_rows);
699  for (unsigned int i = 0; i < nb_rows; i++)
700  c[i] = (*this)[i][j];
701  return c;
702 }
703 
712 vpRotationMatrix vpRotationMatrix::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
713 {
714  vpMatrix meanR(3, 3);
716  for (size_t i = 0; i < vec_M.size(); i++) {
717  R = vec_M[i].getRotationMatrix();
718  meanR += (vpMatrix) R;
719  }
720  meanR /= static_cast<double>(vec_M.size());
721 
722  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
723  vpMatrix M, U, V;
724  vpColVector sv;
725  meanR.pseudoInverse(M, sv, 1e-6, U, V);
726  double det = sv[0]*sv[1]*sv[2];
727  if (det > 0) {
728  meanR = U * V.t();
729  }
730  else {
731  vpMatrix D(3,3);
732  D = 0.0;
733  D[0][0] = D[1][1] = 1.0; D[2][2] = -1;
734  meanR = U * D * V.t();
735  }
736 
737  R = meanR;
738  return R;
739 }
740 
749 vpRotationMatrix vpRotationMatrix::mean(const std::vector<vpRotationMatrix> &vec_R)
750 {
751  vpMatrix meanR(3, 3);
753  for (size_t i = 0; i < vec_R.size(); i++) {
754  meanR += (vpMatrix) vec_R[i];
755  }
756  meanR /= static_cast<double>(vec_R.size());
757 
758  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
759  vpMatrix M, U, V;
760  vpColVector sv;
761  meanR.pseudoInverse(M, sv, 1e-6, U, V);
762  double det = sv[0]*sv[1]*sv[2];
763  if (det > 0) {
764  meanR = U * V.t();
765  }
766  else {
767  vpMatrix D(3,3);
768  D = 0.0;
769  D[0][0] = D[1][1] = 1.0; D[2][2] = -1;
770  meanR = U * D * V.t();
771  }
772 
773  R = meanR;
774  return R;
775 }
776 
777 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
778 
787 
788 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
vpRotationMatrix inverse() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
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
Implementation of a generic 2D array used as vase class of matrices and vectors.
Definition: vpArray2D.h:70
unsigned int getCols() const
Definition: vpArray2D.h:146
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyxVector.h:158
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:74
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)
static double sqr(double x)
Definition: vpMath.h:108
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:156
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:76
vpTranslationVector operator*(const vpTranslationVector &tv) const
vpMatrix t() const
Definition: vpMatrix.cpp:375
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:92
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:156
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:1932
vpThetaUVector getThetaUVector()
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:154
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:78