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>
105 buildFrom(p[0], p[1], p[2], p[3], p[4], p[5]);
154 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
196 if (list.size() == 12) {
197 std::copy(list.begin(), list.end(),
data);
202 }
else if (list.size() == 16) {
203 std::copy(list.begin(), list.end(),
data);
204 for (
size_t i = 12; i < 15; i++) {
205 if (std::fabs(
data[i]) > std::numeric_limits<double>::epsilon()) {
207 "Cannot initialize homogeneous matrix. "
208 "List element %d (%f) should be 0.",
212 if (std::fabs(
data[15] - 1.) > std::numeric_limits<double>::epsilon()) {
214 "Cannot initialize homogeneous matrix. "
215 "List element 15 (%f) should be 1.",
220 "Cannot initialize homogeneous matrix from a list (%d elements) that has not 12 or 16 elements",
242 "Homogeneous matrix initialization fails since its elements are not valid (rotation part or last row)"));
403 if (v.size() != 12 && v.size() != 16) {
407 for (
unsigned int i = 0; i < 12; i++)
408 this->
data[i] = (
double)v[i];
453 if (v.size() != 12 && v.size() != 16) {
457 for (
unsigned int i = 0; i < 12; i++)
458 this->
data[i] = v[i];
468 for (
int i = 0; i < 4; i++) {
469 for (
int j = 0; j < 4; j++) {
535 (*this) = (*this) * M;
550 "Cannot multiply a (4x4) homogeneous matrix by a "
551 "(%dx1) column vector",
558 for (
unsigned int j = 0; j < 4; j++) {
559 for (
unsigned int i = 0; i < 4; i++) {
589 v1[0] = (*this)[0][0] * v[0] + (*this)[0][1] * v[1] + (*this)[0][2] * v[2] + (*this)[0][3] * v[3];
590 v1[1] = (*this)[1][0] * v[0] + (*this)[1][1] * v[1] + (*this)[1][2] * v[2] + (*this)[1][3] * v[3];
591 v1[2] = (*this)[2][0] * v[0] + (*this)[2][1] * v[1] + (*this)[2][2] * v[2] + (*this)[2][3] * v[3];
592 v1[3] = (*this)[3][0] * v[0] + (*this)[3][1] * v[1] + (*this)[3][2] * v[2] + (*this)[3][3] * v[3];
623 t_out[0] = (*this)[0][0] * t[0] + (*this)[0][1] * t[1] + (*this)[0][2] * t[2] + (*this)[0][3];
624 t_out[1] = (*this)[1][0] * t[0] + (*this)[1][1] * t[1] + (*this)[1][2] * t[2] + (*this)[1][3];
625 t_out[2] = (*this)[2][0] * t[0] + (*this)[2][1] * t[1] + (*this)[2][2] * t[2] + (*this)[2][3];
726 "Cannot set homogenous matrix out of bounds. It has only %d elements while you try to initialize "
747 const double epsilon = std::numeric_limits<double>::epsilon();
758 for (
unsigned int i = 0; i <
size(); i++) {
772 for (
unsigned int i = 0; i < 3; i++)
773 for (
unsigned int j = 0; j < 3; j++)
774 R[i][j] = (*
this)[i][j];
782 t[0] = (*this)[0][3];
783 t[1] = (*this)[1][3];
784 t[2] = (*this)[2][3];
811 for (
unsigned int i = 0; i < 3; i++)
812 for (
unsigned int j = 0; j < 3; j++)
813 (*
this)[i][j] = R[i][j];
833 (*this)[0][3] = t[0];
834 (*this)[1][3] = t[1];
835 (*this)[2][3] = t[2];
888 (*this)[0][1] = (*this)[0][2] = (*this)[0][3] = 0;
889 (*this)[1][0] = (*this)[1][2] = (*this)[1][3] = 0;
890 (*this)[2][0] = (*this)[2][1] = (*this)[2][3] = 0;
891 (*this)[3][0] = (*this)[3][1] = (*this)[3][2] = 0;
962 for (
unsigned int i = 0; i < 4; i++) {
963 for (
unsigned int j = 0; j < 4; j++) {
1005 for (
unsigned int i = 0; i < 12; i++)
1006 M[i] = (
float)(this->
data[i]);
1016 for (
unsigned int i = 0; i < 12; i++)
1017 M[i] = this->
data[i];
1083 unsigned int nb_rows =
getRows();
1085 for (
unsigned int i = 0; i < nb_rows; i++)
1086 c[i] = (*
this)[i][j];
1098 const double N =
static_cast<double>(p.size());
1102 for (
size_t i = 0; i < p.size(); i++) {
1103 for (
unsigned int j = 0; j < 3; j++) {
1104 p_bar[j] += p.at(i).oP[j];
1105 q_bar[j] += q.at(i).oP[j];
1109 for (
unsigned int j = 0; j < 3; j++) {
1114 vpMatrix pc(
static_cast<unsigned int>(p.size()), 3);
1115 vpMatrix qc(
static_cast<unsigned int>(q.size()), 3);
1117 for (
unsigned int i = 0; i < static_cast<unsigned int>(p.size()); i++) {
1118 for (
unsigned int j = 0; j < 3; j++) {
1119 pc[i][j] = p.at(i).oP[j] - p_bar[j];
1120 qc[i][j] = q.at(i).oP[j] - q_bar[j];
1158 for (
size_t i = 0; i < vec_M.size(); i++) {
1159 R = vec_M[i].getRotationMatrix();
1163 meanR /=
static_cast<double>(vec_M.size());
1164 meanT /=
static_cast<double>(vec_M.size());
1170 double det = sv[0] * sv[1] * sv[2];
1176 D[0][0] = D[1][1] = 1.0;
1178 meanR = U * D * V.
t();
1188 #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
void orthogonalizeRotation()
static vpHomogeneousMatrix compute3d3dTransformation(const std::vector< vpPoint > &p, const std::vector< vpPoint > &q)
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)
static bool isNaN(double value)
static bool equal(double x, double y, double s=0.001)
static bool nul(double x, double s=0.001)
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
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.