Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpHomogeneousMatrix.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Homogeneous matrix.
33 *
34*****************************************************************************/
35
42#include <visp3/core/vpDebug.h>
43#include <visp3/core/vpException.h>
44#include <visp3/core/vpHomogeneousMatrix.h>
45#include <visp3/core/vpMatrix.h>
46#include <visp3/core/vpPoint.h>
47#include <visp3/core/vpQuaternionVector.h>
48
54 : vpArray2D<double>(4, 4)
55{
56 buildFrom(t, q);
57 (*this)[3][3] = 1.;
58}
59
63vpHomogeneousMatrix::vpHomogeneousMatrix() : vpArray2D<double>(4, 4), m_index(0) { eye(); }
64
70{
71 *this = M;
72}
73
79 : vpArray2D<double>(4, 4), m_index(0)
80{
81 buildFrom(t, tu);
82 (*this)[3][3] = 1.;
83}
84
90 : vpArray2D<double>(4, 4), m_index(0)
91{
92 insert(R);
93 insert(t);
94 (*this)[3][3] = 1.;
95}
96
101{
102 buildFrom(p[0], p[1], p[2], p[3], p[4], p[5]);
103 (*this)[3][3] = 1.;
104}
105
145vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<float> &v) : vpArray2D<double>(4, 4), m_index(0)
146{
147 buildFrom(v);
148 (*this)[3][3] = 1.;
149}
150
151#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
190vpHomogeneousMatrix::vpHomogeneousMatrix(const std::initializer_list<double> &list)
191 : vpArray2D<double>(4, 4), m_index(0)
192{
193 if (list.size() == 12) {
194 std::copy(list.begin(), list.end(), data);
195 data[12] = 0.;
196 data[13] = 0.;
197 data[14] = 0.;
198 data[15] = 1.;
199 }
200 else if (list.size() == 16) {
201 std::copy(list.begin(), list.end(), data);
202 for (size_t i = 12; i < 15; i++) {
203 if (std::fabs(data[i]) > std::numeric_limits<double>::epsilon()) {
205 "Cannot initialize homogeneous matrix. "
206 "List element %d (%f) should be 0.",
207 i, data[i]));
208 }
209 }
210 if (std::fabs(data[15] - 1.) > std::numeric_limits<double>::epsilon()) {
212 "Cannot initialize homogeneous matrix. "
213 "List element 15 (%f) should be 1.",
214 data[15]));
215 }
216 }
217 else {
219 "Cannot initialize homogeneous matrix from a list (%d elements) that has not 12 or 16 elements",
220 list.size()));
221 }
222
223 if (!isAnHomogeneousMatrix()) {
224 if (isAnHomogeneousMatrix(1e-3)) {
225 // re-orthogonalize rotation matrix since the input is close to a valid rotation matrix
226 vpRotationMatrix R(*this);
227 R.orthogonalize();
228
229 data[0] = R[0][0];
230 data[1] = R[0][1];
231 data[2] = R[0][2];
232 data[4] = R[1][0];
233 data[5] = R[1][1];
234 data[6] = R[1][2];
235 data[8] = R[2][0];
236 data[9] = R[2][1];
237 data[10] = R[2][2];
238 }
239 else {
240 throw(vpException(
242 "Homogeneous matrix initialization fails since its elements are not valid (rotation part or last row)"));
243 }
244 }
245}
246#endif
247
287vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<double> &v) : vpArray2D<double>(4, 4), m_index(0)
288{
289 buildFrom(v);
290 (*this)[3][3] = 1.;
291}
292
298vpHomogeneousMatrix::vpHomogeneousMatrix(double tx, double ty, double tz, double tux, double tuy, double tuz)
299 : vpArray2D<double>(4, 4), m_index(0)
300{
301 buildFrom(tx, ty, tz, tux, tuy, tuz);
302 (*this)[3][3] = 1.;
303}
304
310{
311 insert(tu);
312 insert(t);
313}
314
320{
321 insert(R);
322 insert(t);
323}
324
329{
330 vpTranslationVector tv(p[0], p[1], p[2]);
331 vpThetaUVector tu(p[3], p[4], p[5]);
332
333 insert(tu);
334 insert(tv);
335}
336
346
352void vpHomogeneousMatrix::buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
353{
354 vpRotationMatrix R(tux, tuy, tuz);
355 vpTranslationVector t(tx, ty, tz);
356
357 insert(R);
358 insert(t);
359}
360
401void vpHomogeneousMatrix::buildFrom(const std::vector<float> &v)
402{
403 if (v.size() != 12 && v.size() != 16) {
404 throw(vpException(vpException::dimensionError, "Cannot convert std::vector<float> to vpHomogeneousMatrix"));
405 }
406
407 for (unsigned int i = 0; i < 12; i++)
408 this->data[i] = (double)v[i];
409}
410
451void vpHomogeneousMatrix::buildFrom(const std::vector<double> &v)
452{
453 if (v.size() != 12 && v.size() != 16) {
454 throw(vpException(vpException::dimensionError, "Cannot convert std::vector<double> to vpHomogeneousMatrix"));
455 }
456
457 for (unsigned int i = 0; i < 12; i++)
458 this->data[i] = v[i];
459}
460
467{
468 for (int i = 0; i < 4; i++) {
469 for (int j = 0; j < 4; j++) {
470 rowPtrs[i][j] = M.rowPtrs[i][j];
471 }
472 }
473 return *this;
474}
475
494{
496
497 vpRotationMatrix R1, R2, R;
498 vpTranslationVector T1, T2, T;
499
500 extract(T1);
501 M.extract(T2);
502
503 extract(R1);
504 M.extract(R2);
505
506 R = R1 * R2;
507
508 T = R1 * T2 + T1;
509
510 p.insert(T);
511 p.insert(R);
512
513 return p;
514}
515
534{
535 (*this) = (*this) * M;
536 return (*this);
537}
538
547{
548 if (v.getRows() != 4) {
550 "Cannot multiply a (4x4) homogeneous matrix by a "
551 "(%dx1) column vector",
552 v.getRows()));
553 }
555
556 p = 0.0;
557
558 for (unsigned int j = 0; j < 4; j++) {
559 for (unsigned int i = 0; i < 4; i++) {
560 p[i] += rowPtrs[i][j] * v[j];
561 }
562 }
563
564 return p;
565}
566
579{
580 vpPoint aP;
581
582 vpColVector v(4), v1(4);
583
584 v[0] = bP.get_X();
585 v[1] = bP.get_Y();
586 v[2] = bP.get_Z();
587 v[3] = bP.get_W();
588
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];
593
594 v1 /= v1[3];
595
596 // v1 = M*v ;
597 aP.set_X(v1[0]);
598 aP.set_Y(v1[1]);
599 aP.set_Z(v1[2]);
600 aP.set_W(v1[3]);
601
602 aP.set_oX(v1[0]);
603 aP.set_oY(v1[1]);
604 aP.set_oZ(v1[2]);
605 aP.set_oW(v1[3]);
606
607 return aP;
608}
609
621{
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];
626
627 return t_out;
628}
629
645{
646 return (vpHomogeneousMatrix((*this).getTranslationVector(), (*this).getRotationMatrix() * R));
647}
648
692{
693 m_index = 0;
694 data[m_index] = val;
695 return *this;
696}
697
741{
742 m_index++;
743 if (m_index >= size()) {
745 "Cannot set homogenous matrix out of bounds. It has only %d elements while you try to initialize "
746 "with %d elements",
747 size(), m_index + 1));
748 }
749 data[m_index] = val;
750 return *this;
751}
752
753/*********************************************************************/
754
762{
764 extract(R);
765
766 const double epsilon = std::numeric_limits<double>::epsilon();
767 return R.isARotationMatrix(threshold) && vpMath::nul((*this)[3][0], epsilon) && vpMath::nul((*this)[3][1], epsilon) &&
768 vpMath::nul((*this)[3][2], epsilon) && vpMath::equal((*this)[3][3], 1.0, epsilon);
769}
770
776{
777 for (unsigned int i = 0; i < size(); i++) {
778 if (vpMath::isNaN(data[i])) {
779 return false;
780 }
781 }
782 return true;
783}
784
790{
791 for (unsigned int i = 0; i < 3; i++)
792 for (unsigned int j = 0; j < 3; j++)
793 R[i][j] = (*this)[i][j];
794}
795
800{
801 t[0] = (*this)[0][3];
802 t[1] = (*this)[1][3];
803 t[2] = (*this)[2][3];
804}
809{
811 (*this).extract(R);
812 tu.buildFrom(R);
813}
814
819{
821 (*this).extract(R);
822 q.buildFrom(R);
823}
824
829{
830 for (unsigned int i = 0; i < 3; i++)
831 for (unsigned int j = 0; j < 3; j++)
832 (*this)[i][j] = R[i][j];
833}
834
842{
843 vpRotationMatrix R(tu);
844 insert(R);
845}
846
851{
852 (*this)[0][3] = t[0];
853 (*this)[1][3] = t[1];
854 (*this)[2][3] = t[2];
855}
856
864
880{
882
884 extract(R);
886 extract(T);
887
889 RtT = -(R.t() * T);
890
891 Mi.insert(R.t());
892 Mi.insert(RtT);
893
894 return Mi;
895}
896
901{
902 (*this)[0][0] = 1;
903 (*this)[1][1] = 1;
904 (*this)[2][2] = 1;
905 (*this)[3][3] = 1;
906
907 (*this)[0][1] = (*this)[0][2] = (*this)[0][3] = 0;
908 (*this)[1][0] = (*this)[1][2] = (*this)[1][3] = 0;
909 (*this)[2][0] = (*this)[2][1] = (*this)[2][3] = 0;
910 (*this)[3][0] = (*this)[3][1] = (*this)[3][2] = 0;
911}
912
928
951void vpHomogeneousMatrix::save(std::ofstream &f) const
952{
953 if (!f.fail()) {
954 f << *this;
955 }
956 else {
957 throw(vpException(vpException::ioError, "Cannot save homogeneous matrix: ostream not open"));
958 }
959}
960
979void vpHomogeneousMatrix::load(std::ifstream &f)
980{
981 if (!f.fail()) {
982 for (unsigned int i = 0; i < 4; i++) {
983 for (unsigned int j = 0; j < 4; j++) {
984 f >> (*this)[i][j];
985 }
986 }
987 }
988 else {
989 throw(vpException(vpException::ioError, "Cannot load homogeneous matrix: ifstream not open"));
990 }
991}
992
997{
998 vpRotationMatrix R(*this);
999 R.orthogonalize();
1000
1001 data[0] = R[0][0];
1002 data[1] = R[0][1];
1003 data[2] = R[0][2];
1004 data[4] = R[1][0];
1005 data[5] = R[1][1];
1006 data[6] = R[1][2];
1007 data[8] = R[2][0];
1008 data[9] = R[2][1];
1009 data[10] = R[2][2];
1010}
1011
1014{
1015 vpPoseVector r(*this);
1016 std::cout << r.t();
1017}
1018
1023void vpHomogeneousMatrix::convert(std::vector<float> &M)
1024{
1025 M.resize(12);
1026 for (unsigned int i = 0; i < 12; i++)
1027 M[i] = (float)(this->data[i]);
1028}
1029
1034void vpHomogeneousMatrix::convert(std::vector<double> &M)
1035{
1036 M.resize(12);
1037 for (unsigned int i = 0; i < 12; i++)
1038 M[i] = this->data[i];
1039}
1040
1045{
1047 this->extract(tr);
1048 return tr;
1049}
1050
1055{
1057 this->extract(R);
1058 return R;
1059}
1060
1066{
1067 vpThetaUVector tu;
1068 this->extract(tu);
1069 return tu;
1070}
1071
1101{
1102 if (j >= getCols())
1103 throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the homogeneous matrix"));
1104 unsigned int nb_rows = getRows();
1105 vpColVector c(nb_rows);
1106 for (unsigned int i = 0; i < nb_rows; i++)
1107 c[i] = (*this)[i][j];
1108 return c;
1109}
1110
1117vpHomogeneousMatrix vpHomogeneousMatrix::compute3d3dTransformation(const std::vector<vpPoint> &p, const std::vector<vpPoint> &q)
1118{
1119 const double N = static_cast<double>(p.size());
1120
1121 vpColVector p_bar(3, 0.0);
1122 vpColVector q_bar(3, 0.0);
1123 for (size_t i = 0; i < p.size(); i++) {
1124 for (unsigned int j = 0; j < 3; j++) {
1125 p_bar[j] += p.at(i).oP[j];
1126 q_bar[j] += q.at(i).oP[j];
1127 }
1128 }
1129
1130 for (unsigned int j = 0; j < 3; j++) {
1131 p_bar[j] /= N;
1132 q_bar[j] /= N;
1133 }
1134
1135 vpMatrix pc(static_cast<unsigned int>(p.size()), 3);
1136 vpMatrix qc(static_cast<unsigned int>(q.size()), 3);
1137
1138 for (unsigned int i = 0; i < static_cast<unsigned int>(p.size()); i++) {
1139 for (unsigned int j = 0; j < 3; j++) {
1140 pc[i][j] = p.at(i).oP[j] - p_bar[j];
1141 qc[i][j] = q.at(i).oP[j] - q_bar[j];
1142 }
1143 }
1144
1145 const vpMatrix pct_qc = pc.t() * qc;
1146 vpMatrix U = pct_qc, V;
1147 vpColVector W;
1148 U.svd(W, V);
1149
1150 vpMatrix Vt = V.t();
1151 vpMatrix R = U * Vt;
1152 if (R.det() < 0) {
1153 Vt[2][0] *= -1;
1154 Vt[2][1] *= -1;
1155 Vt[2][2] *= -1;
1156
1157 R = U * Vt;
1158 }
1159
1160 const vpColVector t = p_bar - R * q_bar;
1161
1163}
1164
1174vpHomogeneousMatrix vpHomogeneousMatrix::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
1175{
1176 vpMatrix meanR(3, 3);
1177 vpColVector meanT(3);
1179 for (size_t i = 0; i < vec_M.size(); i++) {
1180 R = vec_M[i].getRotationMatrix();
1181 meanR += (vpMatrix)R;
1182 meanT += (vpColVector)vec_M[i].getTranslationVector();
1183 }
1184 meanR /= static_cast<double>(vec_M.size());
1185 meanT /= static_cast<double>(vec_M.size());
1186
1187 // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
1188 vpMatrix M, U, V;
1189 vpColVector sv;
1190 meanR.pseudoInverse(M, sv, 1e-6, U, V);
1191 double det = sv[0] * sv[1] * sv[2];
1192 if (det > 0) {
1193 meanR = U * V.t();
1194 }
1195 else {
1196 vpMatrix D(3, 3);
1197 D = 0.0;
1198 D[0][0] = D[1][1] = 1.0;
1199 D[2][2] = -1;
1200 meanR = U * D * V.t();
1201 }
1202
1203 R = meanR;
1204
1205 vpTranslationVector t(meanT);
1207 return mean;
1208}
1209
1210#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1211
1219
1220#endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1221
1222#ifdef VISP_HAVE_NLOHMANN_JSON
1223const std::string vpHomogeneousMatrix::jsonTypeName = "vpHomogeneousMatrix";
1224#include <visp3/core/vpJsonParsing.h>
1225void vpHomogeneousMatrix::convert_to_json(nlohmann::json &j) const
1226{
1227 const vpArray2D<double> *asArray = (vpArray2D<double>*) this;
1228 to_json(j, *asArray);
1230}
1231
1232void vpHomogeneousMatrix::parse_json(const nlohmann::json &j)
1233{
1234 vpArray2D<double> *asArray = (vpArray2D<double>*) this;
1235 if (j.is_object() && j.contains("type")) { // Specific conversions
1236 const bool converted = convertFromTypeAndBuildFrom<vpHomogeneousMatrix, vpPoseVector>(j, *this);
1237 if (!converted) {
1238 from_json(j, *asArray);
1239 }
1240 }
1241 else { // Generic 2D array conversion
1242 from_json(j, *asArray);
1243 }
1244
1245 if (getCols() != 4 && getRows() != 4) {
1246 throw vpException(vpException::badValue, "From JSON, tried to read something that is not a 4x4 matrix");
1247 }
1248 if (!isAnHomogeneousMatrix()) {
1249 throw vpException(vpException::badValue, "From JSON read a non homogeneous matrix into a vpHomogeneousMatrix");
1250 }
1251}
1252#endif
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition vpArray2D.h:131
unsigned int getCols() const
Definition vpArray2D.h:280
double * data
Address of the first element of the data array.
Definition vpArray2D.h:144
double ** rowPtrs
Address of the first element of each rows.
Definition vpArray2D.h:138
unsigned int rowNum
Number of rows in the array.
Definition vpArray2D.h:134
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< double > &A)
Definition vpArray2D.h:529
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
vpArray2D< double > t() const
Compute the transpose of the array.
Definition vpArray2D.h:1059
unsigned int getRows() const
Definition vpArray2D.h:290
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ ioError
I/O error.
Definition vpException.h:79
@ badValue
Used to indicate that a value is not in the allowed range.
Definition vpException.h:85
@ dimensionError
Bad dimension.
Definition vpException.h:83
@ fatalError
Fatal error.
Definition vpException.h:84
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
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)
friend void to_json(nlohmann::json &j, const vpHomogeneousMatrix &cam)
void extract(vpRotationMatrix &R) const
vpColVector getCol(unsigned int j) const
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)
Definition vpMath.cpp:93
static bool equal(double x, double y, double threshold=0.001)
Definition vpMath.h:369
static bool nul(double x, double threshold=0.001)
Definition vpMath.h:360
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void svd(vpColVector &w, vpMatrix &V)
vpMatrix t() const
Definition vpMatrix.cpp:461
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 ...
Definition vpPoint.h:77
void set_W(double cW)
Set the point cW coordinate in the camera frame.
Definition vpPoint.cpp:496
void set_oW(double oW)
Set the point oW coordinate in the object frame.
Definition vpPoint.cpp:505
double get_Y() const
Get the point cY coordinate in the camera frame.
Definition vpPoint.cpp:451
void set_oY(double oY)
Set the point oY coordinate in the object frame.
Definition vpPoint.cpp:501
void set_X(double cX)
Set the point cX coordinate in the camera frame.
Definition vpPoint.cpp:490
double get_W() const
Get the point cW coordinate in the camera frame.
Definition vpPoint.cpp:455
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
Definition vpPoint.cpp:492
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition vpPoint.cpp:453
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
Definition vpPoint.cpp:503
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition vpPoint.cpp:494
void set_oX(double oX)
Set the point oX coordinate in the object frame.
Definition vpPoint.cpp:499
double get_X() const
Get the point cX coordinate in the camera frame.
Definition vpPoint.cpp:449
Implementation of a pose vector and operations on poses.
vpRowVector t() const
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
void resize(unsigned int i, bool flagNullify=true)
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.