Visual Servoing Platform version 3.5.0
vpHomogeneousMatrix.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 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 http://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 * Authors:
35 * Eric Marchand
36 *
37 *****************************************************************************/
38
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>
51
57 : vpArray2D<double>(4, 4)
58{
59 buildFrom(t, q);
60 (*this)[3][3] = 1.;
61}
62
66vpHomogeneousMatrix::vpHomogeneousMatrix() : vpArray2D<double>(4, 4), m_index(0) { eye(); }
67
72vpHomogeneousMatrix::vpHomogeneousMatrix(const vpHomogeneousMatrix &M) : vpArray2D<double>(4, 4), m_index(0) { *this = M; }
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) : vpArray2D<double>(4, 4), m_index(0)
191{
192 if (list.size() == 12) {
193 std::copy(list.begin(), list.end(), data);
194 data[12] = 0.;
195 data[13] = 0.;
196 data[14] = 0.;
197 data[15] = 1.;
198 }
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()) {
203 throw(vpException(vpException::fatalError, "Cannot initialize homogeneous matrix. "
204 "List element %d (%f) should be 0.", i, data[i]));
205 }
206 }
207 if (std::fabs(data[15] - 1.) > std::numeric_limits<double>::epsilon()) {
208 throw(vpException(vpException::fatalError, "Cannot initialize homogeneous matrix. "
209 "List element 15 (%f) should be 1.", data[15]));
210 }
211 }
212 else {
213 throw(vpException(vpException::fatalError, "Cannot initialize homogeneous matrix from a list (%d elements) that has not 12 or 16 elements", list.size()));
214 }
215
216 if (!isAnHomogeneousMatrix()) {
217 if (isAnHomogeneousMatrix(1e-3)) {
218 // re-orthogonalize rotation matrix since the input is close to a valid rotation matrix
219 vpMatrix U {
220 {data[0], data[1], data[2]},
221 {data[4], data[5], data[6]},
222 {data[8], data[9], data[10]}
223 };
224 vpColVector w;
225 vpMatrix V;
226 U.svd(w, V);
227 vpMatrix R = U * V.t();
228
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];
232 } else {
233 throw(vpException(vpException::fatalError, "Rotation matrix initialization fails since it's elements doesn't represent a rotation matrix"));
234 }
235 }
236}
237#endif
238
278vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<double> &v) : vpArray2D<double>(4, 4), m_index(0)
279{
280 buildFrom(v);
281 (*this)[3][3] = 1.;
282}
283
289vpHomogeneousMatrix::vpHomogeneousMatrix(double tx, double ty, double tz, double tux, double tuy, double tuz)
290 : vpArray2D<double>(4, 4), m_index(0)
291{
292 buildFrom(tx, ty, tz, tux, tuy, tuz);
293 (*this)[3][3] = 1.;
294}
295
301{
302 insert(tu);
303 insert(t);
304}
305
311{
312 insert(R);
313 insert(t);
314}
315
320{
321 vpTranslationVector tv(p[0], p[1], p[2]);
322 vpThetaUVector tu(p[3], p[4], p[5]);
323
324 insert(tu);
325 insert(tv);
326}
327
333{
334 insert(t);
335 insert(q);
336}
337
343void vpHomogeneousMatrix::buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
344{
345 vpRotationMatrix R(tux, tuy, tuz);
346 vpTranslationVector t(tx, ty, tz);
347
348 insert(R);
349 insert(t);
350}
351
392void vpHomogeneousMatrix::buildFrom(const std::vector<float> &v)
393{
394 if (v.size() != 12 && v.size() != 16) {
395 throw(vpException(vpException::dimensionError, "Cannot convert std::vector<float> to vpHomogeneousMatrix"));
396 }
397
398 for (unsigned int i = 0; i < 12; i++)
399 this->data[i] = (double)v[i];
400}
401
442void vpHomogeneousMatrix::buildFrom(const std::vector<double> &v)
443{
444 if (v.size() != 12 && v.size() != 16) {
445 throw(vpException(vpException::dimensionError, "Cannot convert std::vector<double> to vpHomogeneousMatrix"));
446 }
447
448 for (unsigned int i = 0; i < 12; i++)
449 this->data[i] = v[i];
450}
451
458{
459 for (int i = 0; i < 4; i++) {
460 for (int j = 0; j < 4; j++) {
461 rowPtrs[i][j] = M.rowPtrs[i][j];
462 }
463 }
464 return *this;
465}
466
485{
487
488 vpRotationMatrix R1, R2, R;
489 vpTranslationVector T1, T2, T;
490
491 extract(T1);
492 M.extract(T2);
493
494 extract(R1);
495 M.extract(R2);
496
497 R = R1 * R2;
498
499 T = R1 * T2 + T1;
500
501 p.insert(T);
502 p.insert(R);
503
504 return p;
505}
506
525{
526 (*this) = (*this) * M;
527 return (*this);
528}
529
538{
539 if (v.getRows() != 4) {
541 "Cannot multiply a (4x4) homogeneous matrix by a "
542 "(%dx1) column vector",
543 v.getRows()));
544 }
546
547 p = 0.0;
548
549 for (unsigned int j = 0; j < 4; j++) {
550 for (unsigned int i = 0; i < 4; i++) {
551 p[i] += rowPtrs[i][j] * v[j];
552 }
553 }
554
555 return p;
556}
557
570{
571 vpPoint aP;
572
573 vpColVector v(4), v1(4);
574
575 v[0] = bP.get_X();
576 v[1] = bP.get_Y();
577 v[2] = bP.get_Z();
578 v[3] = bP.get_W();
579
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];
584
585 v1 /= v1[3];
586
587 // v1 = M*v ;
588 aP.set_X(v1[0]);
589 aP.set_Y(v1[1]);
590 aP.set_Z(v1[2]);
591 aP.set_W(v1[3]);
592
593 aP.set_oX(v1[0]);
594 aP.set_oY(v1[1]);
595 aP.set_oZ(v1[2]);
596 aP.set_oW(v1[3]);
597
598 return aP;
599}
600
612{
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];
617
618 return t_out;
619}
620
664{
665 m_index = 0;
666 data[m_index] = val;
667 return *this;
668}
669
713{
714 m_index ++;
715 if (m_index >= size()) {
716 throw(vpException(vpException::dimensionError, "Cannot set homogenous matrix out of bounds. It has only %d elements while you try to initialize with %d elements", size(), m_index+1));
717 }
718 data[m_index] = val;
719 return *this;
720}
721
722/*********************************************************************/
723
729{
731 extract(R);
732
733 return R.isARotationMatrix(threshold);
734}
735
741{
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];
745}
746
751{
752 t[0] = (*this)[0][3];
753 t[1] = (*this)[1][3];
754 t[2] = (*this)[2][3];
755}
760{
762 (*this).extract(R);
763 tu.buildFrom(R);
764}
765
770{
772 (*this).extract(R);
773 q.buildFrom(R);
774}
775
780{
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];
784}
785
793{
794 vpRotationMatrix R(tu);
795 insert(R);
796}
797
802{
803 (*this)[0][3] = t[0];
804 (*this)[1][3] = t[1];
805 (*this)[2][3] = t[2];
806}
807
815
831{
833
835 extract(R);
837 extract(T);
838
840 RtT = -(R.t() * T);
841
842 Mi.insert(R.t());
843 Mi.insert(RtT);
844
845 return Mi;
846}
847
852{
853 (*this)[0][0] = 1;
854 (*this)[1][1] = 1;
855 (*this)[2][2] = 1;
856 (*this)[3][3] = 1;
857
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;
862}
863
879
902void vpHomogeneousMatrix::save(std::ofstream &f) const
903{
904 if (!f.fail()) {
905 f << *this;
906 } else {
907 throw(vpException(vpException::ioError, "Cannot save homogeneous matrix: ostream not open"));
908 }
909}
910
929void vpHomogeneousMatrix::load(std::ifstream &f)
930{
931 if (!f.fail()) {
932 for (unsigned int i = 0; i < 4; i++) {
933 for (unsigned int j = 0; j < 4; j++) {
934 f >> (*this)[i][j];
935 }
936 }
937 } else {
938 throw(vpException(vpException::ioError, "Cannot load homogeneous matrix: ifstream not open"));
939 }
940}
941
944{
945 vpPoseVector r(*this);
946 std::cout << r.t();
947}
948
953void vpHomogeneousMatrix::convert(std::vector<float> &M)
954{
955 M.resize(12);
956 for (unsigned int i = 0; i < 12; i++)
957 M[i] = (float)(this->data[i]);
958}
959
964void vpHomogeneousMatrix::convert(std::vector<double> &M)
965{
966 M.resize(12);
967 for (unsigned int i = 0; i < 12; i++)
968 M[i] = this->data[i];
969}
970
975{
977 this->extract(tr);
978 return tr;
979}
980
985{
987 this->extract(R);
988 return R;
989}
990
996{
998 this->extract(tu);
999 return tu;
1000}
1001
1031{
1032 if (j >= getCols())
1033 throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the homogeneous matrix"));
1034 unsigned int nb_rows = getRows();
1035 vpColVector c(nb_rows);
1036 for (unsigned int i = 0; i < nb_rows; i++)
1037 c[i] = (*this)[i][j];
1038 return c;
1039}
1040
1050vpHomogeneousMatrix vpHomogeneousMatrix::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
1051{
1052 vpMatrix meanR(3, 3);
1053 vpColVector meanT(3);
1055 for (size_t i = 0; i < vec_M.size(); i++) {
1056 R = vec_M[i].getRotationMatrix();
1057 meanR += (vpMatrix) R;
1058 meanT += (vpColVector) vec_M[i].getTranslationVector();
1059 }
1060 meanR /= static_cast<double>(vec_M.size());
1061 meanT /= static_cast<double>(vec_M.size());
1062
1063 // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
1064 vpMatrix M, U, V;
1065 vpColVector sv;
1066 meanR.pseudoInverse(M, sv, 1e-6, U, V);
1067 double det = sv[0]*sv[1]*sv[2];
1068 if (det > 0) {
1069 meanR = U * V.t();
1070 }
1071 else {
1072 vpMatrix D(3,3);
1073 D = 0.0;
1074 D[0][0] = D[1][1] = 1.0; D[2][2] = -1;
1075 meanR = U * D * V.t();
1076 }
1077
1078 R = meanR;
1079
1080 vpTranslationVector t(meanT);
1082 return mean;
1083}
1084
1085#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1086
1094
1095#endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:132
unsigned int getCols() const
Definition: vpArray2D.h:279
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:139
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ ioError
I/O error.
Definition: vpException.h:91
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
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.
Definition: vpMatrix.h:154
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2030
vpMatrix t() const
Definition: vpMatrix.cpp:464
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
void set_W(double cW)
Set the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:499
void set_oW(double oW)
Set the point oW coordinate in the object frame.
Definition: vpPoint.cpp:508
double get_Y() const
Get the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:454
void set_oY(double oY)
Set the point oY coordinate in the object frame.
Definition: vpPoint.cpp:504
void set_X(double cX)
Set the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:493
double get_W() const
Get the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:458
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:495
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:456
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:506
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:497
void set_oX(double oX)
Set the point oX coordinate in the object frame.
Definition: vpPoint.cpp:502
double get_X() const
Get the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:452
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
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
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.