40 #include <visp3/core/vpMath.h>
41 #include <visp3/core/vpMatrix.h>
44 #include <visp3/core/vpRotationMatrix.h>
47 #include <visp3/core/vpException.h>
53 const unsigned int vpRotationMatrix::constr_val_3 = 3;
61 const unsigned int val_3 = 3;
62 for (
unsigned int i = 0; i < val_3; ++i) {
63 for (
unsigned int j = 0; j < val_3; ++j) {
85 const unsigned int val_3 = 3;
86 for (
unsigned int i = 0; i < val_3; ++i) {
87 for (
unsigned int j = 0; j < val_3; ++j) {
95 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
125 if (
dsize !=
static_cast<unsigned int>(list.size())) {
127 "Cannot set a 3-by-3 rotation matrix from a %d-elements list of doubles."));
130 std::copy(list.begin(), list.end(),
data);
139 "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
165 const unsigned int val_3 = 3;
171 for (
unsigned int i = 0; i < val_3; ++i) {
172 for (
unsigned int j = 0; j < val_3; ++j) {
173 (*this)[i][j] = M[i][j];
179 "from a matrix that is not a "
260 "Cannot set rotation matrix out of bounds. It has only %d elements while you try to initialize "
275 const unsigned int val_3 = 3;
276 for (
unsigned int i = 0; i < val_3; ++i) {
277 for (
unsigned int j = 0; j < val_3; ++j) {
279 for (
unsigned int k = 0; k < val_3; ++k) {
307 const unsigned int val_3 = 3;
314 for (
unsigned int i = 0; i < val_3; ++i) {
315 for (
unsigned int j = 0; j < val_3; ++j) {
317 for (
unsigned int k = 0; k < val_3; ++k) {
318 s += (*this)[i][k] * M[k][j];
380 const unsigned int rows_size = 3;
381 if (v.
getRows() != rows_size) {
383 "Cannot multiply a (3x3) rotation matrix by a %d "
384 "dimension column vector",
389 for (
unsigned int j = 0; j <
colNum; ++j) {
391 for (
unsigned int i = 0; i <
rowNum; ++i) {
392 v_out[i] +=
rowPtrs[i][j] * vj;
406 const unsigned int val_3 = 3;
408 for (
unsigned int j = 0; j < val_3; ++j) {
412 for (
unsigned int j = 0; j < val_3; ++j) {
413 for (
unsigned int i = 0; i < val_3; ++i) {
429 for (
unsigned int i = 0; i <
rowNum; ++i) {
430 for (
unsigned int j = 0; j <
colNum; ++j) {
444 for (
unsigned int i = 0; i <
rowNum; ++i) {
445 for (
unsigned int j = 0; j <
colNum; ++j) {
463 bool isRotation =
true;
464 const unsigned int val_3 = 3;
472 for (
unsigned int i = 0; i < val_3; ++i) {
473 for (
unsigned int j = 0; j < val_3; ++j) {
475 if (fabs(RtR[i][j] - 1) > threshold) {
480 if (fabs(RtR[i][j]) > threshold) {
488 const unsigned int index_2 = 2;
489 for (
unsigned int i = 0; i < val_3; ++i) {
496 for (
unsigned int i = 0; i < val_3; ++i) {
573 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
600 :
vpArray2D<double>(3, 3, list), m_index(0)
609 "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
625 const unsigned int val_3 = 3;
626 for (
unsigned int i = 0; i < val_3; ++i) {
627 for (
unsigned int j = 0; j < val_3; ++j) {
628 Rt[j][i] = (*this)[i][j];
679 const unsigned int val_3 = 3;
680 for (
unsigned int i = 0; i < val_3; ++i) {
681 std::cout << tu[i] <<
" ";
684 std::cout << std::endl;
698 double theta, si, co, sinc, mcosc;
701 const unsigned int index_0 = 0;
702 const unsigned int index_1 = 1;
703 const unsigned int index_2 = 2;
704 theta = sqrt((v[index_0] * v[index_0]) + (v[index_1] * v[index_1]) + (v[index_2] * v[index_2]));
710 R[index_0][index_0] = co + (mcosc * v[index_0] * v[index_0]);
711 R[index_0][index_1] = (-sinc * v[index_2]) + (mcosc * v[index_0] * v[index_1]);
712 R[index_0][index_2] = (sinc * v[index_1]) + (mcosc * v[index_0] * v[index_2]);
713 R[index_1][index_0] = (sinc * v[index_2]) + (mcosc * v[index_1] * v[index_0]);
714 R[index_1][index_1] = co + (mcosc * v[index_1] * v[index_1]);
715 R[index_1][index_2] = (-sinc * v[index_0]) + (mcosc * v[index_1] * v[index_2]);
716 R[index_2][index_0] = (-sinc * v[index_1]) + (mcosc * v[index_2] * v[index_0]);
717 R[index_2][index_1] = (sinc * v[index_0]) + (mcosc * v[index_2] * v[index_1]);
718 R[index_2][index_2] = co + (mcosc * v[index_2] * v[index_2]);
720 const unsigned int val_3 = 3;
721 for (
unsigned int i = 0; i < val_3; ++i) {
722 for (
unsigned int j = 0; j < val_3; ++j) {
723 (*this)[i][j] = R[i][j];
735 const unsigned int val_3 = 3;
736 for (
unsigned int i = 0; i < val_3; ++i) {
737 for (
unsigned int j = 0; j < val_3; ++j) {
738 (*this)[i][j] = M[i][j];
765 double c0, c1, c2, s0, s1, s2;
766 const unsigned int index_0 = 0;
767 const unsigned int index_1 = 1;
768 const unsigned int index_2 = 2;
770 c0 = cos(v[index_0]);
771 c1 = cos(v[index_1]);
772 c2 = cos(v[index_2]);
773 s0 = sin(v[index_0]);
774 s1 = sin(v[index_1]);
775 s2 = sin(v[index_2]);
777 (*this)[index_0][index_0] = (c0 * c1 * c2) - (s0 * s2);
778 (*this)[index_0][index_1] = (-c0 * c1 * s2) - (s0 * c2);
779 (*this)[index_0][index_2] = c0 * s1;
780 (*this)[index_1][index_0] = (s0 * c1 * c2) + (c0 * s2);
781 (*this)[index_1][index_1] = (-s0 * c1 * s2) + (c0 * c2);
782 (*this)[index_1][index_2] = s0 * s1;
783 (*this)[index_2][index_0] = -s1 * c2;
784 (*this)[index_2][index_1] = s1 * s2;
785 (*this)[index_2][index_2] = c1;
800 double c0, c1, c2, s0, s1, s2;
801 const unsigned int index_0 = 0;
802 const unsigned int index_1 = 1;
803 const unsigned int index_2 = 2;
805 c0 = cos(v[index_0]);
806 c1 = cos(v[index_1]);
807 c2 = cos(v[index_2]);
808 s0 = sin(v[index_0]);
809 s1 = sin(v[index_1]);
810 s2 = sin(v[index_2]);
812 (*this)[index_0][index_0] = c1 * c2;
813 (*this)[index_0][index_1] = -c1 * s2;
814 (*this)[index_0][index_2] = s1;
815 (*this)[index_1][index_0] = (c0 * s2) + (s0 * s1 * c2);
816 (*this)[index_1][index_1] = (c0 * c2) - (s0 * s1 * s2);
817 (*this)[index_1][index_2] = -s0 * c1;
818 (*this)[index_2][index_0] = (-c0 * s1 * c2) + (s0 * s2);
819 (*this)[index_2][index_1] = (c0 * s1 * s2) + (c2 * s0);
820 (*this)[index_2][index_2] = c0 * c1;
833 double c0, c1, c2, s0, s1, s2;
834 const unsigned int index_0 = 0;
835 const unsigned int index_1 = 1;
836 const unsigned int index_2 = 2;
838 c0 = cos(v[index_0]);
839 c1 = cos(v[index_1]);
840 c2 = cos(v[index_2]);
841 s0 = sin(v[index_0]);
842 s1 = sin(v[index_1]);
843 s2 = sin(v[index_2]);
845 (*this)[index_0][index_0] = c0 * c1;
846 (*this)[index_0][index_1] = (c0 * s1 * s2) - (s0 * c2);
847 (*this)[index_0][index_2] = (c0 * s1 * c2) + (s0 * s2);
849 (*this)[index_1][index_0] = s0 * c1;
850 (*this)[index_1][index_1] = (s0 * s1 * s2) + (c0 * c2);
851 (*this)[index_1][index_2] = (s0 * s1 * c2) - (c0 * s2);
853 (*this)[index_2][index_0] = -s1;
854 (*this)[index_2][index_1] = c1 * s2;
855 (*this)[index_2][index_2] = c1 * c2;
880 const unsigned int index_0 = 0;
881 const unsigned int index_1 = 1;
882 const unsigned int index_2 = 2;
883 (*this)[index_0][index_0] = (((a * a) + (b * b)) - (c * c)) - (d * d);
884 (*this)[index_0][index_1] = (2.0 * b * c) - (2.0 * a * d);
885 (*this)[index_0][index_2] = (2.0 * a * c) + (2.0 * b * d);
887 (*this)[index_1][index_0] = (2.0 * a * d) + (2.0 * b * c);
888 (*this)[index_1][index_1] = (((a * a) - (b * b)) + (c * c)) - (d * d);
889 (*this)[index_1][index_2] = (2.0 * c * d) - (2.0 * a * b);
891 (*this)[index_2][index_0] = (2.0 * b * d) - (2.0 * a * c);
892 (*this)[index_2][index_1] = (2.0 * a * b) + (2.0 * c * d);
893 (*this)[index_2][index_2] = ((a * a) - (b * b) - (c * c)) + (d * d);
944 unsigned int nb_rows =
getRows();
946 for (
unsigned int i = 0; i < nb_rows; ++i) {
947 c[i] = (*this)[i][j];
965 size_t vec_m_size = vec_M.
size();
966 for (
size_t i = 0; i < vec_m_size; ++i) {
967 R = vec_M[i].getRotationMatrix();
970 meanR /=
static_cast<double>(vec_M.size());
976 const unsigned int index_0 = 0;
977 const unsigned int index_1 = 1;
978 const unsigned int index_2 = 2;
979 double det = sv[index_0] * sv[index_1] * sv[index_2];
986 D[index_0][index_0] = 1.0;
987 D[index_1][index_1] = 1.0;
988 D[index_2][index_2] = -1;
989 meanR = U * D * V.
t();
1008 size_t vec_r_size = vec_R.
size();
1009 for (
size_t i = 0; i < vec_r_size; ++i) {
1010 meanR +=
static_cast<vpMatrix>(vec_R[i]);
1012 meanR /=
static_cast<double>(vec_R.size());
1018 const unsigned int index_0 = 0;
1019 const unsigned int index_1 = 1;
1020 const unsigned int index_2 = 2;
1021 double det = sv[index_0] * sv[index_1] * sv[index_2];
1028 D[index_0][index_0] = 1.0;
1029 D[index_1][index_1] = 1.0;
1030 D[index_2][index_2] = -1;
1031 meanR = U * D * V.
t();
1049 const unsigned int index_0 = 0;
1050 const unsigned int index_1 = 1;
1051 const unsigned int index_2 = 2;
1052 const unsigned int index_3 = 3;
1053 const unsigned int index_4 = 4;
1054 const unsigned int index_5 = 5;
1055 const unsigned int index_6 = 6;
1056 const unsigned int index_7 = 7;
1057 const unsigned int index_8 = 8;
1059 double det = R.
det();
1061 Vt[index_2][index_0] *= -1;
1062 Vt[index_2][index_1] *= -1;
1063 Vt[index_2][index_2] *= -1;
1068 data[index_0] = R[index_0][index_0];
1069 data[index_1] = R[index_0][index_1];
1070 data[index_2] = R[index_0][index_2];
1071 data[index_3] = R[index_1][index_0];
1072 data[index_4] = R[index_1][index_1];
1073 data[index_5] = R[index_1][index_2];
1074 data[index_6] = R[index_2][index_0];
1075 data[index_7] = R[index_2][index_1];
1076 data[index_8] = R[index_2][index_2];
1079 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1088 void vpRotationMatrix::setIdentity() {
eye(); }
1099 unsigned int Rrow = R.
getRows();
1100 unsigned int Rcol = R.
getCols();
1102 for (
unsigned int i = 0; i < Rrow; ++i) {
1103 for (
unsigned int j = 0; j < Rcol; ++j) {
1104 C[i][j] = R[i][j] * x;
Implementation of a generic 2D array used as base class for matrices and vectors.
unsigned int getCols() const
double * data
Address of the first element of the data array.
double ** rowPtrs
Address of the first element of each rows.
unsigned int rowNum
Number of rows in the array.
unsigned int dsize
Current array size (rowNum * colNum)
unsigned int size() const
Return the number of elements of the 2D array.
unsigned int getRows() const
unsigned int colNum
Number of columns in the array.
Implementation of column vector and the associated operations.
vpColVector operator*(const double &x, const vpColVector &v)
error that can be emitted by ViSP classes.
@ dimensionError
Bad dimension.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
vpTranslationVector getTranslationVector() const
static double sinc(double x)
static double sqr(double x)
static double mcosc(double cosx, double x)
Implementation of a matrix and operations on matrices.
void svd(vpColVector &w, vpMatrix &V)
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.
vpRotationMatrix & operator*=(double x)
bool isARotationMatrix(double threshold=1e-6) const
vpRotationMatrix & operator,(double val)
vpColVector getCol(unsigned int j) const
vpThetaUVector getThetaUVector()
vpRotationMatrix & operator<<(double val)
vpRotationMatrix & buildFrom(const vpHomogeneousMatrix &M)
vpRotationMatrix t() const
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.