41 #include <visp3/core/vpException.h>
42 #include <visp3/core/vpHomogeneousMatrix.h>
43 #include <visp3/core/vpMatrix.h>
44 #include <visp3/core/vpPoint.h>
45 #include <visp3/core/vpQuaternionVector.h>
91 const unsigned int index_3 = 3;
94 (*this)[index_3][index_3] = 1.;
102 const unsigned int index_0 = 0;
103 const unsigned int index_1 = 1;
104 const unsigned int index_2 = 2;
105 const unsigned int index_3 = 3;
106 const unsigned int index_4 = 4;
107 const unsigned int index_5 = 5;
108 build(p[index_0], p[index_1], p[index_2], p[index_3], p[index_4], p[index_5]);
109 (*this)[index_3][index_3] = 1.;
161 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
205 if (list.size() == 12) {
206 std::copy(list.begin(), list.end(),
data);
212 else if (list.size() == 16) {
213 std::copy(list.begin(), list.end(),
data);
214 for (
size_t i = 12; i < 15; ++i) {
215 if (std::fabs(
data[i]) > std::numeric_limits<double>::epsilon()) {
217 "Cannot initialize homogeneous matrix. "
218 "List element %d (%f) should be 0.",
222 if (std::fabs(
data[15] - 1.) > std::numeric_limits<double>::epsilon()) {
224 "Cannot initialize homogeneous matrix. "
225 "List element 15 (%f) should be 1.",
231 "Cannot initialize homogeneous matrix from a list (%d elements) that has not 12 or 16 elements",
237 const unsigned int index_0 = 0;
238 const unsigned int index_1 = 1;
239 const unsigned int index_2 = 2;
240 const unsigned int index_4 = 4;
241 const unsigned int index_5 = 5;
242 const unsigned int index_6 = 6;
243 const unsigned int index_8 = 8;
244 const unsigned int index_9 = 9;
245 const unsigned int index_10 = 10;
250 data[index_0] = R[index_0][index_0];
251 data[index_1] = R[index_0][index_1];
252 data[index_2] = R[index_0][index_2];
253 data[index_4] = R[index_1][index_0];
254 data[index_5] = R[index_1][index_1];
255 data[index_6] = R[index_1][index_2];
256 data[index_8] = R[index_2][index_0];
257 data[index_9] = R[index_2][index_1];
258 data[index_10] = R[index_2][index_2];
263 "Homogeneous matrix initialization fails since its elements are not valid (rotation part or last row)"));
326 build(tx, ty, tz, tux, tuy, tuz);
330 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
378 build(tx, ty, tz, tux, tuy, tuz);
510 const unsigned int index_0 = 0;
511 const unsigned int index_1 = 1;
512 const unsigned int index_2 = 2;
513 const unsigned int index_3 = 3;
514 const unsigned int index_4 = 4;
515 const unsigned int index_5 = 5;
596 if ((v.size() != 12) && (v.size() != 16)) {
600 for (
unsigned int i = 0; i < 12; ++i) {
601 this->
data[i] =
static_cast<double>(v[i]);
652 if ((v.size() != 12) && (v.size() != 16)) {
656 for (
unsigned int i = 0; i < 12; ++i) {
657 this->
data[i] = v[i];
669 for (
int i = 0; i < 4; ++i) {
670 for (
int j = 0; j < 4; ++j) {
744 (*this) = (*this) * M;
757 const unsigned int val_4 = 4;
760 "Cannot multiply a (4x4) homogeneous matrix by a "
761 "(%dx1) column vector",
768 for (
unsigned int j = 0; j < val_4; ++j) {
769 for (
unsigned int i = 0; i < val_4; ++i) {
794 const unsigned int index_0 = 0;
795 const unsigned int index_1 = 1;
796 const unsigned int index_2 = 2;
797 const unsigned int index_3 = 3;
799 v[index_0] = bP.
get_X();
800 v[index_1] = bP.
get_Y();
801 v[index_2] = bP.
get_Z();
802 v[index_3] = bP.
get_W();
804 v1[index_0] = ((*this)[index_0][0] * v[0]) + ((*
this)[index_0][1] * v[1]) + ((*
this)[index_0][index_2] * v[index_2]) + ((*
this)[index_0][index_3] * v[index_3]);
805 v1[index_1] = ((*this)[index_1][0] * v[0]) + ((*
this)[index_1][1] * v[1]) + ((*
this)[index_1][index_2] * v[index_2]) + ((*
this)[index_1][index_3] * v[index_3]);
806 v1[index_2] = ((*this)[index_2][0] * v[0]) + ((*
this)[index_2][1] * v[1]) + ((*
this)[index_2][index_2] * v[index_2]) + ((*
this)[index_2][index_3] * v[index_3]);
807 v1[index_3] = ((*this)[index_3][0] * v[0]) + ((*
this)[index_3][1] * v[1]) + ((*
this)[index_3][index_2] * v[index_2]) + ((*
this)[index_3][index_3] * v[index_3]);
812 aP.
set_X(v1[index_0]);
813 aP.
set_Y(v1[index_1]);
814 aP.
set_Z(v1[index_2]);
815 aP.
set_W(v1[index_3]);
838 const unsigned int index_0 = 0;
839 const unsigned int index_1 = 1;
840 const unsigned int index_2 = 2;
841 const unsigned int index_3 = 3;
842 t_out[index_0] = (((*this)[index_0][0] *
t[0]) + ((*
this)[index_0][1] *
t[1]) + ((*
this)[index_0][index_2] *
t[index_2])) + (*this)[index_0][index_3];
843 t_out[index_1] = (((*this)[index_1][0] *
t[0]) + ((*
this)[index_1][1] *
t[1]) + ((*
this)[index_1][index_2] *
t[index_2])) + (*this)[index_1][index_3];
844 t_out[index_2] = (((*this)[index_2][0] *
t[0]) + ((*
this)[index_2][1] *
t[1]) + ((*
this)[index_2][index_2] *
t[index_2])) + (*this)[index_2][index_3];
865 return (
vpHomogeneousMatrix((*this).getTranslationVector(), (*this).getRotationMatrix() * R));
972 "Cannot set homogenous matrix out of bounds. It has only %d elements while you try to initialize "
993 const unsigned int index_0 = 0;
994 const unsigned int index_1 = 1;
995 const unsigned int index_2 = 2;
996 const unsigned int index_3 = 3;
997 const double epsilon = std::numeric_limits<double>::epsilon();
998 bool isLastRowOK =
vpMath::nul((*
this)[index_3][index_0], epsilon) &&
vpMath::nul((*
this)[index_3][index_1], epsilon) &&
1009 unsigned int l_size =
size();
1010 for (
unsigned int i = 0; i < l_size; ++i) {
1024 const unsigned int val_3 = 3;
1025 for (
unsigned int i = 0; i < val_3; ++i) {
1026 for (
unsigned int j = 0; j < val_3; ++j) {
1027 R[i][j] = (*this)[i][j];
1037 const unsigned int index_0 = 0;
1038 const unsigned int index_1 = 1;
1039 const unsigned int index_2 = 2;
1040 const unsigned int index_3 = 3;
1041 t[index_0] = (*this)[index_0][index_3];
1042 t[index_1] = (*this)[index_1][index_3];
1043 t[index_2] = (*this)[index_2][index_3];
1070 const unsigned int val_3 = 3;
1071 for (
unsigned int i = 0; i < val_3; ++i) {
1072 for (
unsigned int j = 0; j < val_3; ++j) {
1073 (*this)[i][j] = R[i][j];
1095 const unsigned int index_0 = 0;
1096 const unsigned int index_1 = 1;
1097 const unsigned int index_2 = 2;
1098 const unsigned int index_3 = 3;
1099 (*this)[index_0][index_3] =
t[index_0];
1100 (*this)[index_1][index_3] =
t[index_1];
1101 (*this)[index_2][index_3] =
t[index_2];
1149 const unsigned int index_0 = 0;
1150 const unsigned int index_1 = 1;
1151 const unsigned int index_2 = 2;
1152 const unsigned int index_3 = 3;
1153 (*this)[index_0][index_0] = 1;
1154 (*this)[index_1][index_1] = 1;
1155 (*this)[index_2][index_2] = 1;
1156 (*this)[index_3][index_3] = 1;
1158 (*this)[index_0][index_1] = 0;
1159 (*this)[index_0][index_2] = 0;
1160 (*this)[index_0][index_3] = 0;
1161 (*this)[index_1][index_0] = 0;
1162 (*this)[index_1][index_2] = 0;
1163 (*this)[index_1][index_3] = 0;
1164 (*this)[index_2][index_0] = 0;
1165 (*this)[index_2][index_1] = 0;
1166 (*this)[index_2][index_3] = 0;
1167 (*this)[index_3][index_0] = 0;
1168 (*this)[index_3][index_1] = 0;
1169 (*this)[index_3][index_2] = 0;
1201 f.open(filename.c_str());
1209 const unsigned int val_4 = 4;
1210 for (
unsigned int i = 0; i < val_4; ++i) {
1211 for (
unsigned int j = 0; j < val_4; ++j) {
1224 f.open(filename.c_str());
1237 const unsigned int index_0 = 0;
1238 const unsigned int index_1 = 1;
1239 const unsigned int index_2 = 2;
1240 const unsigned int index_4 = 4;
1241 const unsigned int index_5 = 5;
1242 const unsigned int index_6 = 6;
1243 const unsigned int index_8 = 8;
1244 const unsigned int index_9 = 9;
1245 const unsigned int index_10 = 10;
1247 data[index_0] = R[index_0][index_0];
1248 data[index_1] = R[index_0][index_1];
1249 data[index_2] = R[index_0][index_2];
1250 data[index_4] = R[index_1][index_0];
1251 data[index_5] = R[index_1][index_1];
1252 data[index_6] = R[index_1][index_2];
1253 data[index_8] = R[index_2][index_0];
1254 data[index_9] = R[index_2][index_1];
1255 data[index_10] = R[index_2][index_2];
1272 for (
unsigned int i = 0; i < 12; ++i) {
1273 M[i] =
static_cast<float>(this->
data[i]);
1284 for (
unsigned int i = 0; i < 12; ++i) {
1285 M[i] = this->
data[i];
1357 unsigned int nb_rows =
getRows();
1359 for (
unsigned int i = 0; i < nb_rows; ++i) {
1360 c[i] = (*this)[i][j];
1373 const double N =
static_cast<double>(p.size());
1377 size_t p_size = p.size();
1378 const unsigned int val_3 = 3;
1379 for (
size_t i = 0; i < p_size; ++i) {
1380 for (
unsigned int j = 0; j < val_3; ++j) {
1381 p_bar[j] += p.at(i).oP[j];
1382 q_bar[j] += q.at(i).oP[j];
1386 for (
unsigned int j = 0; j < val_3; ++j) {
1391 vpMatrix pc(
static_cast<unsigned int>(p.size()), 3);
1392 vpMatrix qc(
static_cast<unsigned int>(q.size()), 3);
1394 for (
unsigned int i = 0; i < static_cast<unsigned int>(p_size); ++i) {
1395 for (
unsigned int j = 0; j < val_3; ++j) {
1396 pc[i][j] = p.at(i).oP[j] - p_bar[j];
1397 qc[i][j] = q.at(i).oP[j] - q_bar[j];
1409 const unsigned int index_0 = 0;
1410 const unsigned int index_1 = 1;
1411 const unsigned int index_2 = 2;
1412 Vt[index_2][index_0] *= -1.;
1413 Vt[index_2][index_1] *= -1.;
1414 Vt[index_2][index_2] *= -1.;
1438 const unsigned int index_0 = 0;
1439 const unsigned int index_1 = 1;
1440 const unsigned int index_2 = 2;
1441 size_t vec_m_size = vec_M.
size();
1442 for (
size_t i = 0; i < vec_m_size; ++i) {
1443 R = vec_M[i].getRotationMatrix();
1447 meanR /=
static_cast<double>(vec_M.size());
1448 meanT /=
static_cast<double>(vec_M.size());
1454 double det = sv[index_0] * sv[index_1] * sv[index_2];
1461 D[index_0][index_0] = 1.0;
1462 D[index_1][index_1] = 1.0;
1463 D[index_2][index_2] = -1.;
1464 meanR = U * D * V.
t();
1474 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1486 #ifdef VISP_HAVE_NLOHMANN_JSON
1488 #include <visp3/core/vpJsonParsing.h>
1489 void vpHomogeneousMatrix::convert_to_json(nlohmann::json &j)
const
1496 void vpHomogeneousMatrix::parse_json(
const nlohmann::json &j)
1498 #ifdef ENABLE_VISP_NAMESPACE
1502 if (j.is_object() && j.contains(
"type")) {
1503 const bool converted = convertFromTypeAndBuildFrom<vpHomogeneousMatrix, vpPoseVector>(j, *
this);
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.
vpArray2D< double > t() const
Compute the transpose of the array.
unsigned int getRows() const
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
@ dimensionError
Bad dimension.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpThetaUVector getThetaUVector() const
void load(std::ifstream &f)
friend void from_json(const nlohmann::json &j, vpHomogeneousMatrix &T)
VP_DEPRECATED void setIdentity()
static const std::string jsonTypeName
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 & build(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix inverse() const
static vpHomogeneousMatrix mean(const std::vector< vpHomogeneousMatrix > &vec_M)
vpTranslationVector getTranslationVector() const
VP_DEPRECATED 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)
friend void to_json(nlohmann::json &j, const vpHomogeneousMatrix &T)
vpHomogeneousMatrix & operator,(double val)
static bool isNaN(double value)
static bool equal(double x, double y, double threshold=0.001)
static bool nul(double x, double threshold=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 & build(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 & build(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.