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++) {
92#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++)
297 for (
unsigned int i = 0; i < 3; i++) {
298 for (
unsigned int j = 0; j < 3; j++) {
300 for (
unsigned int k = 0; k < 3; k++)
301 s += (*
this)[i][k] * M[k][j];
360 "Cannot multiply a (3x3) rotation matrix by a %d "
361 "dimension column vector",
366 for (
unsigned int j = 0; j <
colNum; j++) {
368 for (
unsigned int i = 0; i <
rowNum; i++) {
369 v_out[i] +=
rowPtrs[i][j] * vj;
384 for (
unsigned int j = 0; j < 3; j++)
387 for (
unsigned int j = 0; j < 3; j++) {
388 for (
unsigned int i = 0; i < 3; i++) {
404 for (
unsigned int i = 0; i <
rowNum; i++)
405 for (
unsigned int j = 0; j <
colNum; j++)
417 for (
unsigned int i = 0; i <
rowNum; i++)
418 for (
unsigned int j = 0; j <
colNum; j++)
434 bool isRotation =
true;
442 for (
unsigned int i = 0; i < 3; i++) {
443 for (
unsigned int j = 0; j < 3; j++) {
445 if (fabs(RtR[i][j] - 1) > threshold) {
450 if (fabs(RtR[i][j]) > threshold) {
458 for (
unsigned int i = 0; i < 3; i++) {
465 for (
unsigned int i = 0; i < 3; i++) {
542#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
567 :
vpArray2D<double>(3, 3, list), m_index(0)
576 "Rotation matrix initialization fails since its elements do not represent a valid rotation matrix"));
592 for (
unsigned int i = 0; i < 3; i++)
593 for (
unsigned int j = 0; j < 3; j++)
594 Rt[j][i] = (*
this)[i][j];
639 for (
unsigned int i = 0; i < 3; i++)
640 std::cout << tu[i] <<
" ";
642 std::cout << std::endl;
656 double theta, si, co, sinc, mcosc;
659 theta = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
665 R[0][0] = co + mcosc * v[0] * v[0];
666 R[0][1] = -sinc * v[2] + mcosc * v[0] * v[1];
667 R[0][2] = sinc * v[1] + mcosc * v[0] * v[2];
668 R[1][0] = sinc * v[2] + mcosc * v[1] * v[0];
669 R[1][1] = co + mcosc * v[1] * v[1];
670 R[1][2] = -sinc * v[0] + mcosc * v[1] * v[2];
671 R[2][0] = -sinc * v[1] + mcosc * v[2] * v[0];
672 R[2][1] = sinc * v[0] + mcosc * v[2] * v[1];
673 R[2][2] = co + mcosc * v[2] * v[2];
675 for (
unsigned int i = 0; i < 3; i++)
676 for (
unsigned int j = 0; j < 3; j++)
677 (*
this)[i][j] = R[i][j];
687 for (
unsigned int i = 0; i < 3; i++)
688 for (
unsigned int j = 0; j < 3; j++)
689 (*
this)[i][j] = M[i][j];
714 double c0, c1, c2, s0, s1, s2;
723 (*this)[0][0] = c0 * c1 * c2 - s0 * s2;
724 (*this)[0][1] = -c0 * c1 * s2 - s0 * c2;
725 (*this)[0][2] = c0 * s1;
726 (*this)[1][0] = s0 * c1 * c2 + c0 * s2;
727 (*this)[1][1] = -s0 * c1 * s2 + c0 * c2;
728 (*this)[1][2] = s0 * s1;
729 (*this)[2][0] = -s1 * c2;
730 (*this)[2][1] = s1 * s2;
746 double c0, c1, c2, s0, s1, s2;
755 (*this)[0][0] = c1 * c2;
756 (*this)[0][1] = -c1 * s2;
758 (*this)[1][0] = c0 * s2 + s0 * s1 * c2;
759 (*this)[1][1] = c0 * c2 - s0 * s1 * s2;
760 (*this)[1][2] = -s0 * c1;
761 (*this)[2][0] = -c0 * s1 * c2 + s0 * s2;
762 (*this)[2][1] = c0 * s1 * s2 + c2 * s0;
763 (*this)[2][2] = c0 * c1;
776 double c0, c1, c2, s0, s1, s2;
785 (*this)[0][0] = c0 * c1;
786 (*this)[0][1] = c0 * s1 * s2 - s0 * c2;
787 (*this)[0][2] = c0 * s1 * c2 + s0 * s2;
789 (*this)[1][0] = s0 * c1;
790 (*this)[1][1] = s0 * s1 * s2 + c0 * c2;
791 (*this)[1][2] = s0 * s1 * c2 - c0 * s2;
794 (*this)[2][1] = c1 * s2;
795 (*this)[2][2] = c1 * c2;
820 (*this)[0][0] = a * a + b * b - c * c - d * d;
821 (*this)[0][1] = 2 * b * c - 2 * a * d;
822 (*this)[0][2] = 2 * a * c + 2 * b * d;
824 (*this)[1][0] = 2 * a * d + 2 * b * c;
825 (*this)[1][1] = a * a - b * b + c * c - d * d;
826 (*this)[1][2] = 2 * c * d - 2 * a * b;
828 (*this)[2][0] = 2 * b * d - 2 * a * c;
829 (*this)[2][1] = 2 * a * b + 2 * c * d;
830 (*this)[2][2] = a * a - b * b - c * c + d * d;
841 unsigned int Rrow = R.
getRows();
842 unsigned int Rcol = R.
getCols();
844 for (
unsigned int i = 0; i < Rrow; i++)
845 for (
unsigned int j = 0; j < Rcol; j++)
846 C[i][j] = R[i][j] * x;
893 unsigned int nb_rows =
getRows();
895 for (
unsigned int i = 0; i < nb_rows; i++)
896 c[i] = (*
this)[i][j];
913 for (
size_t i = 0; i < vec_M.size(); i++) {
914 R = vec_M[i].getRotationMatrix();
917 meanR /=
static_cast<double>(vec_M.size());
923 double det = sv[0] * sv[1] * sv[2];
930 D[0][0] = D[1][1] = 1.0;
932 meanR = U * D * V.
t();
951 for (
size_t i = 0; i < vec_R.size(); i++) {
954 meanR /=
static_cast<double>(vec_R.size());
960 double det = sv[0] * sv[1] * sv[2];
967 D[0][0] = D[1][1] = 1.0;
969 meanR = U * D * V.
t();
988 double det = R.
det();
1008#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a generic 2D array used as base class for matrices and vectors.
unsigned int getCols() const
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< double > &A)
unsigned int size() const
unsigned int getRows() const
Implementation of column vector and the associated operations.
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 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.
vpColVector operator*(const double &x, const vpColVector &v)