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) {
85 for (
unsigned int i = 0; i < 3; ++i) {
86 for (
unsigned int j = 0; j < 3; ++j) {
94 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
120 if (
dsize !=
static_cast<unsigned int>(list.size())) {
122 "Cannot set a 3-by-3 rotation matrix from a %d-elements list of doubles."));
125 std::copy(list.begin(), list.end(),
data);
134 "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
165 for (
unsigned int i = 0; i < 3; ++i) {
166 for (
unsigned int j = 0; j < 3; ++j) {
167 (*this)[i][j] = M[i][j];
173 "from a matrix that is not a "
246 "Cannot set rotation matrix out of bounds. It has only %d elements while you try to initialize "
261 for (
unsigned int i = 0; i < 3; ++i) {
262 for (
unsigned int j = 0; j < 3; ++j) {
264 for (
unsigned int k = 0; k < 3; ++k) {
298 for (
unsigned int i = 0; i < 3; ++i) {
299 for (
unsigned int j = 0; j < 3; ++j) {
301 for (
unsigned int k = 0; k < 3; ++k) {
302 s += (*this)[i][k] * M[k][j];
362 "Cannot multiply a (3x3) rotation matrix by a %d "
363 "dimension column vector",
368 for (
unsigned int j = 0; j <
colNum; ++j) {
370 for (
unsigned int i = 0; i <
rowNum; ++i) {
371 v_out[i] +=
rowPtrs[i][j] * vj;
386 for (
unsigned int j = 0; j < 3; ++j) {
390 for (
unsigned int j = 0; j < 3; ++j) {
391 for (
unsigned int i = 0; i < 3; ++i) {
407 for (
unsigned int i = 0; i <
rowNum; ++i) {
408 for (
unsigned int j = 0; j <
colNum; ++j) {
422 for (
unsigned int i = 0; i <
rowNum; ++i) {
423 for (
unsigned int j = 0; j <
colNum; ++j) {
441 bool isRotation =
true;
449 for (
unsigned int i = 0; i < 3; ++i) {
450 for (
unsigned int j = 0; j < 3; ++j) {
452 if (fabs(RtR[i][j] - 1) > threshold) {
457 if (fabs(RtR[i][j]) > threshold) {
465 for (
unsigned int i = 0; i < 3; ++i) {
472 for (
unsigned int i = 0; i < 3; ++i) {
549 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
572 :
vpArray2D<double>(3, 3, list), m_index(0)
581 "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
597 for (
unsigned int i = 0; i < 3; ++i) {
598 for (
unsigned int j = 0; j < 3; ++j) {
599 Rt[j][i] = (*this)[i][j];
646 for (
unsigned int i = 0; i < 3; ++i) {
647 std::cout << tu[i] <<
" ";
650 std::cout << std::endl;
664 double theta, si, co, sinc, mcosc;
667 theta = sqrt((v[0] * v[0]) + (v[1] * v[1]) + (v[2] * v[2]));
673 R[0][0] = co + (mcosc * v[0] * v[0]);
674 R[0][1] = (-sinc * v[2]) + (mcosc * v[0] * v[1]);
675 R[0][2] = (sinc * v[1]) + (mcosc * v[0] * v[2]);
676 R[1][0] = (sinc * v[2]) + (mcosc * v[1] * v[0]);
677 R[1][1] = co + (mcosc * v[1] * v[1]);
678 R[1][2] = (-sinc * v[0]) + (mcosc * v[1] * v[2]);
679 R[2][0] = (-sinc * v[1]) + (mcosc * v[2] * v[0]);
680 R[2][1] = (sinc * v[0]) + (mcosc * v[2] * v[1]);
681 R[2][2] = co + (mcosc * v[2] * v[2]);
683 for (
unsigned int i = 0; i < 3; ++i) {
684 for (
unsigned int j = 0; j < 3; ++j) {
685 (*this)[i][j] = R[i][j];
697 for (
unsigned int i = 0; i < 3; ++i) {
698 for (
unsigned int j = 0; j < 3; ++j) {
699 (*this)[i][j] = M[i][j];
726 double c0, c1, c2, s0, s1, s2;
735 (*this)[0][0] = (c0 * c1 * c2) - (s0 * s2);
736 (*this)[0][1] = (-c0 * c1 * s2) - (s0 * c2);
737 (*this)[0][2] = c0 * s1;
738 (*this)[1][0] = (s0 * c1 * c2) + (c0 * s2);
739 (*this)[1][1] = (-s0 * c1 * s2) + (c0 * c2);
740 (*this)[1][2] = s0 * s1;
741 (*this)[2][0] = -s1 * c2;
742 (*this)[2][1] = s1 * s2;
758 double c0, c1, c2, s0, s1, s2;
767 (*this)[0][0] = c1 * c2;
768 (*this)[0][1] = -c1 * s2;
770 (*this)[1][0] = (c0 * s2) + (s0 * s1 * c2);
771 (*this)[1][1] = (c0 * c2) - (s0 * s1 * s2);
772 (*this)[1][2] = -s0 * c1;
773 (*this)[2][0] = (-c0 * s1 * c2) + (s0 * s2);
774 (*this)[2][1] = (c0 * s1 * s2) + (c2 * s0);
775 (*this)[2][2] = c0 * c1;
788 double c0, c1, c2, s0, s1, s2;
797 (*this)[0][0] = c0 * c1;
798 (*this)[0][1] = (c0 * s1 * s2) - (s0 * c2);
799 (*this)[0][2] = (c0 * s1 * c2) + (s0 * s2);
801 (*this)[1][0] = s0 * c1;
802 (*this)[1][1] = (s0 * s1 * s2) + (c0 * c2);
803 (*this)[1][2] = (s0 * s1 * c2) - (c0 * s2);
806 (*this)[2][1] = c1 * s2;
807 (*this)[2][2] = c1 * c2;
832 (*this)[0][0] = (((a * a) + (b * b)) - (c * c)) - (d * d);
833 (*this)[0][1] = (2 * b * c) - (2 * a * d);
834 (*this)[0][2] = (2 * a * c) + (2 * b * d);
836 (*this)[1][0] = (2 * a * d) + (2 * b * c);
837 (*this)[1][1] = (((a * a) - (b * b)) + (c * c)) - (d * d);
838 (*this)[1][2] = (2 * c * d) - (2 * a * b);
840 (*this)[2][0] = (2 * b * d) - (2 * a * c);
841 (*this)[2][1] = (2 * a * b) + (2 * c * d);
842 (*this)[2][2] = ((a * a) - (b * b) - (c * c)) + (d * d);
853 unsigned int Rrow = R.
getRows();
854 unsigned int Rcol = R.
getCols();
856 for (
unsigned int i = 0; i < Rrow; ++i) {
857 for (
unsigned int j = 0; j < Rcol; ++j) {
858 C[i][j] = R[i][j] * x;
908 unsigned int nb_rows =
getRows();
910 for (
unsigned int i = 0; i < nb_rows; ++i) {
911 c[i] = (*this)[i][j];
929 size_t vec_m_size = vec_M.
size();
930 for (
size_t i = 0; i < vec_m_size; ++i) {
931 R = vec_M[i].getRotationMatrix();
934 meanR /=
static_cast<double>(vec_M.size());
940 double det = sv[0] * sv[1] * sv[2];
950 meanR = U * D * V.
t();
969 size_t vec_r_size = vec_R.
size();
970 for (
size_t i = 0; i < vec_r_size; ++i) {
973 meanR /=
static_cast<double>(vec_R.size());
979 double det = sv[0] * sv[1] * sv[2];
989 meanR = U * D * V.
t();
1008 double det = R.
det();
1028 #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.