45 #include <visp3/core/vpMath.h>
46 #include <visp3/core/vpMatrix.h>
49 #include <visp3/core/vpRotationMatrix.h>
52 #include <visp3/core/vpException.h>
56 #include <visp3/core/vpDebug.h>
57 const double vpRotationMatrix::threshold = 1e-6;
66 for (
unsigned int i = 0; i < 3; i++) {
67 for (
unsigned int j = 0; j < 3; j++) {
87 for (
unsigned int i = 0; i < 3; i++) {
88 for (
unsigned int j = 0; j < 3; j++) {
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];
127 "from a matrix that is not a "
141 for (
unsigned int i = 0; i < 3; i++) {
142 for (
unsigned int j = 0; j < 3; j++) {
144 for (
unsigned int k = 0; k < 3; k++)
173 for (
unsigned int i = 0; i < 3; i++) {
174 for (
unsigned int j = 0; j < 3; j++) {
176 for (
unsigned int k = 0; k < 3; k++)
177 s += (*
this)[i][k] * M[k][j];
217 "Cannot multiply a (3x3) rotation matrix by a %d "
218 "dimension column vector",
223 for (
unsigned int j = 0; j <
colNum; j++) {
225 for (
unsigned int i = 0; i <
rowNum; i++) {
226 v_out[i] +=
rowPtrs[i][j] * vj;
241 for (
unsigned int j = 0; j < 3; j++)
244 for (
unsigned int j = 0; j < 3; j++) {
245 for (
unsigned int i = 0; i < 3; i++) {
261 for (
unsigned int i = 0; i <
rowNum; i++)
262 for (
unsigned int j = 0; j <
colNum; j++)
274 for (
unsigned int i = 0; i <
rowNum; i++)
275 for (
unsigned int j = 0; j <
colNum; j++)
289 bool isRotation =
true;
293 for (i = 0; i < 3; i++) {
294 for (j = 0; j < 3; j++) {
296 if (fabs(RtR[i][j] - 1) > threshold)
299 if (fabs(RtR[i][j]) > threshold)
306 for (i = 0; i < 3; i++) {
312 for (i = 0; i < 3; i++) {
395 for (i = 0; i < 3; i++)
396 for (j = 0; j < 3; j++)
397 Rt[j][i] = (*
this)[i][j];
442 for (
unsigned int i = 0; i < 3; i++)
443 std::cout << tu[i] <<
" ";
445 std::cout << std::endl;
460 double theta, si, co, sinc, mcosc;
463 theta = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
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];
479 for (i = 0; i < 3; i++)
480 for (j = 0; j < 3; j++)
481 (*
this)[i][j] = R[i][j];
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];
518 double c0, c1, c2, s0, s1, s2;
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;
550 double c0, c1, c2, s0, s1, s2;
559 (*this)[0][0] = c1 * c2;
560 (*this)[0][1] = -c1 * s2;
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;
580 double c0, c1, c2, s0, s1, s2;
589 (*this)[0][0] = c0 * c1;
590 (*this)[0][1] = c0 * s1 * s2 - s0 * c2;
591 (*this)[0][2] = c0 * s1 * c2 + s0 * s2;
593 (*this)[1][0] = s0 * c1;
594 (*this)[1][1] = s0 * s1 * s2 + c0 * c2;
595 (*this)[1][2] = s0 * s1 * c2 - c0 * s2;
598 (*this)[2][1] = c1 * s2;
599 (*this)[2][2] = c1 * c2;
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;
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;
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;
645 unsigned int Rrow = R.
getRows();
646 unsigned int Rcol = R.
getCols();
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;
697 unsigned int nb_rows =
getRows();
699 for (
unsigned int i = 0; i < nb_rows; i++)
700 c[i] = (*
this)[i][j];
716 for (
size_t i = 0; i < vec_M.size(); i++) {
717 R = vec_M[i].getRotationMatrix();
720 meanR /= static_cast<double>(vec_M.size());
725 meanR.pseudoInverse(M, sv, 1e-6, U, V);
726 double det = sv[0]*sv[1]*sv[2];
733 D[0][0] = D[1][1] = 1.0; D[2][2] = -1;
734 meanR = U * D * V.t();
753 for (
size_t i = 0; i < vec_R.size(); i++) {
756 meanR /= static_cast<double>(vec_R.size());
761 meanR.pseudoInverse(M, sv, 1e-6, U, V);
762 double det = sv[0]*sv[1]*sv[2];
769 D[0][0] = D[1][1] = 1.0; D[2][2] = -1;
770 meanR = U * D * V.t();
777 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
788 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)