Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
92#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
118vpRotationMatrix &vpRotationMatrix::operator=(const std::initializer_list<double> &list)
119{
120 if (dsize != static_cast<unsigned int>(list.size())) {
122 "Cannot set a 3-by-3 rotation matrix from a %d-elements list of doubles."));
123 }
124
125 std::copy(list.begin(), list.end(), data);
126
127 if (!isARotationMatrix()) {
128 if (isARotationMatrix(1e-3)) {
130 }
131 else {
132 throw(vpException(
134 "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
135 }
136 }
137
138 return *this;
139}
140#endif
141
159{
160 if ((M.getCols() != 3) && (M.getRows() != 3)) {
161 throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
162 M.getRows(), M.getCols()));
163 }
164
165 for (unsigned int i = 0; i < 3; i++) {
166 for (unsigned int j = 0; j < 3; j++) {
167 (*this)[i][j] = M[i][j];
168 }
169 }
170
171 if (isARotationMatrix() == false) {
172 throw(vpException(vpException::fatalError, "Cannot set a rotation matrix "
173 "from a matrix that is not a "
174 "rotation matrix"));
175 }
176
177 return *this;
178}
179
208{
209 m_index = 0;
210 data[m_index] = val;
211 return *this;
212}
213
242{
243 m_index++;
244 if (m_index >= size()) {
246 "Cannot set rotation matrix out of bounds. It has only %d elements while you try to initialize "
247 "with %d elements",
248 size(), m_index + 1));
249 }
250 data[m_index] = val;
251 return *this;
252}
253
258{
260
261 for (unsigned int i = 0; i < 3; i++) {
262 for (unsigned int j = 0; j < 3; j++) {
263 double s = 0;
264 for (unsigned int k = 0; k < 3; k++)
265 s += rowPtrs[i][k] * R.rowPtrs[k][j];
266 p[i][j] = s;
267 }
268 }
269 return p;
270}
271
290{
291 if (M.getRows() != 3 || M.getCols() != 3) {
292 throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
293 M.getRows(), M.getCols()));
294 }
295 vpMatrix p(3, 3);
296
297 for (unsigned int i = 0; i < 3; i++) {
298 for (unsigned int j = 0; j < 3; j++) {
299 double s = 0;
300 for (unsigned int k = 0; k < 3; k++)
301 s += (*this)[i][k] * M[k][j];
302 p[i][j] = s;
303 }
304 }
305 return p;
306}
307
326
357{
358 if (v.getRows() != 3) {
360 "Cannot multiply a (3x3) rotation matrix by a %d "
361 "dimension column vector",
362 v.getRows()));
363 }
364 vpColVector v_out(3);
365
366 for (unsigned int j = 0; j < colNum; j++) {
367 double vj = v[j]; // optimization em 5/12/2006
368 for (unsigned int i = 0; i < rowNum; i++) {
369 v_out[i] += rowPtrs[i][j] * vj;
370 }
371 }
372
373 return v_out;
374}
375
381{
383
384 for (unsigned int j = 0; j < 3; j++)
385 p[j] = 0;
386
387 for (unsigned int j = 0; j < 3; j++) {
388 for (unsigned int i = 0; i < 3; i++) {
389 p[i] += rowPtrs[i][j] * tv[j];
390 }
391 }
392
393 return p;
394}
395
401{
403
404 for (unsigned int i = 0; i < rowNum; i++)
405 for (unsigned int j = 0; j < colNum; j++)
406 R[i][j] = rowPtrs[i][j] * x;
407
408 return R;
409}
410
416{
417 for (unsigned int i = 0; i < rowNum; i++)
418 for (unsigned int j = 0; j < colNum; j++)
419 rowPtrs[i][j] *= x;
420
421 return *this;
422}
423
424/*********************************************************************/
425
432bool vpRotationMatrix::isARotationMatrix(double threshold) const
433{
434 bool isRotation = true;
435
436 if (getCols() != 3 || getRows() != 3) {
437 return false;
438 }
439
440 // test R^TR = Id ;
441 vpRotationMatrix RtR = (*this).t() * (*this);
442 for (unsigned int i = 0; i < 3; i++) {
443 for (unsigned int j = 0; j < 3; j++) {
444 if (i == j) {
445 if (fabs(RtR[i][j] - 1) > threshold) {
446 isRotation = false;
447 }
448 }
449 else {
450 if (fabs(RtR[i][j]) > threshold) {
451 isRotation = false;
452 }
453 }
454 }
455 }
456 // test if it is a basis
457 // test || Ci || = 1
458 for (unsigned int i = 0; i < 3; i++) {
459 if ((sqrt(vpMath::sqr(RtR[0][i]) + vpMath::sqr(RtR[1][i]) + vpMath::sqr(RtR[2][i])) - 1) > threshold) {
460 isRotation = false;
461 }
462 }
463
464 // test || Ri || = 1
465 for (unsigned int i = 0; i < 3; i++) {
466 if ((sqrt(vpMath::sqr(RtR[i][0]) + vpMath::sqr(RtR[i][1]) + vpMath::sqr(RtR[i][2])) - 1) > threshold) {
467 isRotation = false;
468 }
469 }
470
471 // test if the basis is orthogonal
472 return isRotation;
473}
474
478vpRotationMatrix::vpRotationMatrix() : vpArray2D<double>(3, 3), m_index(0) { eye(); }
479
484vpRotationMatrix::vpRotationMatrix(const vpRotationMatrix &M) : vpArray2D<double>(3, 3), m_index(0) { (*this) = M; }
485
490
495vpRotationMatrix::vpRotationMatrix(const vpThetaUVector &tu) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(tu); }
496
500vpRotationMatrix::vpRotationMatrix(const vpPoseVector &p) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(p); }
501
506vpRotationMatrix::vpRotationMatrix(const vpRzyzVector &euler) : vpArray2D<double>(3, 3), m_index(0)
507{
508 buildFrom(euler);
509}
510
515vpRotationMatrix::vpRotationMatrix(const vpRxyzVector &Rxyz) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(Rxyz); }
516
521vpRotationMatrix::vpRotationMatrix(const vpRzyxVector &Rzyx) : vpArray2D<double>(3, 3), m_index(0) { buildFrom(Rzyx); }
522
526vpRotationMatrix::vpRotationMatrix(const vpMatrix &R) : vpArray2D<double>(3, 3), m_index(0) { *this = R; }
527
532vpRotationMatrix::vpRotationMatrix(double tux, double tuy, double tuz) : vpArray2D<double>(3, 3), m_index(0)
533{
534 buildFrom(tux, tuy, tuz);
535}
536
541
542#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
566vpRotationMatrix::vpRotationMatrix(const std::initializer_list<double> &list)
567 : vpArray2D<double>(3, 3, list), m_index(0)
568{
569 if (!isARotationMatrix()) {
570 if (isARotationMatrix(1e-3)) {
572 }
573 else {
574 throw(vpException(
576 "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
577 }
578 }
579}
580#endif
581
589{
591
592 for (unsigned int i = 0; i < 3; i++)
593 for (unsigned int j = 0; j < 3; j++)
594 Rt[j][i] = (*this)[i][j];
595
596 return Rt;
597}
598
606{
607 vpRotationMatrix Ri = (*this).t();
608
609 return Ri;
610}
611
630
636{
637 vpThetaUVector tu(*this);
638
639 for (unsigned int i = 0; i < 3; i++)
640 std::cout << tu[i] << " ";
641
642 std::cout << std::endl;
643}
644
655{
656 double theta, si, co, sinc, mcosc;
658
659 theta = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
660 si = sin(theta);
661 co = cos(theta);
662 sinc = vpMath::sinc(si, theta);
663 mcosc = vpMath::mcosc(co, theta);
664
665 R[0][0] = co + mcosc * v[0] * v[0];
666 R[0][1] = -sinc * v[2] + mcosc * v[0] * v[1];
667 R[0][2] = sinc * v[1] + mcosc * v[0] * v[2];
668 R[1][0] = sinc * v[2] + mcosc * v[1] * v[0];
669 R[1][1] = co + mcosc * v[1] * v[1];
670 R[1][2] = -sinc * v[0] + mcosc * v[1] * v[2];
671 R[2][0] = -sinc * v[1] + mcosc * v[2] * v[0];
672 R[2][1] = sinc * v[0] + mcosc * v[2] * v[1];
673 R[2][2] = co + mcosc * v[2] * v[2];
674
675 for (unsigned int i = 0; i < 3; i++)
676 for (unsigned int j = 0; j < 3; j++)
677 (*this)[i][j] = R[i][j];
678
679 return *this;
680}
681
686{
687 for (unsigned int i = 0; i < 3; i++)
688 for (unsigned int j = 0; j < 3; j++)
689 (*this)[i][j] = M[i][j];
690
691 return *this;
692}
693
704
713{
714 double c0, c1, c2, s0, s1, s2;
715
716 c0 = cos(v[0]);
717 c1 = cos(v[1]);
718 c2 = cos(v[2]);
719 s0 = sin(v[0]);
720 s1 = sin(v[1]);
721 s2 = sin(v[2]);
722
723 (*this)[0][0] = c0 * c1 * c2 - s0 * s2;
724 (*this)[0][1] = -c0 * c1 * s2 - s0 * c2;
725 (*this)[0][2] = c0 * s1;
726 (*this)[1][0] = s0 * c1 * c2 + c0 * s2;
727 (*this)[1][1] = -s0 * c1 * s2 + c0 * c2;
728 (*this)[1][2] = s0 * s1;
729 (*this)[2][0] = -s1 * c2;
730 (*this)[2][1] = s1 * s2;
731 (*this)[2][2] = c1;
732
733 return (*this);
734}
735
745{
746 double c0, c1, c2, s0, s1, s2;
747
748 c0 = cos(v[0]);
749 c1 = cos(v[1]);
750 c2 = cos(v[2]);
751 s0 = sin(v[0]);
752 s1 = sin(v[1]);
753 s2 = sin(v[2]);
754
755 (*this)[0][0] = c1 * c2;
756 (*this)[0][1] = -c1 * s2;
757 (*this)[0][2] = s1;
758 (*this)[1][0] = c0 * s2 + s0 * s1 * c2;
759 (*this)[1][1] = c0 * c2 - s0 * s1 * s2;
760 (*this)[1][2] = -s0 * c1;
761 (*this)[2][0] = -c0 * s1 * c2 + s0 * s2;
762 (*this)[2][1] = c0 * s1 * s2 + c2 * s0;
763 (*this)[2][2] = c0 * c1;
764
765 return (*this);
766}
767
775{
776 double c0, c1, c2, s0, s1, s2;
777
778 c0 = cos(v[0]);
779 c1 = cos(v[1]);
780 c2 = cos(v[2]);
781 s0 = sin(v[0]);
782 s1 = sin(v[1]);
783 s2 = sin(v[2]);
784
785 (*this)[0][0] = c0 * c1;
786 (*this)[0][1] = c0 * s1 * s2 - s0 * c2;
787 (*this)[0][2] = c0 * s1 * c2 + s0 * s2;
788
789 (*this)[1][0] = s0 * c1;
790 (*this)[1][1] = s0 * s1 * s2 + c0 * c2;
791 (*this)[1][2] = s0 * s1 * c2 - c0 * s2;
792
793 (*this)[2][0] = -s1;
794 (*this)[2][1] = c1 * s2;
795 (*this)[2][2] = c1 * c2;
796
797 return (*this);
798}
799
804vpRotationMatrix vpRotationMatrix::buildFrom(double tux, double tuy, double tuz)
805{
806 vpThetaUVector tu(tux, tuy, tuz);
807 buildFrom(tu);
808 return *this;
809}
810
815{
816 double a = q.w();
817 double b = q.x();
818 double c = q.y();
819 double d = q.z();
820 (*this)[0][0] = a * a + b * b - c * c - d * d;
821 (*this)[0][1] = 2 * b * c - 2 * a * d;
822 (*this)[0][2] = 2 * a * c + 2 * b * d;
823
824 (*this)[1][0] = 2 * a * d + 2 * b * c;
825 (*this)[1][1] = a * a - b * b + c * c - d * d;
826 (*this)[1][2] = 2 * c * d - 2 * a * b;
827
828 (*this)[2][0] = 2 * b * d - 2 * a * c;
829 (*this)[2][1] = 2 * a * b + 2 * c * d;
830 (*this)[2][2] = a * a - b * b - c * c + d * d;
831 return *this;
832}
833
837vpRotationMatrix operator*(const double &x, const vpRotationMatrix &R)
838{
840
841 unsigned int Rrow = R.getRows();
842 unsigned int Rcol = R.getCols();
843
844 for (unsigned int i = 0; i < Rrow; i++)
845 for (unsigned int j = 0; j < Rcol; j++)
846 C[i][j] = R[i][j] * x;
847
848 return C;
849}
850
856{
858 tu.buildFrom(*this);
859 return tu;
860}
861
890{
891 if (j >= getCols())
892 throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the homogeneous matrix"));
893 unsigned int nb_rows = getRows();
894 vpColVector c(nb_rows);
895 for (unsigned int i = 0; i < nb_rows; i++)
896 c[i] = (*this)[i][j];
897 return c;
898}
899
909vpRotationMatrix vpRotationMatrix::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
910{
911 vpMatrix meanR(3, 3);
913 for (size_t i = 0; i < vec_M.size(); i++) {
914 R = vec_M[i].getRotationMatrix();
915 meanR += (vpMatrix)R;
916 }
917 meanR /= static_cast<double>(vec_M.size());
918
919 // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
920 vpMatrix M, U, V;
921 vpColVector sv;
922 meanR.pseudoInverse(M, sv, 1e-6, U, V);
923 double det = sv[0] * sv[1] * sv[2];
924 if (det > 0) {
925 meanR = U * V.t();
926 }
927 else {
928 vpMatrix D(3, 3);
929 D = 0.0;
930 D[0][0] = D[1][1] = 1.0;
931 D[2][2] = -1;
932 meanR = U * D * V.t();
933 }
934
935 R = meanR;
936 return R;
937}
938
947vpRotationMatrix vpRotationMatrix::mean(const std::vector<vpRotationMatrix> &vec_R)
948{
949 vpMatrix meanR(3, 3);
951 for (size_t i = 0; i < vec_R.size(); i++) {
952 meanR += (vpMatrix)vec_R[i];
953 }
954 meanR /= static_cast<double>(vec_R.size());
955
956 // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
957 vpMatrix M, U, V;
958 vpColVector sv;
959 meanR.pseudoInverse(M, sv, 1e-6, U, V);
960 double det = sv[0] * sv[1] * sv[2];
961 if (det > 0) {
962 meanR = U * V.t();
963 }
964 else {
965 vpMatrix D(3, 3);
966 D = 0.0;
967 D[0][0] = D[1][1] = 1.0;
968 D[2][2] = -1;
969 meanR = U * D * V.t();
970 }
971
972 R = meanR;
973 return R;
974}
975
980{
981 vpMatrix U = *this;
982 vpColVector w;
983 vpMatrix V;
984 U.svd(w, V);
985 vpMatrix Vt = V.t();
986 vpMatrix R = U * Vt;
987
988 double det = R.det();
989 if (det < 0) {
990 Vt[2][0] *= -1;
991 Vt[2][1] *= -1;
992 Vt[2][2] *= -1;
993
994 R = U * Vt;
995 }
996
997 data[0] = R[0][0];
998 data[1] = R[0][1];
999 data[2] = R[0][2];
1000 data[3] = R[1][0];
1001 data[4] = R[1][1];
1002 data[5] = R[1][2];
1003 data[6] = R[2][0];
1004 data[7] = R[2][1];
1005 data[8] = R[2][2];
1006}
1007
1008#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1009
1018
1019#endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition vpArray2D.h:131
unsigned int getCols() const
Definition vpArray2D.h:280
double * data
Address of the first element of the data array.
Definition vpArray2D.h:144
double ** rowPtrs
Address of the first element of each rows.
Definition vpArray2D.h:138
unsigned int rowNum
Number of rows in the array.
Definition vpArray2D.h:134
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< double > &A)
Definition vpArray2D.h:529
unsigned int dsize
Current array size (rowNum * colNum)
Definition vpArray2D.h:140
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
unsigned int getRows() const
Definition vpArray2D.h:290
unsigned int colNum
Number of columns in the array.
Definition vpArray2D.h:136
Implementation of column vector and the associated operations.
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:124
static double mcosc(double cosx, double x)
Definition vpMath.cpp:233
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void svd(vpColVector &w, vpMatrix &V)
vpMatrix t() const
Definition vpMatrix.cpp:461
vpMatrix pseudoInverse(double svThreshold=1e-6) const
double det(vpDetMethod method=LU_DECOMPOSITION) const
Implementation of a pose vector and operations on poses.
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)
vpColVector getCol(unsigned int j) const
vpThetaUVector getThetaUVector()
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.
Implementation of a rotation vector as Euler angle minimal representation.
Implementation of a rotation vector as Euler angle minimal representation.
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.
vpColVector operator*(const double &x, const vpColVector &v)