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())) {
127 std::copy(list.begin(), list.end(),
data);
160 for (
unsigned int i = 0; i < 3; i++) {
161 for (
unsigned int j = 0; j < 3; j++) {
162 (*this)[i][j] = M[i][j];
168 "from a matrix that is not a "
253 for (
unsigned int i = 0; i < 3; i++) {
254 for (
unsigned int j = 0; j < 3; j++) {
256 for (
unsigned int k = 0; k < 3; k++)
285 for (
unsigned int i = 0; i < 3; i++) {
286 for (
unsigned int j = 0; j < 3; j++) {
288 for (
unsigned int k = 0; k < 3; k++)
289 s += (*
this)[i][k] * M[k][j];
329 "Cannot multiply a (3x3) rotation matrix by a %d "
330 "dimension column vector",
335 for (
unsigned int j = 0; j <
colNum; j++) {
337 for (
unsigned int i = 0; i <
rowNum; i++) {
338 v_out[i] +=
rowPtrs[i][j] * vj;
353 for (
unsigned int j = 0; j < 3; j++)
356 for (
unsigned int j = 0; j < 3; j++) {
357 for (
unsigned int i = 0; i < 3; i++) {
373 for (
unsigned int i = 0; i <
rowNum; i++)
374 for (
unsigned int j = 0; j <
colNum; j++)
386 for (
unsigned int i = 0; i <
rowNum; i++)
387 for (
unsigned int j = 0; j <
colNum; j++)
400 bool isRotation =
true;
408 for (
unsigned int i = 0; i < 3; i++) {
409 for (
unsigned int j = 0; j < 3; j++) {
411 if (fabs(RtR[i][j] - 1) > threshold) {
415 if (fabs(RtR[i][j]) > threshold) {
423 for (
unsigned int i = 0; i < 3; i++) {
430 for (
unsigned int i = 0; i < 3; i++) {
504 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
546 for (
unsigned int i = 0; i < 3; i++)
547 for (
unsigned int j = 0; j < 3; j++)
548 Rt[j][i] = (*
this)[i][j];
593 for (
unsigned int i = 0; i < 3; i++)
594 std::cout << tu[i] <<
" ";
596 std::cout << std::endl;
610 double theta, si, co, sinc, mcosc;
613 theta = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
619 R[0][0] = co + mcosc * v[0] * v[0];
620 R[0][1] = -sinc * v[2] + mcosc * v[0] * v[1];
621 R[0][2] = sinc * v[1] + mcosc * v[0] * v[2];
622 R[1][0] = sinc * v[2] + mcosc * v[1] * v[0];
623 R[1][1] = co + mcosc * v[1] * v[1];
624 R[1][2] = -sinc * v[0] + mcosc * v[1] * v[2];
625 R[2][0] = -sinc * v[1] + mcosc * v[2] * v[0];
626 R[2][1] = sinc * v[0] + mcosc * v[2] * v[1];
627 R[2][2] = co + mcosc * v[2] * v[2];
629 for (
unsigned int i = 0; i < 3; i++)
630 for (
unsigned int j = 0; j < 3; j++)
631 (*
this)[i][j] = R[i][j];
641 for (
unsigned int i = 0; i < 3; i++)
642 for (
unsigned int j = 0; j < 3; j++)
643 (*
this)[i][j] = M[i][j];
668 double c0, c1, c2, s0, s1, s2;
677 (*this)[0][0] = c0 * c1 * c2 - s0 * s2;
678 (*this)[0][1] = -c0 * c1 * s2 - s0 * c2;
679 (*this)[0][2] = c0 * s1;
680 (*this)[1][0] = s0 * c1 * c2 + c0 * s2;
681 (*this)[1][1] = -s0 * c1 * s2 + c0 * c2;
682 (*this)[1][2] = s0 * s1;
683 (*this)[2][0] = -s1 * c2;
684 (*this)[2][1] = s1 * s2;
700 double c0, c1, c2, s0, s1, s2;
709 (*this)[0][0] = c1 * c2;
710 (*this)[0][1] = -c1 * s2;
712 (*this)[1][0] = c0 * s2 + s0 * s1 * c2;
713 (*this)[1][1] = c0 * c2 - s0 * s1 * s2;
714 (*this)[1][2] = -s0 * c1;
715 (*this)[2][0] = -c0 * s1 * c2 + s0 * s2;
716 (*this)[2][1] = c0 * s1 * s2 + c2 * s0;
717 (*this)[2][2] = c0 * c1;
730 double c0, c1, c2, s0, s1, s2;
739 (*this)[0][0] = c0 * c1;
740 (*this)[0][1] = c0 * s1 * s2 - s0 * c2;
741 (*this)[0][2] = c0 * s1 * c2 + s0 * s2;
743 (*this)[1][0] = s0 * c1;
744 (*this)[1][1] = s0 * s1 * s2 + c0 * c2;
745 (*this)[1][2] = s0 * s1 * c2 - c0 * s2;
748 (*this)[2][1] = c1 * s2;
749 (*this)[2][2] = c1 * c2;
774 (*this)[0][0] = a * a + b * b - c * c - d * d;
775 (*this)[0][1] = 2 * b * c - 2 * a * d;
776 (*this)[0][2] = 2 * a * c + 2 * b * d;
778 (*this)[1][0] = 2 * a * d + 2 * b * c;
779 (*this)[1][1] = a * a - b * b + c * c - d * d;
780 (*this)[1][2] = 2 * c * d - 2 * a * b;
782 (*this)[2][0] = 2 * b * d - 2 * a * c;
783 (*this)[2][1] = 2 * a * b + 2 * c * d;
784 (*this)[2][2] = a * a - b * b - c * c + d * d;
795 unsigned int Rrow = R.
getRows();
796 unsigned int Rcol = R.
getCols();
798 for (
unsigned int i = 0; i < Rrow; i++)
799 for (
unsigned int j = 0; j < Rcol; j++)
800 C[i][j] = R[i][j] * x;
847 unsigned int nb_rows =
getRows();
849 for (
unsigned int i = 0; i < nb_rows; i++)
850 c[i] = (*
this)[i][j];
866 for (
size_t i = 0; i < vec_M.size(); i++) {
867 R = vec_M[i].getRotationMatrix();
870 meanR /=
static_cast<double>(vec_M.size());
876 double det = sv[0]*sv[1]*sv[2];
883 D[0][0] = D[1][1] = 1.0; D[2][2] = -1;
884 meanR = U * D * V.
t();
903 for (
size_t i = 0; i < vec_R.size(); i++) {
906 meanR /=
static_cast<double>(vec_R.size());
912 double det = sv[0]*sv[1]*sv[2];
919 D[0][0] = D[1][1] = 1.0; D[2][2] = -1;
920 meanR = U * D * V.
t();
927 #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)
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.