Visual Servoing Platform version 3.5.0
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)
121vpRotationMatrix& 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
398bool 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
443vpRotationMatrix::vpRotationMatrix() : vpArray2D<double>(3, 3), m_index(0) { eye(); }
444
449vpRotationMatrix::vpRotationMatrix(const vpRotationMatrix &M) : vpArray2D<double>(3, 3), m_index(0) { (*this) = M; }
450
455
460vpRotationMatrix::vpRotationMatrix(const vpThetaUVector &tu) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(tu); }
461
465vpRotationMatrix::vpRotationMatrix(const vpPoseVector &p) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(p); }
466
471vpRotationMatrix::vpRotationMatrix(const vpRzyzVector &euler) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(euler); }
472
477vpRotationMatrix::vpRotationMatrix(const vpRxyzVector &Rxyz) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(Rxyz); }
478
483vpRotationMatrix::vpRotationMatrix(const vpRzyxVector &Rzyx) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(Rzyx); }
484
488vpRotationMatrix::vpRotationMatrix(const vpMatrix &R) : vpArray2D<double>(3, 3), m_index(0) { *this = R; }
489
494vpRotationMatrix::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)
528vpRotationMatrix::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{
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
758vpRotationMatrix 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
791vpRotationMatrix 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{
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
862vpRotationMatrix 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
899vpRotationMatrix 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 generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:132
unsigned int getCols() const
Definition: vpArray2D.h:279
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:291
unsigned int getRows() const
Definition: vpArray2D.h:289
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:210
static double sqr(double x)
Definition: vpMath.h:116
static double mcosc(double cosx, double x)
Definition: vpMath.cpp:177
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vpMatrix t() const
Definition: vpMatrix.cpp:464
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
Implementation of a rotation vector as quaternion angle minimal representation.
double w() const
Returns w-component of the quaternion.
double y() const
Returns y-component of the quaternion.
double z() const
Returns z-component of the quaternion.
double x() const
Returns x-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.