Visual Servoing Platform  version 3.3.1 under development (2020-07-03)
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  throw(vpException(vpException::fatalError, "Rotation matrix initialization fails since it's elements doesn't represent a rotation matrix"));
218  }
219 }
220 #endif
221 
261 vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<double> &v) : vpArray2D<double>(4, 4), m_index(0)
262 {
263  buildFrom(v);
264  (*this)[3][3] = 1.;
265 }
266 
272 vpHomogeneousMatrix::vpHomogeneousMatrix(double tx, double ty, double tz, double tux, double tuy, double tuz)
273  : vpArray2D<double>(4, 4), m_index(0)
274 {
275  buildFrom(tx, ty, tz, tux, tuy, tuz);
276  (*this)[3][3] = 1.;
277 }
278 
284 {
285  insert(tu);
286  insert(t);
287 }
288 
294 {
295  insert(R);
296  insert(t);
297 }
298 
303 {
304  vpTranslationVector tv(p[0], p[1], p[2]);
305  vpThetaUVector tu(p[3], p[4], p[5]);
306 
307  insert(tu);
308  insert(tv);
309 }
310 
316 {
317  insert(t);
318  insert(q);
319 }
320 
326 void vpHomogeneousMatrix::buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
327 {
328  vpRotationMatrix R(tux, tuy, tuz);
329  vpTranslationVector t(tx, ty, tz);
330 
331  insert(R);
332  insert(t);
333 }
334 
375 void vpHomogeneousMatrix::buildFrom(const std::vector<float> &v)
376 {
377  if (v.size() != 12 && v.size() != 16) {
378  throw(vpException(vpException::dimensionError, "Cannot convert std::vector<float> to vpHomogeneousMatrix"));
379  }
380 
381  for (unsigned int i = 0; i < 12; i++)
382  this->data[i] = (double)v[i];
383 }
384 
425 void vpHomogeneousMatrix::buildFrom(const std::vector<double> &v)
426 {
427  if (v.size() != 12 && v.size() != 16) {
428  throw(vpException(vpException::dimensionError, "Cannot convert std::vector<double> to vpHomogeneousMatrix"));
429  }
430 
431  for (unsigned int i = 0; i < 12; i++)
432  this->data[i] = v[i];
433 }
434 
441 {
442  for (int i = 0; i < 4; i++) {
443  for (int j = 0; j < 4; j++) {
444  rowPtrs[i][j] = M.rowPtrs[i][j];
445  }
446  }
447  return *this;
448 }
449 
468 {
470 
471  vpRotationMatrix R1, R2, R;
472  vpTranslationVector T1, T2, T;
473 
474  extract(T1);
475  M.extract(T2);
476 
477  extract(R1);
478  M.extract(R2);
479 
480  R = R1 * R2;
481 
482  T = R1 * T2 + T1;
483 
484  p.insert(T);
485  p.insert(R);
486 
487  return p;
488 }
489 
508 {
509  (*this) = (*this) * M;
510  return (*this);
511 }
512 
521 {
522  if (v.getRows() != 4) {
524  "Cannot multiply a (4x4) homogeneous matrix by a "
525  "(%dx1) column vector",
526  v.getRows()));
527  }
528  vpColVector p(rowNum);
529 
530  p = 0.0;
531 
532  for (unsigned int j = 0; j < 4; j++) {
533  for (unsigned int i = 0; i < 4; i++) {
534  p[i] += rowPtrs[i][j] * v[j];
535  }
536  }
537 
538  return p;
539 }
540 
553 {
554  vpPoint aP;
555 
556  vpColVector v(4), v1(4);
557 
558  v[0] = bP.get_X();
559  v[1] = bP.get_Y();
560  v[2] = bP.get_Z();
561  v[3] = bP.get_W();
562 
563  v1[0] = (*this)[0][0] * v[0] + (*this)[0][1] * v[1] + (*this)[0][2] * v[2] + (*this)[0][3] * v[3];
564  v1[1] = (*this)[1][0] * v[0] + (*this)[1][1] * v[1] + (*this)[1][2] * v[2] + (*this)[1][3] * v[3];
565  v1[2] = (*this)[2][0] * v[0] + (*this)[2][1] * v[1] + (*this)[2][2] * v[2] + (*this)[2][3] * v[3];
566  v1[3] = (*this)[3][0] * v[0] + (*this)[3][1] * v[1] + (*this)[3][2] * v[2] + (*this)[3][3] * v[3];
567 
568  v1 /= v1[3];
569 
570  // v1 = M*v ;
571  aP.set_X(v1[0]);
572  aP.set_Y(v1[1]);
573  aP.set_Z(v1[2]);
574  aP.set_W(v1[3]);
575 
576  aP.set_oX(v1[0]);
577  aP.set_oY(v1[1]);
578  aP.set_oZ(v1[2]);
579  aP.set_oW(v1[3]);
580 
581  return aP;
582 }
583 
595 {
596  vpTranslationVector t_out;
597  t_out[0] = (*this)[0][0] * t[0] + (*this)[0][1] * t[1] + (*this)[0][2] * t[2] + (*this)[0][3];
598  t_out[1] = (*this)[1][0] * t[0] + (*this)[1][1] * t[1] + (*this)[1][2] * t[2] + (*this)[1][3];
599  t_out[2] = (*this)[2][0] * t[0] + (*this)[2][1] * t[1] + (*this)[2][2] * t[2] + (*this)[2][3];
600 
601  return t_out;
602 }
603 
647 {
648  m_index = 0;
649  data[m_index] = val;
650  return *this;
651 }
652 
696 {
697  m_index ++;;
698  if (m_index >= size()) {
699  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));
700  }
701  data[m_index] = val;
702  return *this;
703 }
704 
705 /*********************************************************************/
706 
712 {
714  extract(R);
715 
716  return R.isARotationMatrix();
717 }
718 
724 {
725  for (unsigned int i = 0; i < 3; i++)
726  for (unsigned int j = 0; j < 3; j++)
727  R[i][j] = (*this)[i][j];
728 }
729 
734 {
735  t[0] = (*this)[0][3];
736  t[1] = (*this)[1][3];
737  t[2] = (*this)[2][3];
738 }
743 {
745  (*this).extract(R);
746  tu.buildFrom(R);
747 }
748 
753 {
755  (*this).extract(R);
756  q.buildFrom(R);
757 }
758 
763 {
764  for (unsigned int i = 0; i < 3; i++)
765  for (unsigned int j = 0; j < 3; j++)
766  (*this)[i][j] = R[i][j];
767 }
768 
776 {
777  vpRotationMatrix R(tu);
778  insert(R);
779 }
780 
785 {
786  (*this)[0][3] = t[0];
787  (*this)[1][3] = t[1];
788  (*this)[2][3] = t[2];
789 }
790 
798 
814 {
816 
818  extract(R);
820  extract(T);
821 
823  RtT = -(R.t() * T);
824 
825  Mi.insert(R.t());
826  Mi.insert(RtT);
827 
828  return Mi;
829 }
830 
835 {
836  (*this)[0][0] = 1;
837  (*this)[1][1] = 1;
838  (*this)[2][2] = 1;
839  (*this)[3][3] = 1;
840 
841  (*this)[0][1] = (*this)[0][2] = (*this)[0][3] = 0;
842  (*this)[1][0] = (*this)[1][2] = (*this)[1][3] = 0;
843  (*this)[2][0] = (*this)[2][1] = (*this)[2][3] = 0;
844  (*this)[3][0] = (*this)[3][1] = (*this)[3][2] = 0;
845 }
846 
862 
885 void vpHomogeneousMatrix::save(std::ofstream &f) const
886 {
887  if (!f.fail()) {
888  f << *this;
889  } else {
890  throw(vpException(vpException::ioError, "Cannot save homogeneous matrix: ostream not open"));
891  }
892 }
893 
912 void vpHomogeneousMatrix::load(std::ifstream &f)
913 {
914  if (!f.fail()) {
915  for (unsigned int i = 0; i < 4; i++) {
916  for (unsigned int j = 0; j < 4; j++) {
917  f >> (*this)[i][j];
918  }
919  }
920  } else {
921  throw(vpException(vpException::ioError, "Cannot load homogeneous matrix: ifstream not open"));
922  }
923 }
924 
927 {
928  vpPoseVector r(*this);
929  std::cout << r.t();
930 }
931 
936 void vpHomogeneousMatrix::convert(std::vector<float> &M)
937 {
938  M.resize(12);
939  for (unsigned int i = 0; i < 12; i++)
940  M[i] = (float)(this->data[i]);
941 }
942 
947 void vpHomogeneousMatrix::convert(std::vector<double> &M)
948 {
949  M.resize(12);
950  for (unsigned int i = 0; i < 12; i++)
951  M[i] = this->data[i];
952 }
953 
958 {
960  this->extract(tr);
961  return tr;
962 }
963 
968 {
970  this->extract(R);
971  return R;
972 }
973 
979 {
980  vpThetaUVector tu;
981  this->extract(tu);
982  return tu;
983 }
984 
1014 {
1015  if (j >= getCols())
1016  throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the homogeneous matrix"));
1017  unsigned int nb_rows = getRows();
1018  vpColVector c(nb_rows);
1019  for (unsigned int i = 0; i < nb_rows; i++)
1020  c[i] = (*this)[i][j];
1021  return c;
1022 }
1023 
1033 vpHomogeneousMatrix vpHomogeneousMatrix::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
1034 {
1035  vpMatrix meanR(3, 3);
1036  vpColVector meanT(3);
1037  vpRotationMatrix R;
1038  for (size_t i = 0; i < vec_M.size(); i++) {
1039  R = vec_M[i].getRotationMatrix();
1040  meanR += (vpMatrix) R;
1041  meanT += (vpColVector) vec_M[i].getTranslationVector();
1042  }
1043  meanR /= static_cast<double>(vec_M.size());
1044  meanT /= static_cast<double>(vec_M.size());
1045 
1046  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
1047  vpMatrix M, U, V;
1048  vpColVector sv;
1049  meanR.pseudoInverse(M, sv, 1e-6, U, V);
1050  double det = sv[0]*sv[1]*sv[2];
1051  if (det > 0) {
1052  meanR = U * V.t();
1053  }
1054  else {
1055  vpMatrix D(3,3);
1056  D = 0.0;
1057  D[0][0] = D[1][1] = 1.0; D[2][2] = -1;
1058  meanR = U * D * V.t();
1059  }
1060 
1061  R = meanR;
1062 
1063  vpTranslationVector t(meanT);
1064  vpHomogeneousMatrix mean(t, R);
1065  return mean;
1066 }
1067 
1068 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1069 
1077 
1078 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:156
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2399
void set_W(double cW)
Set the point cW coordinate in the camera frame.
Definition: vpPoint.cpp:485
vpHomogeneousMatrix & operator<<(double val)
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:552
void set_oY(double oY)
Set the point oY coordinate in the object frame.
Definition: vpPoint.cpp:490
vpHomogeneousMatrix operator*(const vpHomogeneousMatrix &M) const
bool isAnHomogeneousMatrix() 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.
bool isARotationMatrix() const
double ** rowPtrs
Address of the first element of each rows.
Definition: vpArray2D.h:139
vpRotationMatrix getRotationMatrix() const