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++) {
96 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) 124 if (
dsize != static_cast<unsigned int>(list.size())) {
128 std::copy(list.begin(), list.end(),
data);
161 for (
unsigned int i = 0; i < 3; i++) {
162 for (
unsigned int j = 0; j < 3; j++) {
163 (*this)[i][j] = M[i][j];
169 "from a matrix that is not a " 254 for (
unsigned int i = 0; i < 3; i++) {
255 for (
unsigned int j = 0; j < 3; j++) {
257 for (
unsigned int k = 0; k < 3; k++)
286 for (
unsigned int i = 0; i < 3; i++) {
287 for (
unsigned int j = 0; j < 3; j++) {
289 for (
unsigned int k = 0; k < 3; k++)
290 s += (*
this)[i][k] * M[k][j];
330 "Cannot multiply a (3x3) rotation matrix by a %d " 331 "dimension column vector",
336 for (
unsigned int j = 0; j <
colNum; j++) {
338 for (
unsigned int i = 0; i <
rowNum; i++) {
339 v_out[i] +=
rowPtrs[i][j] * vj;
354 for (
unsigned int j = 0; j < 3; j++)
357 for (
unsigned int j = 0; j < 3; j++) {
358 for (
unsigned int i = 0; i < 3; i++) {
374 for (
unsigned int i = 0; i <
rowNum; i++)
375 for (
unsigned int j = 0; j <
colNum; j++)
387 for (
unsigned int i = 0; i <
rowNum; i++)
388 for (
unsigned int j = 0; j <
colNum; j++)
402 bool isRotation =
true;
410 for (i = 0; i < 3; i++) {
411 for (j = 0; j < 3; j++) {
413 if (fabs(RtR[i][j] - 1) > threshold)
416 if (fabs(RtR[i][j]) > threshold)
423 for (i = 0; i < 3; i++) {
429 for (i = 0; i < 3; i++) {
501 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) 544 for (i = 0; i < 3; i++)
545 for (j = 0; j < 3; j++)
546 Rt[j][i] = (*
this)[i][j];
591 for (
unsigned int i = 0; i < 3; i++)
592 std::cout << tu[i] <<
" ";
594 std::cout << std::endl;
609 double theta, si, co, sinc, mcosc;
612 theta = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
618 R[0][0] = co + mcosc * v[0] * v[0];
619 R[0][1] = -sinc * v[2] + mcosc * v[0] * v[1];
620 R[0][2] = sinc * v[1] + mcosc * v[0] * v[2];
621 R[1][0] = sinc * v[2] + mcosc * v[1] * v[0];
622 R[1][1] = co + mcosc * v[1] * v[1];
623 R[1][2] = -sinc * v[0] + mcosc * v[1] * v[2];
624 R[2][0] = -sinc * v[1] + mcosc * v[2] * v[0];
625 R[2][1] = sinc * v[0] + mcosc * v[2] * v[1];
626 R[2][2] = co + mcosc * v[2] * v[2];
628 for (i = 0; i < 3; i++)
629 for (j = 0; j < 3; j++)
630 (*
this)[i][j] = R[i][j];
640 for (
unsigned int i = 0; i < 3; i++)
641 for (
unsigned int j = 0; j < 3; j++)
642 (*
this)[i][j] = M[i][j];
667 double c0, c1, c2, s0, s1, s2;
676 (*this)[0][0] = c0 * c1 * c2 - s0 * s2;
677 (*this)[0][1] = -c0 * c1 * s2 - s0 * c2;
678 (*this)[0][2] = c0 * s1;
679 (*this)[1][0] = s0 * c1 * c2 + c0 * s2;
680 (*this)[1][1] = -s0 * c1 * s2 + c0 * c2;
681 (*this)[1][2] = s0 * s1;
682 (*this)[2][0] = -s1 * c2;
683 (*this)[2][1] = s1 * s2;
699 double c0, c1, c2, s0, s1, s2;
708 (*this)[0][0] = c1 * c2;
709 (*this)[0][1] = -c1 * s2;
711 (*this)[1][0] = c0 * s2 + s0 * s1 * c2;
712 (*this)[1][1] = c0 * c2 - s0 * s1 * s2;
713 (*this)[1][2] = -s0 * c1;
714 (*this)[2][0] = -c0 * s1 * c2 + s0 * s2;
715 (*this)[2][1] = c0 * s1 * s2 + c2 * s0;
716 (*this)[2][2] = c0 * c1;
729 double c0, c1, c2, s0, s1, s2;
738 (*this)[0][0] = c0 * c1;
739 (*this)[0][1] = c0 * s1 * s2 - s0 * c2;
740 (*this)[0][2] = c0 * s1 * c2 + s0 * s2;
742 (*this)[1][0] = s0 * c1;
743 (*this)[1][1] = s0 * s1 * s2 + c0 * c2;
744 (*this)[1][2] = s0 * s1 * c2 - c0 * s2;
747 (*this)[2][1] = c1 * s2;
748 (*this)[2][2] = c1 * c2;
773 (*this)[0][0] = a * a + b * b - c * c - d * d;
774 (*this)[0][1] = 2 * b * c - 2 * a * d;
775 (*this)[0][2] = 2 * a * c + 2 * b * d;
777 (*this)[1][0] = 2 * a * d + 2 * b * c;
778 (*this)[1][1] = a * a - b * b + c * c - d * d;
779 (*this)[1][2] = 2 * c * d - 2 * a * b;
781 (*this)[2][0] = 2 * b * d - 2 * a * c;
782 (*this)[2][1] = 2 * a * b + 2 * c * d;
783 (*this)[2][2] = a * a - b * b - c * c + d * d;
794 unsigned int Rrow = R.
getRows();
795 unsigned int Rcol = R.
getCols();
797 for (
unsigned int i = 0; i < Rrow; i++)
798 for (
unsigned int j = 0; j < Rcol; j++)
799 C[i][j] = R[i][j] * x;
846 unsigned int nb_rows =
getRows();
848 for (
unsigned int i = 0; i < nb_rows; i++)
849 c[i] = (*
this)[i][j];
865 for (
size_t i = 0; i < vec_M.size(); i++) {
866 R = vec_M[i].getRotationMatrix();
869 meanR /=
static_cast<double>(vec_M.size());
875 double det = sv[0]*sv[1]*sv[2];
882 D[0][0] = D[1][1] = 1.0; D[2][2] = -1;
883 meanR = U * D * V.
t();
902 for (
size_t i = 0; i < vec_R.size(); i++) {
905 meanR /=
static_cast<double>(vec_R.size());
911 double det = sv[0]*sv[1]*sv[2];
918 D[0][0] = D[1][1] = 1.0; D[2][2] = -1;
919 meanR = U * D * V.
t();
926 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) 937 #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.
vpRotationMatrix & operator,(double val)
error that can be emited by ViSP classes.
double y() const
Returns y-component of the quaternion.
vpRotationMatrix t() const
double * data
Address of the first element of the data array.
Implementation of a generic 2D array used as base class for matrices and vectors. ...
unsigned int size() const
Return the number of elements of the 2D array.
unsigned int getCols() const
Implementation of a rotation vector as Euler angle minimal representation.
vp_deprecated void setIdentity()
vpColVector getCol(unsigned int j) const
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.
static vpRotationMatrix mean(const std::vector< vpHomogeneousMatrix > &vec_M)
static double mcosc(double cosx, double x)
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
vpRotationMatrix & operator<<(double val)
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
unsigned int dsize
Current array size (rowNum * colNum)
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.
vpRotationMatrix & operator*=(double x)