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)
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.
error that can be emited by ViSP classes.
@ dimensionError
Bad dimension.
Implementation of an homogeneous matrix and operations on such kind of matrices.
VISP_EXPORT vpImagePoint operator*(const vpImagePoint &ip1, double scale)
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.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Implementation of a pose vector and operations on poses.
Implementation of a rotation vector as quaternion angle minimal representation.
double w() const
Returns w-component of the quaternion.
double y() const
Returns y-component of the quaternion.
double z() const
Returns z-component of the quaternion.
double x() const
Returns x-component of the quaternion.
Implementation of a rotation matrix and operations on such kind of matrices.
vp_deprecated void setIdentity()
vpRotationMatrix & operator*=(double x)
vpRotationMatrix & operator,(double val)
bool isARotationMatrix() const
vpColVector getCol(unsigned int j) const
vpThetaUVector getThetaUVector()
vpRotationMatrix & operator<<(double val)
vpRotationMatrix t() const
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 buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.