45#include <visp3/core/vpDebug.h>
46#include <visp3/core/vpException.h>
47#include <visp3/core/vpHomogeneousMatrix.h>
48#include <visp3/core/vpMatrix.h>
49#include <visp3/core/vpPoint.h>
50#include <visp3/core/vpQuaternionVector.h>
102 buildFrom(p[0], p[1], p[2], p[3], p[4], p[5]);
151#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
192 if (list.size() == 12) {
193 std::copy(list.begin(), list.end(),
data);
199 else if (list.size() == 16) {
200 std::copy(list.begin(), list.end(),
data);
201 for (
size_t i = 12; i < 15; i++) {
202 if (std::fabs(
data[i]) > std::numeric_limits<double>::epsilon()) {
204 "List element %d (%f) should be 0.", i,
data[i]));
207 if (std::fabs(
data[15] - 1.) > std::numeric_limits<double>::epsilon()) {
209 "List element 15 (%f) should be 1.",
data[15]));
229 data[0] = R[0][0];
data[1] = R[0][1];
data[2] = R[0][2];
230 data[4] = R[1][0];
data[5] = R[1][1];
data[6] = R[1][2];
231 data[8] = R[2][0];
data[9] = R[2][1];
data[10] = R[2][2];
394 if (v.size() != 12 && v.size() != 16) {
398 for (
unsigned int i = 0; i < 12; i++)
399 this->
data[i] = (
double)v[i];
444 if (v.size() != 12 && v.size() != 16) {
448 for (
unsigned int i = 0; i < 12; i++)
449 this->
data[i] = v[i];
459 for (
int i = 0; i < 4; i++) {
460 for (
int j = 0; j < 4; j++) {
526 (*this) = (*this) * M;
541 "Cannot multiply a (4x4) homogeneous matrix by a "
542 "(%dx1) column vector",
549 for (
unsigned int j = 0; j < 4; j++) {
550 for (
unsigned int i = 0; i < 4; i++) {
580 v1[0] = (*this)[0][0] * v[0] + (*this)[0][1] * v[1] + (*this)[0][2] * v[2] + (*this)[0][3] * v[3];
581 v1[1] = (*this)[1][0] * v[0] + (*this)[1][1] * v[1] + (*this)[1][2] * v[2] + (*this)[1][3] * v[3];
582 v1[2] = (*this)[2][0] * v[0] + (*this)[2][1] * v[1] + (*this)[2][2] * v[2] + (*this)[2][3] * v[3];
583 v1[3] = (*this)[3][0] * v[0] + (*this)[3][1] * v[1] + (*this)[3][2] * v[2] + (*this)[3][3] * v[3];
614 t_out[0] = (*this)[0][0] * t[0] + (*this)[0][1] * t[1] + (*this)[0][2] * t[2] + (*this)[0][3];
615 t_out[1] = (*this)[1][0] * t[0] + (*this)[1][1] * t[1] + (*this)[1][2] * t[2] + (*this)[1][3];
616 t_out[2] = (*this)[2][0] * t[0] + (*this)[2][1] * t[1] + (*this)[2][2] * t[2] + (*this)[2][3];
742 for (
unsigned int i = 0; i < 3; i++)
743 for (
unsigned int j = 0; j < 3; j++)
744 R[i][j] = (*
this)[i][j];
752 t[0] = (*this)[0][3];
753 t[1] = (*this)[1][3];
754 t[2] = (*this)[2][3];
781 for (
unsigned int i = 0; i < 3; i++)
782 for (
unsigned int j = 0; j < 3; j++)
783 (*
this)[i][j] = R[i][j];
803 (*this)[0][3] = t[0];
804 (*this)[1][3] = t[1];
805 (*this)[2][3] = t[2];
858 (*this)[0][1] = (*this)[0][2] = (*this)[0][3] = 0;
859 (*this)[1][0] = (*this)[1][2] = (*this)[1][3] = 0;
860 (*this)[2][0] = (*this)[2][1] = (*this)[2][3] = 0;
861 (*this)[3][0] = (*this)[3][1] = (*this)[3][2] = 0;
932 for (
unsigned int i = 0; i < 4; i++) {
933 for (
unsigned int j = 0; j < 4; j++) {
956 for (
unsigned int i = 0; i < 12; i++)
957 M[i] = (
float)(this->
data[i]);
967 for (
unsigned int i = 0; i < 12; i++)
968 M[i] = this->
data[i];
1034 unsigned int nb_rows =
getRows();
1036 for (
unsigned int i = 0; i < nb_rows; i++)
1037 c[i] = (*
this)[i][j];
1055 for (
size_t i = 0; i < vec_M.size(); i++) {
1056 R = vec_M[i].getRotationMatrix();
1060 meanR /=
static_cast<double>(vec_M.size());
1061 meanT /=
static_cast<double>(vec_M.size());
1067 double det = sv[0]*sv[1]*sv[2];
1074 D[0][0] = D[1][1] = 1.0; D[2][2] = -1;
1075 meanR = U * D * V.
t();
1085#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 size() const
Return the number of elements of the 2D array.
unsigned int getRows() const
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.
vpThetaUVector getThetaUVector() const
void load(std::ifstream &f)
vp_deprecated void setIdentity()
void print() const
Print the matrix as a pose vector .
vpRotationMatrix getRotationMatrix() const
bool isAnHomogeneousMatrix(double threshold=1e-6) const
vpHomogeneousMatrix & operator*=(const vpHomogeneousMatrix &M)
vpHomogeneousMatrix inverse() const
static vpHomogeneousMatrix mean(const std::vector< vpHomogeneousMatrix > &vec_M)
vpTranslationVector getTranslationVector() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void convert(std::vector< float > &M)
void extract(vpRotationMatrix &R) const
vpColVector getCol(unsigned int j) const
vpHomogeneousMatrix & operator<<(double val)
void insert(const vpRotationMatrix &R)
vpHomogeneousMatrix operator*(const vpHomogeneousMatrix &M) const
void save(std::ofstream &f) const
vpHomogeneousMatrix & operator=(const vpHomogeneousMatrix &M)
vpHomogeneousMatrix & operator,(double val)
Implementation of a matrix and operations on matrices.
void svd(vpColVector &w, vpMatrix &V)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
void set_W(double cW)
Set the point cW coordinate in the camera frame.
void set_oW(double oW)
Set the point oW coordinate in the object frame.
double get_Y() const
Get the point cY coordinate in the camera frame.
void set_oY(double oY)
Set the point oY coordinate in the object frame.
void set_X(double cX)
Set the point cX coordinate in the camera frame.
double get_W() const
Get the point cW coordinate in the camera frame.
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
double get_Z() const
Get the point cZ coordinate in the camera frame.
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
void set_oX(double oX)
Set the point oX coordinate in the object frame.
double get_X() const
Get the point cX coordinate in the camera frame.
Implementation of a pose vector and operations on poses.
Implementation of a rotation vector as quaternion angle minimal representation.
vpQuaternionVector buildFrom(const double qx, const double qy, const double qz, const double qw)
Implementation of a rotation matrix and operations on such kind of matrices.
bool isARotationMatrix(double threshold=1e-6) const
vpRotationMatrix t() const
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.