Visual Servoing Platform  version 3.5.1 under development (2022-12-04)
vpHomogeneousMatrix.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2022 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 
66 vpHomogeneousMatrix::vpHomogeneousMatrix() : vpArray2D<double>(4, 4), m_index(0) { eye(); }
67 
73 {
74  *this = M;
75 }
76 
82  : vpArray2D<double>(4, 4), m_index(0)
83 {
84  buildFrom(t, tu);
85  (*this)[3][3] = 1.;
86 }
87 
93  : vpArray2D<double>(4, 4), m_index(0)
94 {
95  insert(R);
96  insert(t);
97  (*this)[3][3] = 1.;
98 }
99 
104 {
105  buildFrom(p[0], p[1], p[2], p[3], p[4], p[5]);
106  (*this)[3][3] = 1.;
107 }
108 
148 vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<float> &v) : vpArray2D<double>(4, 4), m_index(0)
149 {
150  buildFrom(v);
151  (*this)[3][3] = 1.;
152 }
153 
154 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
193 vpHomogeneousMatrix::vpHomogeneousMatrix(const std::initializer_list<double> &list)
194  : vpArray2D<double>(4, 4), m_index(0)
195 {
196  if (list.size() == 12) {
197  std::copy(list.begin(), list.end(), data);
198  data[12] = 0.;
199  data[13] = 0.;
200  data[14] = 0.;
201  data[15] = 1.;
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.",
209  i, data[i]));
210  }
211  }
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.",
216  data[15]));
217  }
218  } else {
220  "Cannot initialize homogeneous matrix from a list (%d elements) that has not 12 or 16 elements",
221  list.size()));
222  }
223 
224  if (!isAnHomogeneousMatrix()) {
225  if (isAnHomogeneousMatrix(1e-3)) {
226  // re-orthogonalize rotation matrix since the input is close to a valid rotation matrix
227  vpRotationMatrix R(*this);
228  R.orthogonalize();
229 
230  data[0] = R[0][0];
231  data[1] = R[0][1];
232  data[2] = R[0][2];
233  data[4] = R[1][0];
234  data[5] = R[1][1];
235  data[6] = R[1][2];
236  data[8] = R[2][0];
237  data[9] = R[2][1];
238  data[10] = R[2][2];
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 
287 vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<double> &v) : vpArray2D<double>(4, 4), m_index(0)
288 {
289  buildFrom(v);
290  (*this)[3][3] = 1.;
291 }
292 
298 vpHomogeneousMatrix::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 
342 {
343  insert(t);
344  insert(q);
345 }
346 
352 void 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 
401 void 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 
451 void 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  }
554  vpColVector p(rowNum);
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 {
622  vpTranslationVector t_out;
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 
673 {
674  m_index = 0;
675  data[m_index] = val;
676  return *this;
677 }
678 
722 {
723  m_index++;
724  if (m_index >= size()) {
726  "Cannot set homogenous matrix out of bounds. It has only %d elements while you try to initialize "
727  "with %d elements",
728  size(), m_index + 1));
729  }
730  data[m_index] = val;
731  return *this;
732 }
733 
734 /*********************************************************************/
735 
742 bool vpHomogeneousMatrix::isAnHomogeneousMatrix(double threshold) const
743 {
745  extract(R);
746 
747  const double epsilon = std::numeric_limits<double>::epsilon();
748  return R.isARotationMatrix(threshold) && vpMath::nul((*this)[3][0], epsilon) && vpMath::nul((*this)[3][1], epsilon) &&
749  vpMath::nul((*this)[3][2], epsilon) && vpMath::equal((*this)[3][3], 1.0, epsilon);
750 }
751 
757 {
758  for (unsigned int i = 0; i < size(); i++) {
759  if (vpMath::isNaN(data[i])) {
760  return false;
761  }
762  }
763  return true;
764 }
765 
771 {
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];
775 }
776 
781 {
782  t[0] = (*this)[0][3];
783  t[1] = (*this)[1][3];
784  t[2] = (*this)[2][3];
785 }
790 {
792  (*this).extract(R);
793  tu.buildFrom(R);
794 }
795 
800 {
802  (*this).extract(R);
803  q.buildFrom(R);
804 }
805 
810 {
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];
814 }
815 
823 {
824  vpRotationMatrix R(tu);
825  insert(R);
826 }
827 
832 {
833  (*this)[0][3] = t[0];
834  (*this)[1][3] = t[1];
835  (*this)[2][3] = t[2];
836 }
837 
845 
861 {
863 
865  extract(R);
867  extract(T);
868 
870  RtT = -(R.t() * T);
871 
872  Mi.insert(R.t());
873  Mi.insert(RtT);
874 
875  return Mi;
876 }
877 
882 {
883  (*this)[0][0] = 1;
884  (*this)[1][1] = 1;
885  (*this)[2][2] = 1;
886  (*this)[3][3] = 1;
887 
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;
892 }
893 
909 
932 void vpHomogeneousMatrix::save(std::ofstream &f) const
933 {
934  if (!f.fail()) {
935  f << *this;
936  } else {
937  throw(vpException(vpException::ioError, "Cannot save homogeneous matrix: ostream not open"));
938  }
939 }
940 
959 void vpHomogeneousMatrix::load(std::ifstream &f)
960 {
961  if (!f.fail()) {
962  for (unsigned int i = 0; i < 4; i++) {
963  for (unsigned int j = 0; j < 4; j++) {
964  f >> (*this)[i][j];
965  }
966  }
967  } else {
968  throw(vpException(vpException::ioError, "Cannot load homogeneous matrix: ifstream not open"));
969  }
970 }
971 
976 {
977  vpRotationMatrix R(*this);
978  R.orthogonalize();
979 
980  data[0] = R[0][0];
981  data[1] = R[0][1];
982  data[2] = R[0][2];
983  data[4] = R[1][0];
984  data[5] = R[1][1];
985  data[6] = R[1][2];
986  data[8] = R[2][0];
987  data[9] = R[2][1];
988  data[10] = R[2][2];
989 }
990 
993 {
994  vpPoseVector r(*this);
995  std::cout << r.t();
996 }
997 
1002 void vpHomogeneousMatrix::convert(std::vector<float> &M)
1003 {
1004  M.resize(12);
1005  for (unsigned int i = 0; i < 12; i++)
1006  M[i] = (float)(this->data[i]);
1007 }
1008 
1013 void vpHomogeneousMatrix::convert(std::vector<double> &M)
1014 {
1015  M.resize(12);
1016  for (unsigned int i = 0; i < 12; i++)
1017  M[i] = this->data[i];
1018 }
1019 
1024 {
1026  this->extract(tr);
1027  return tr;
1028 }
1029 
1034 {
1035  vpRotationMatrix R;
1036  this->extract(R);
1037  return R;
1038 }
1039 
1045 {
1046  vpThetaUVector tu;
1047  this->extract(tu);
1048  return tu;
1049 }
1050 
1080 {
1081  if (j >= getCols())
1082  throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the homogeneous matrix"));
1083  unsigned int nb_rows = getRows();
1084  vpColVector c(nb_rows);
1085  for (unsigned int i = 0; i < nb_rows; i++)
1086  c[i] = (*this)[i][j];
1087  return c;
1088 }
1089 
1096 vpHomogeneousMatrix vpHomogeneousMatrix::compute3d3dTransformation(const std::vector<vpPoint> &p, const std::vector<vpPoint> &q)
1097 {
1098  const double N = static_cast<double>(p.size());
1099 
1100  vpColVector p_bar(3, 0.0);
1101  vpColVector q_bar(3, 0.0);
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];
1106  }
1107  }
1108 
1109  for (unsigned int j = 0; j < 3; j++) {
1110  p_bar[j] /= N;
1111  q_bar[j] /= N;
1112  }
1113 
1114  vpMatrix pc(static_cast<unsigned int>(p.size()), 3);
1115  vpMatrix qc(static_cast<unsigned int>(q.size()), 3);
1116 
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];
1121  }
1122  }
1123 
1124  const vpMatrix pct_qc = pc.t() * qc;
1125  vpMatrix U = pct_qc, V;
1126  vpColVector W;
1127  U.svd(W, V);
1128 
1129  vpMatrix Vt = V.t();
1130  vpMatrix R = U * Vt;
1131  if (R.det() < 0) {
1132  Vt[2][0] *= -1;
1133  Vt[2][1] *= -1;
1134  Vt[2][2] *= -1;
1135 
1136  R = U * Vt;
1137  }
1138 
1139  const vpColVector t = p_bar - R * q_bar;
1140 
1142 }
1143 
1153 vpHomogeneousMatrix vpHomogeneousMatrix::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
1154 {
1155  vpMatrix meanR(3, 3);
1156  vpColVector meanT(3);
1157  vpRotationMatrix R;
1158  for (size_t i = 0; i < vec_M.size(); i++) {
1159  R = vec_M[i].getRotationMatrix();
1160  meanR += (vpMatrix)R;
1161  meanT += (vpColVector)vec_M[i].getTranslationVector();
1162  }
1163  meanR /= static_cast<double>(vec_M.size());
1164  meanT /= static_cast<double>(vec_M.size());
1165 
1166  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
1167  vpMatrix M, U, V;
1168  vpColVector sv;
1169  meanR.pseudoInverse(M, sv, 1e-6, U, V);
1170  double det = sv[0] * sv[1] * sv[2];
1171  if (det > 0) {
1172  meanR = U * V.t();
1173  } else {
1174  vpMatrix D(3, 3);
1175  D = 0.0;
1176  D[0][0] = D[1][1] = 1.0;
1177  D[2][2] = -1;
1178  meanR = U * D * V.t();
1179  }
1180 
1181  R = meanR;
1182 
1183  vpTranslationVector t(meanT);
1184  vpHomogeneousMatrix mean(t, R);
1185  return mean;
1186 }
1187 
1188 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1189 
1197 
1198 #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:281
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:293
unsigned int getRows() const
Definition: vpArray2D.h:291
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
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)
Definition: vpMath.cpp:86
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:364
static bool nul(double x, double s=0.001)
Definition: vpMath.h:355
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2024
vpMatrix t() const
Definition: vpMatrix.cpp:465
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2235
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:6464
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.