42 #include <visp3/core/vpMath.h>
43 #include <visp3/core/vpMatrix.h>
46 #include <visp3/core/vpRotationMatrix.h>
49 #include <visp3/core/vpException.h>
53 #include <visp3/core/vpDebug.h>
62 for (
unsigned int i = 0; i < 3; i++) {
63 for (
unsigned int j = 0; j < 3; j++) {
83 for (
unsigned int i = 0; i < 3; i++) {
84 for (
unsigned int j = 0; j < 3; j++) {
117 if (
dsize !=
static_cast<unsigned int>(list.size())) {
119 "Cannot set a 3-by-3 rotation matrix from a %d-elements list of doubles."));
122 std::copy(list.begin(), list.end(),
data);
131 "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
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 "
242 "Cannot set rotation matrix out of bounds. It has only %d elements while you try to initialize "
257 for (
unsigned int i = 0; i < 3; i++) {
258 for (
unsigned int j = 0; j < 3; j++) {
260 for (
unsigned int k = 0; k < 3; k++)
293 for (
unsigned int i = 0; i < 3; i++) {
294 for (
unsigned int j = 0; j < 3; j++) {
296 for (
unsigned int k = 0; k < 3; k++)
297 s += (*
this)[i][k] * M[k][j];
356 "Cannot multiply a (3x3) rotation matrix by a %d "
357 "dimension column vector",
362 for (
unsigned int j = 0; j <
colNum; j++) {
364 for (
unsigned int i = 0; i <
rowNum; i++) {
365 v_out[i] +=
rowPtrs[i][j] * vj;
380 for (
unsigned int j = 0; j < 3; j++)
383 for (
unsigned int j = 0; j < 3; j++) {
384 for (
unsigned int i = 0; i < 3; i++) {
400 for (
unsigned int i = 0; i <
rowNum; i++)
401 for (
unsigned int j = 0; j <
colNum; j++)
413 for (
unsigned int i = 0; i <
rowNum; i++)
414 for (
unsigned int j = 0; j <
colNum; j++)
430 bool isRotation =
true;
438 for (
unsigned int i = 0; i < 3; i++) {
439 for (
unsigned int j = 0; j < 3; j++) {
441 if (fabs(RtR[i][j] - 1) > threshold) {
446 if (fabs(RtR[i][j]) > threshold) {
454 for (
unsigned int i = 0; i < 3; i++) {
461 for (
unsigned int i = 0; i < 3; i++) {
560 :
vpArray2D<double>(3, 3, list), m_index(0)
569 "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
584 for (
unsigned int i = 0; i < 3; i++)
585 for (
unsigned int j = 0; j < 3; j++)
586 Rt[j][i] = (*
this)[i][j];
631 for (
unsigned int i = 0; i < 3; i++)
632 std::cout << tu[i] <<
" ";
634 std::cout << std::endl;
648 double theta, si, co, sinc, mcosc;
651 theta = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
657 R[0][0] = co + mcosc * v[0] * v[0];
658 R[0][1] = -sinc * v[2] + mcosc * v[0] * v[1];
659 R[0][2] = sinc * v[1] + mcosc * v[0] * v[2];
660 R[1][0] = sinc * v[2] + mcosc * v[1] * v[0];
661 R[1][1] = co + mcosc * v[1] * v[1];
662 R[1][2] = -sinc * v[0] + mcosc * v[1] * v[2];
663 R[2][0] = -sinc * v[1] + mcosc * v[2] * v[0];
664 R[2][1] = sinc * v[0] + mcosc * v[2] * v[1];
665 R[2][2] = co + mcosc * v[2] * v[2];
667 for (
unsigned int i = 0; i < 3; i++)
668 for (
unsigned int j = 0; j < 3; j++)
669 (*
this)[i][j] = R[i][j];
679 for (
unsigned int i = 0; i < 3; i++)
680 for (
unsigned int j = 0; j < 3; j++)
681 (*
this)[i][j] = M[i][j];
706 double c0, c1, c2, s0, s1, s2;
715 (*this)[0][0] = c0 * c1 * c2 - s0 * s2;
716 (*this)[0][1] = -c0 * c1 * s2 - s0 * c2;
717 (*this)[0][2] = c0 * s1;
718 (*this)[1][0] = s0 * c1 * c2 + c0 * s2;
719 (*this)[1][1] = -s0 * c1 * s2 + c0 * c2;
720 (*this)[1][2] = s0 * s1;
721 (*this)[2][0] = -s1 * c2;
722 (*this)[2][1] = s1 * s2;
738 double c0, c1, c2, s0, s1, s2;
747 (*this)[0][0] = c1 * c2;
748 (*this)[0][1] = -c1 * s2;
750 (*this)[1][0] = c0 * s2 + s0 * s1 * c2;
751 (*this)[1][1] = c0 * c2 - s0 * s1 * s2;
752 (*this)[1][2] = -s0 * c1;
753 (*this)[2][0] = -c0 * s1 * c2 + s0 * s2;
754 (*this)[2][1] = c0 * s1 * s2 + c2 * s0;
755 (*this)[2][2] = c0 * c1;
768 double c0, c1, c2, s0, s1, s2;
777 (*this)[0][0] = c0 * c1;
778 (*this)[0][1] = c0 * s1 * s2 - s0 * c2;
779 (*this)[0][2] = c0 * s1 * c2 + s0 * s2;
781 (*this)[1][0] = s0 * c1;
782 (*this)[1][1] = s0 * s1 * s2 + c0 * c2;
783 (*this)[1][2] = s0 * s1 * c2 - c0 * s2;
786 (*this)[2][1] = c1 * s2;
787 (*this)[2][2] = c1 * c2;
812 (*this)[0][0] = a * a + b * b - c * c - d * d;
813 (*this)[0][1] = 2 * b * c - 2 * a * d;
814 (*this)[0][2] = 2 * a * c + 2 * b * d;
816 (*this)[1][0] = 2 * a * d + 2 * b * c;
817 (*this)[1][1] = a * a - b * b + c * c - d * d;
818 (*this)[1][2] = 2 * c * d - 2 * a * b;
820 (*this)[2][0] = 2 * b * d - 2 * a * c;
821 (*this)[2][1] = 2 * a * b + 2 * c * d;
822 (*this)[2][2] = a * a - b * b - c * c + d * d;
833 unsigned int Rrow = R.
getRows();
834 unsigned int Rcol = R.
getCols();
836 for (
unsigned int i = 0; i < Rrow; i++)
837 for (
unsigned int j = 0; j < Rcol; j++)
838 C[i][j] = R[i][j] * x;
885 unsigned int nb_rows =
getRows();
887 for (
unsigned int i = 0; i < nb_rows; i++)
888 c[i] = (*
this)[i][j];
905 for (
size_t i = 0; i < vec_M.size(); i++) {
906 R = vec_M[i].getRotationMatrix();
909 meanR /=
static_cast<double>(vec_M.size());
915 double det = sv[0] * sv[1] * sv[2];
922 D[0][0] = D[1][1] = 1.0;
924 meanR = U * D * V.
t();
943 for (
size_t i = 0; i < vec_R.size(); i++) {
946 meanR /=
static_cast<double>(vec_R.size());
952 double det = sv[0] * sv[1] * sv[2];
959 D[0][0] = D[1][1] = 1.0;
961 meanR = U * D * V.
t();
980 double det = R.
det();
1000 #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.
vpColVector operator*(const double &x, const vpColVector &v)
error that can be emitted by ViSP classes.
@ dimensionError
Bad dimension.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
vpTranslationVector getTranslationVector() const
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.
void svd(vpColVector &w, vpMatrix &V)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
double det(vpDetMethod method=LU_DECOMPOSITION) const
Implementation of a pose vector and operations on poses.
Implementation of a rotation vector as quaternion angle minimal representation.
const double & z() const
Returns the z-component of the quaternion.
const double & x() const
Returns the x-component of the quaternion.
const double & y() const
Returns the y-component of the quaternion.
const double & w() const
Returns the w-component of the quaternion.
Implementation of a rotation matrix and operations on such kind of matrices.
vp_deprecated void setIdentity()
vpRotationMatrix & operator*=(double x)
bool isARotationMatrix(double threshold=1e-6) const
vpRotationMatrix & operator,(double val)
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.