Visual Servoing Platform  version 3.4.1 under development (2021-08-04)
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 
67 
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 
145 vpHomogeneousMatrix::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)
152 
190 vpHomogeneousMatrix::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 
278 vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<double> &v) : vpArray2D<double>(4, 4), m_index(0)
279 {
280  buildFrom(v);
281  (*this)[3][3] = 1.;
282 }
283 
289 vpHomogeneousMatrix::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 
343 void 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 
392 void 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 
442 void 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  }
545  vpColVector p(rowNum);
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 {
613  vpTranslationVector t_out;
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 
728 bool vpHomogeneousMatrix::isAnHomogeneousMatrix(double threshold) const
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 
902 void 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 
929 void 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 
953 void 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 
964 void 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 {
997  vpThetaUVector tu;
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 
1050 vpHomogeneousMatrix vpHomogeneousMatrix::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
1051 {
1052  vpMatrix meanR(3, 3);
1053  vpColVector meanT(3);
1054  vpRotationMatrix R;
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);
1081  vpHomogeneousMatrix mean(t, R);
1082  return mean;
1083 }
1084 
1085 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1086 
1094 
1095 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2030
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
bool isAnHomogeneousMatrix(double threshold=1e-6) const
void set_W(double cW)
Set the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:485
vpHomogeneousMatrix & operator<<(double val)
bool isARotationMatrix(double threshold=1e-6) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
vp_deprecated void setIdentity()
void print() const
Print the matrix as a pose vector .
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getRows() const
Definition: vpArray2D.h:289
vpHomogeneousMatrix inverse() const
void set_Z(double cZ)
Set the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:483
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
vpQuaternionVector buildFrom(const double qx, const double qy, const double qz, const double qw)
Implementation of a generic 2D array used as base class for matrices and vectors. ...
Definition: vpArray2D.h:131
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
void extract(vpRotationMatrix &R) const
vpThetaUVector getThetaUVector() const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
vpRotationMatrix t() const
void load(std::ifstream &f)
double get_W() const
Get the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:444
vpColVector getCol(unsigned int j) const
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
Implementation of a rotation matrix and operations on such kind of matrices.
unsigned int getCols() const
Definition: vpArray2D.h:279
void set_X(double cX)
Set the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:479
vpHomogeneousMatrix & operator=(const vpHomogeneousMatrix &M)
void insert(const vpRotationMatrix &R)
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
vpMatrix t() const
Definition: vpMatrix.cpp:464
void set_oY(double oY)
Set the point oY coordinate in the object frame.
Definition: vpPoint.cpp:490
vpHomogeneousMatrix operator*(const vpHomogeneousMatrix &M) const
Implementation of a rotation vector as quaternion angle minimal representation.
void set_oW(double oW)
Set the point oW coordinate in the object frame.
Definition: vpPoint.cpp:494
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:492
vpTranslationVector getTranslationVector() const
double get_X() const
Get the point cX coordinate in the camera frame.
Definition: vpPoint.cpp:438
void set_Y(double cY)
Set the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:481
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void set_oX(double oX)
Set the point oX coordinate in the object frame.
Definition: vpPoint.cpp:488
vpHomogeneousMatrix & operator,(double val)
static vpHomogeneousMatrix mean(const std::vector< vpHomogeneousMatrix > &vec_M)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
vpRowVector t() const
void save(std::ofstream &f) const
vpHomogeneousMatrix & operator*=(const vpHomogeneousMatrix &M)
void convert(std::vector< float > &M)
double get_Z() const
Get the point cZ coordinate in the camera frame.
Definition: vpPoint.cpp:442
double get_Y() const
Get the point cY coordinate in the camera frame.
Definition: vpPoint.cpp:440
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:139
vpRotationMatrix getRotationMatrix() const