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());
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());
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) Implementation of a matrix and operations on matrices.
vpRotationMatrix inverse() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
error that can be emited by ViSP classes.
double y() const
Returns y-component of the quaternion.
vpColVector getCol(const unsigned int j) const
vpRotationMatrix t() const
Implementation of a generic 2D array used as vase class of matrices and vectors.
unsigned int getCols() const
Implementation of a rotation vector as Euler angle minimal representation.
vp_deprecated void setIdentity()
vpRotationMatrix & operator=(const vpRotationMatrix &R)
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
static double sinc(double x)
bool isARotationMatrix() const
Implementation of a rotation matrix and operations on such kind of matrices.
double w() const
Returns w-component of the quaternion.
unsigned int rowNum
Number of rows in the array.
vpRotationMatrix & operator*=(const double x)
static vpRotationMatrix mean(const std::vector< vpHomogeneousMatrix > &vec_M)
static double mcosc(double cosx, double x)
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
static double sqr(double x)
double z() const
Returns z-component of the quaternion.
double x() const
Returns x-component of the quaternion.
Implementation of a rotation vector as quaternion angle minimal representation.
unsigned int getRows() const
unsigned int colNum
Number of columns in the array.
vpTranslationVector operator*(const vpTranslationVector &tv) const
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
Implementation of a rotation vector as Euler angle minimal representation.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
vpThetaUVector getThetaUVector()
Implementation of a rotation vector as Euler angle minimal representation.
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
double ** rowPtrs
Address of the first element of each rows.