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