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>
65 for (
unsigned int i = 0; i < 3; i++) {
66 for (
unsigned int j = 0; j < 3; j++) {
86 for (
unsigned int i = 0; i < 3; i++) {
87 for (
unsigned int j = 0; j < 3; j++) {
95 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
123 if (
dsize !=
static_cast<unsigned int>(list.size())) {
125 "Cannot set a 3-by-3 rotation matrix from a %d-elements list of doubles."));
128 std::copy(list.begin(), list.end(),
data);
136 "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
167 for (
unsigned int i = 0; i < 3; i++) {
168 for (
unsigned int j = 0; j < 3; j++) {
169 (*this)[i][j] = M[i][j];
175 "from a matrix that is not a "
248 "Cannot set rotation matrix out of bounds. It has only %d elements while you try to initialize "
263 for (
unsigned int i = 0; i < 3; i++) {
264 for (
unsigned int j = 0; j < 3; j++) {
266 for (
unsigned int k = 0; k < 3; k++)
295 for (
unsigned int i = 0; i < 3; i++) {
296 for (
unsigned int j = 0; j < 3; j++) {
298 for (
unsigned int k = 0; k < 3; k++)
299 s += (*
this)[i][k] * M[k][j];
339 "Cannot multiply a (3x3) rotation matrix by a %d "
340 "dimension column vector",
345 for (
unsigned int j = 0; j <
colNum; j++) {
347 for (
unsigned int i = 0; i <
rowNum; i++) {
348 v_out[i] +=
rowPtrs[i][j] * vj;
363 for (
unsigned int j = 0; j < 3; j++)
366 for (
unsigned int j = 0; j < 3; j++) {
367 for (
unsigned int i = 0; i < 3; i++) {
383 for (
unsigned int i = 0; i <
rowNum; i++)
384 for (
unsigned int j = 0; j <
colNum; j++)
396 for (
unsigned int i = 0; i <
rowNum; i++)
397 for (
unsigned int j = 0; j <
colNum; j++)
413 bool isRotation =
true;
421 for (
unsigned int i = 0; i < 3; i++) {
422 for (
unsigned int j = 0; j < 3; j++) {
424 if (fabs(RtR[i][j] - 1) > threshold) {
428 if (fabs(RtR[i][j]) > threshold) {
436 for (
unsigned int i = 0; i < 3; i++) {
443 for (
unsigned int i = 0; i < 3; i++) {
520 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
545 :
vpArray2D<double>(3, 3, list), m_index(0)
553 "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
569 for (
unsigned int i = 0; i < 3; i++)
570 for (
unsigned int j = 0; j < 3; j++)
571 Rt[j][i] = (*
this)[i][j];
616 for (
unsigned int i = 0; i < 3; i++)
617 std::cout << tu[i] <<
" ";
619 std::cout << std::endl;
633 double theta, si, co, sinc, mcosc;
636 theta = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
642 R[0][0] = co + mcosc * v[0] * v[0];
643 R[0][1] = -sinc * v[2] + mcosc * v[0] * v[1];
644 R[0][2] = sinc * v[1] + mcosc * v[0] * v[2];
645 R[1][0] = sinc * v[2] + mcosc * v[1] * v[0];
646 R[1][1] = co + mcosc * v[1] * v[1];
647 R[1][2] = -sinc * v[0] + mcosc * v[1] * v[2];
648 R[2][0] = -sinc * v[1] + mcosc * v[2] * v[0];
649 R[2][1] = sinc * v[0] + mcosc * v[2] * v[1];
650 R[2][2] = co + mcosc * v[2] * v[2];
652 for (
unsigned int i = 0; i < 3; i++)
653 for (
unsigned int j = 0; j < 3; j++)
654 (*
this)[i][j] = R[i][j];
664 for (
unsigned int i = 0; i < 3; i++)
665 for (
unsigned int j = 0; j < 3; j++)
666 (*
this)[i][j] = M[i][j];
691 double c0, c1, c2, s0, s1, s2;
700 (*this)[0][0] = c0 * c1 * c2 - s0 * s2;
701 (*this)[0][1] = -c0 * c1 * s2 - s0 * c2;
702 (*this)[0][2] = c0 * s1;
703 (*this)[1][0] = s0 * c1 * c2 + c0 * s2;
704 (*this)[1][1] = -s0 * c1 * s2 + c0 * c2;
705 (*this)[1][2] = s0 * s1;
706 (*this)[2][0] = -s1 * c2;
707 (*this)[2][1] = s1 * s2;
723 double c0, c1, c2, s0, s1, s2;
732 (*this)[0][0] = c1 * c2;
733 (*this)[0][1] = -c1 * s2;
735 (*this)[1][0] = c0 * s2 + s0 * s1 * c2;
736 (*this)[1][1] = c0 * c2 - s0 * s1 * s2;
737 (*this)[1][2] = -s0 * c1;
738 (*this)[2][0] = -c0 * s1 * c2 + s0 * s2;
739 (*this)[2][1] = c0 * s1 * s2 + c2 * s0;
740 (*this)[2][2] = c0 * c1;
753 double c0, c1, c2, s0, s1, s2;
762 (*this)[0][0] = c0 * c1;
763 (*this)[0][1] = c0 * s1 * s2 - s0 * c2;
764 (*this)[0][2] = c0 * s1 * c2 + s0 * s2;
766 (*this)[1][0] = s0 * c1;
767 (*this)[1][1] = s0 * s1 * s2 + c0 * c2;
768 (*this)[1][2] = s0 * s1 * c2 - c0 * s2;
771 (*this)[2][1] = c1 * s2;
772 (*this)[2][2] = c1 * c2;
797 (*this)[0][0] = a * a + b * b - c * c - d * d;
798 (*this)[0][1] = 2 * b * c - 2 * a * d;
799 (*this)[0][2] = 2 * a * c + 2 * b * d;
801 (*this)[1][0] = 2 * a * d + 2 * b * c;
802 (*this)[1][1] = a * a - b * b + c * c - d * d;
803 (*this)[1][2] = 2 * c * d - 2 * a * b;
805 (*this)[2][0] = 2 * b * d - 2 * a * c;
806 (*this)[2][1] = 2 * a * b + 2 * c * d;
807 (*this)[2][2] = a * a - b * b - c * c + d * d;
818 unsigned int Rrow = R.
getRows();
819 unsigned int Rcol = R.
getCols();
821 for (
unsigned int i = 0; i < Rrow; i++)
822 for (
unsigned int j = 0; j < Rcol; j++)
823 C[i][j] = R[i][j] * x;
870 unsigned int nb_rows =
getRows();
872 for (
unsigned int i = 0; i < nb_rows; i++)
873 c[i] = (*
this)[i][j];
890 for (
size_t i = 0; i < vec_M.size(); i++) {
891 R = vec_M[i].getRotationMatrix();
894 meanR /=
static_cast<double>(vec_M.size());
900 double det = sv[0] * sv[1] * sv[2];
906 D[0][0] = D[1][1] = 1.0;
908 meanR = U * D * V.
t();
927 for (
size_t i = 0; i < vec_R.size(); i++) {
930 meanR /=
static_cast<double>(vec_R.size());
936 double det = sv[0] * sv[1] * sv[2];
942 D[0][0] = D[1][1] = 1.0;
944 meanR = U * D * V.
t();
963 double det = R.
det();
983 #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.
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.