40 #include <visp3/core/vpMath.h>
41 #include <visp3/core/vpMatrix.h>
44 #include <visp3/core/vpRotationMatrix.h>
47 #include <visp3/core/vpException.h>
60 const unsigned int val_3 = 3;
61 for (
unsigned int i = 0; i < val_3; ++i) {
62 for (
unsigned int j = 0; j < val_3; ++j) {
84 const unsigned int val_3 = 3;
85 for (
unsigned int i = 0; i < val_3; ++i) {
86 for (
unsigned int j = 0; j < val_3; ++j) {
94 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
124 if (
dsize !=
static_cast<unsigned int>(list.size())) {
126 "Cannot set a 3-by-3 rotation matrix from a %d-elements list of doubles."));
129 std::copy(list.begin(), list.end(),
data);
138 "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
169 const unsigned int val_3 = 3;
170 for (
unsigned int i = 0; i < val_3; ++i) {
171 for (
unsigned int j = 0; j < val_3; ++j) {
172 (*this)[i][j] = M[i][j];
178 "from a matrix that is not a "
259 "Cannot set rotation matrix out of bounds. It has only %d elements while you try to initialize "
274 const unsigned int val_3 = 3;
275 for (
unsigned int i = 0; i < val_3; ++i) {
276 for (
unsigned int j = 0; j < val_3; ++j) {
278 for (
unsigned int k = 0; k < 3; ++k) {
312 const unsigned int val_3 = 3;
313 for (
unsigned int i = 0; i < val_3; ++i) {
314 for (
unsigned int j = 0; j < val_3; ++j) {
316 for (
unsigned int k = 0; k < 3; ++k) {
317 s += (*this)[i][k] * M[k][j];
379 const unsigned int rows_size = 3;
380 if (v.
getRows() != rows_size) {
382 "Cannot multiply a (3x3) rotation matrix by a %d "
383 "dimension column vector",
388 for (
unsigned int j = 0; j <
colNum; ++j) {
390 for (
unsigned int i = 0; i <
rowNum; ++i) {
391 v_out[i] +=
rowPtrs[i][j] * vj;
405 const unsigned int val_3 = 3;
407 for (
unsigned int j = 0; j < val_3; ++j) {
411 for (
unsigned int j = 0; j < val_3; ++j) {
412 for (
unsigned int i = 0; i < val_3; ++i) {
428 for (
unsigned int i = 0; i <
rowNum; ++i) {
429 for (
unsigned int j = 0; j <
colNum; ++j) {
443 for (
unsigned int i = 0; i <
rowNum; ++i) {
444 for (
unsigned int j = 0; j <
colNum; ++j) {
462 bool isRotation =
true;
470 const unsigned int val_3 = 3;
471 for (
unsigned int i = 0; i < val_3; ++i) {
472 for (
unsigned int j = 0; j < val_3; ++j) {
474 if (fabs(RtR[i][j] - 1) > threshold) {
479 if (fabs(RtR[i][j]) > threshold) {
487 const unsigned int index_2 = 2;
488 for (
unsigned int i = 0; i < val_3; ++i) {
495 for (
unsigned int i = 0; i < val_3; ++i) {
564 build(tux, tuy, tuz);
572 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
599 :
vpArray2D<double>(3, 3, list), m_index(0)
608 "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
624 const unsigned int val_3 = 3;
625 for (
unsigned int i = 0; i < val_3; ++i) {
626 for (
unsigned int j = 0; j < val_3; ++j) {
627 Rt[j][i] = (*this)[i][j];
678 const unsigned int val_3 = 3;
679 for (
unsigned int i = 0; i < val_3; ++i) {
680 std::cout << tu[i] <<
" ";
683 std::cout << std::endl;
686 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
772 build(tux, tuy, tuz);
798 double theta, si, co, sinc, mcosc;
801 const unsigned int index_0 = 0;
802 const unsigned int index_1 = 1;
803 const unsigned int index_2 = 2;
804 theta = sqrt((v[index_0] * v[index_0]) + (v[index_1] * v[index_1]) + (v[index_2] * v[index_2]));
810 R[index_0][index_0] = co + (mcosc * v[index_0] * v[index_0]);
811 R[index_0][index_1] = (-sinc * v[index_2]) + (mcosc * v[index_0] * v[index_1]);
812 R[index_0][index_2] = (sinc * v[index_1]) + (mcosc * v[index_0] * v[index_2]);
813 R[index_1][index_0] = (sinc * v[index_2]) + (mcosc * v[index_1] * v[index_0]);
814 R[index_1][index_1] = co + (mcosc * v[index_1] * v[index_1]);
815 R[index_1][index_2] = (-sinc * v[index_0]) + (mcosc * v[index_1] * v[index_2]);
816 R[index_2][index_0] = (-sinc * v[index_1]) + (mcosc * v[index_2] * v[index_0]);
817 R[index_2][index_1] = (sinc * v[index_0]) + (mcosc * v[index_2] * v[index_1]);
818 R[index_2][index_2] = co + (mcosc * v[index_2] * v[index_2]);
820 const unsigned int val_3 = 3;
821 for (
unsigned int i = 0; i < val_3; ++i) {
822 for (
unsigned int j = 0; j < val_3; ++j) {
823 (*this)[i][j] = R[i][j];
835 const unsigned int val_3 = 3;
836 for (
unsigned int i = 0; i < val_3; ++i) {
837 for (
unsigned int j = 0; j < val_3; ++j) {
838 (*this)[i][j] = M[i][j];
865 double c0, c1, c2, s0, s1, s2;
866 const unsigned int index_0 = 0;
867 const unsigned int index_1 = 1;
868 const unsigned int index_2 = 2;
870 c0 = cos(v[index_0]);
871 c1 = cos(v[index_1]);
872 c2 = cos(v[index_2]);
873 s0 = sin(v[index_0]);
874 s1 = sin(v[index_1]);
875 s2 = sin(v[index_2]);
877 (*this)[index_0][index_0] = (c0 * c1 * c2) - (s0 * s2);
878 (*this)[index_0][index_1] = (-c0 * c1 * s2) - (s0 * c2);
879 (*this)[index_0][index_2] = c0 * s1;
880 (*this)[index_1][index_0] = (s0 * c1 * c2) + (c0 * s2);
881 (*this)[index_1][index_1] = (-s0 * c1 * s2) + (c0 * c2);
882 (*this)[index_1][index_2] = s0 * s1;
883 (*this)[index_2][index_0] = -s1 * c2;
884 (*this)[index_2][index_1] = s1 * s2;
885 (*this)[index_2][index_2] = c1;
900 double c0, c1, c2, s0, s1, s2;
901 const unsigned int index_0 = 0;
902 const unsigned int index_1 = 1;
903 const unsigned int index_2 = 2;
905 c0 = cos(v[index_0]);
906 c1 = cos(v[index_1]);
907 c2 = cos(v[index_2]);
908 s0 = sin(v[index_0]);
909 s1 = sin(v[index_1]);
910 s2 = sin(v[index_2]);
912 (*this)[index_0][index_0] = c1 * c2;
913 (*this)[index_0][index_1] = -c1 * s2;
914 (*this)[index_0][index_2] = s1;
915 (*this)[index_1][index_0] = (c0 * s2) + (s0 * s1 * c2);
916 (*this)[index_1][index_1] = (c0 * c2) - (s0 * s1 * s2);
917 (*this)[index_1][index_2] = -s0 * c1;
918 (*this)[index_2][index_0] = (-c0 * s1 * c2) + (s0 * s2);
919 (*this)[index_2][index_1] = (c0 * s1 * s2) + (c2 * s0);
920 (*this)[index_2][index_2] = c0 * c1;
933 double c0, c1, c2, s0, s1, s2;
934 const unsigned int index_0 = 0;
935 const unsigned int index_1 = 1;
936 const unsigned int index_2 = 2;
938 c0 = cos(v[index_0]);
939 c1 = cos(v[index_1]);
940 c2 = cos(v[index_2]);
941 s0 = sin(v[index_0]);
942 s1 = sin(v[index_1]);
943 s2 = sin(v[index_2]);
945 (*this)[index_0][index_0] = c0 * c1;
946 (*this)[index_0][index_1] = (c0 * s1 * s2) - (s0 * c2);
947 (*this)[index_0][index_2] = (c0 * s1 * c2) + (s0 * s2);
949 (*this)[index_1][index_0] = s0 * c1;
950 (*this)[index_1][index_1] = (s0 * s1 * s2) + (c0 * c2);
951 (*this)[index_1][index_2] = (s0 * s1 * c2) - (c0 * s2);
953 (*this)[index_2][index_0] = -s1;
954 (*this)[index_2][index_1] = c1 * s2;
955 (*this)[index_2][index_2] = c1 * c2;
980 const unsigned int index_0 = 0;
981 const unsigned int index_1 = 1;
982 const unsigned int index_2 = 2;
983 (*this)[index_0][index_0] = (((a * a) + (b * b)) - (c * c)) - (d * d);
984 (*this)[index_0][index_1] = (2 * b * c) - (2 * a * d);
985 (*this)[index_0][index_2] = (2 * a * c) + (2 * b * d);
987 (*this)[index_1][index_0] = (2 * a * d) + (2 * b * c);
988 (*this)[index_1][index_1] = (((a * a) - (b * b)) + (c * c)) - (d * d);
989 (*this)[index_1][index_2] = (2 * c * d) - (2 * a * b);
991 (*this)[index_2][index_0] = (2 * b * d) - (2 * a * c);
992 (*this)[index_2][index_1] = (2 * a * b) + (2 * c * d);
993 (*this)[index_2][index_2] = ((a * a) - (b * b) - (c * c)) + (d * d);
1044 unsigned int nb_rows =
getRows();
1046 for (
unsigned int i = 0; i < nb_rows; ++i) {
1047 c[i] = (*this)[i][j];
1065 size_t vec_m_size = vec_M.
size();
1066 for (
size_t i = 0; i < vec_m_size; ++i) {
1067 R = vec_M[i].getRotationMatrix();
1070 meanR /=
static_cast<double>(vec_M.size());
1076 const unsigned int index_0 = 0;
1077 const unsigned int index_1 = 1;
1078 const unsigned int index_2 = 2;
1079 double det = sv[index_0] * sv[index_1] * sv[index_2];
1086 D[index_0][index_0] = 1.0;
1087 D[index_1][index_1] = 1.0;
1088 D[index_2][index_2] = -1;
1089 meanR = U * D * V.
t();
1108 size_t vec_r_size = vec_R.
size();
1109 for (
size_t i = 0; i < vec_r_size; ++i) {
1112 meanR /=
static_cast<double>(vec_R.size());
1118 const unsigned int index_0 = 0;
1119 const unsigned int index_1 = 1;
1120 const unsigned int index_2 = 2;
1121 double det = sv[index_0] * sv[index_1] * sv[index_2];
1128 D[index_0][index_0] = 1.0;
1129 D[index_1][index_1] = 1.0;
1130 D[index_2][index_2] = -1;
1131 meanR = U * D * V.
t();
1149 const unsigned int index_0 = 0;
1150 const unsigned int index_1 = 1;
1151 const unsigned int index_2 = 2;
1152 const unsigned int index_3 = 3;
1153 const unsigned int index_4 = 4;
1154 const unsigned int index_5 = 5;
1155 const unsigned int index_6 = 6;
1156 const unsigned int index_7 = 7;
1157 const unsigned int index_8 = 8;
1159 double det = R.
det();
1161 Vt[index_2][index_0] *= -1;
1162 Vt[index_2][index_1] *= -1;
1163 Vt[index_2][index_2] *= -1;
1168 data[index_0] = R[index_0][index_0];
1169 data[index_1] = R[index_0][index_1];
1170 data[index_2] = R[index_0][index_2];
1171 data[index_3] = R[index_1][index_0];
1172 data[index_4] = R[index_1][index_1];
1173 data[index_5] = R[index_1][index_2];
1174 data[index_6] = R[index_2][index_0];
1175 data[index_7] = R[index_2][index_1];
1176 data[index_8] = R[index_2][index_2];
1179 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1199 unsigned int Rrow = R.
getRows();
1200 unsigned int Rcol = R.
getCols();
1202 for (
unsigned int i = 0; i < Rrow; ++i) {
1203 for (
unsigned int j = 0; j < Rcol; ++j) {
1204 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.
VP_DEPRECATED void setIdentity()
vpRotationMatrix & build(const vpHomogeneousMatrix &M)
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 t() const
VP_DEPRECATED 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 & build(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.