Visual Servoing Platform  version 3.2.1 under development (2019-07-16)
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)
80 {
81  buildFrom(t, tu);
82  (*this)[3][3] = 1.;
83 }
84 
90  : vpArray2D<double>(4, 4)
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)
146 {
147  buildFrom(v);
148  (*this)[3][3] = 1.;
149 }
150 
190 vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<double> &v) : vpArray2D<double>(4, 4)
191 {
192  buildFrom(v);
193  (*this)[3][3] = 1.;
194 }
195 
201 vpHomogeneousMatrix::vpHomogeneousMatrix(const double tx, const double ty, const double tz, const double tux,
202  const double tuy, const double tuz)
203  : vpArray2D<double>(4, 4)
204 {
205  buildFrom(tx, ty, tz, tux, tuy, tuz);
206  (*this)[3][3] = 1.;
207 }
208 
214 {
215  insert(tu);
216  insert(t);
217 }
218 
224 {
225  insert(R);
226  insert(t);
227 }
228 
233 {
234  vpTranslationVector tv(p[0], p[1], p[2]);
235  vpThetaUVector tu(p[3], p[4], p[5]);
236 
237  insert(tu);
238  insert(tv);
239 }
240 
246 {
247  insert(t);
248  insert(q);
249 }
250 
256 void vpHomogeneousMatrix::buildFrom(const double tx, const double ty, const double tz, const double tux,
257  const double tuy, const double tuz)
258 {
259  vpRotationMatrix R(tux, tuy, tuz);
260  vpTranslationVector t(tx, ty, tz);
261 
262  insert(R);
263  insert(t);
264 }
265 
306 void vpHomogeneousMatrix::buildFrom(const std::vector<float> &v)
307 {
308  if (v.size() != 12 && v.size() != 16) {
309  throw(vpException(vpException::dimensionError, "Cannot convert std::vector<float> to vpHomogeneousMatrix"));
310  }
311 
312  for (unsigned int i = 0; i < 12; i++)
313  this->data[i] = (double)v[i];
314 }
315 
356 void vpHomogeneousMatrix::buildFrom(const std::vector<double> &v)
357 {
358  if (v.size() != 12 && v.size() != 16) {
359  throw(vpException(vpException::dimensionError, "Cannot convert std::vector<double> to vpHomogeneousMatrix"));
360  }
361 
362  for (unsigned int i = 0; i < 12; i++)
363  this->data[i] = v[i];
364 }
365 
372 {
373  for (int i = 0; i < 4; i++) {
374  for (int j = 0; j < 4; j++) {
375  rowPtrs[i][j] = M.rowPtrs[i][j];
376  }
377  }
378  return *this;
379 }
380 
399 {
401 
402  vpRotationMatrix R1, R2, R;
403  vpTranslationVector T1, T2, T;
404 
405  extract(T1);
406  M.extract(T2);
407 
408  extract(R1);
409  M.extract(R2);
410 
411  R = R1 * R2;
412 
413  T = R1 * T2 + T1;
414 
415  p.insert(T);
416  p.insert(R);
417 
418  return p;
419 }
420 
439 {
440  (*this) = (*this) * M;
441  return (*this);
442 }
443 
452 {
453  if (v.getRows() != 4) {
455  "Cannot multiply a (4x4) homogeneous matrix by a "
456  "(%dx1) column vector",
457  v.getRows()));
458  }
459  vpColVector p(rowNum);
460 
461  p = 0.0;
462 
463  for (unsigned int j = 0; j < 4; j++) {
464  for (unsigned int i = 0; i < 4; i++) {
465  p[i] += rowPtrs[i][j] * v[j];
466  }
467  }
468 
469  return p;
470 }
471 
484 {
485  vpPoint aP;
486 
487  vpColVector v(4), v1(4);
488 
489  v[0] = bP.get_X();
490  v[1] = bP.get_Y();
491  v[2] = bP.get_Z();
492  v[3] = bP.get_W();
493 
494  v1[0] = (*this)[0][0] * v[0] + (*this)[0][1] * v[1] + (*this)[0][2] * v[2] + (*this)[0][3] * v[3];
495  v1[1] = (*this)[1][0] * v[0] + (*this)[1][1] * v[1] + (*this)[1][2] * v[2] + (*this)[1][3] * v[3];
496  v1[2] = (*this)[2][0] * v[0] + (*this)[2][1] * v[1] + (*this)[2][2] * v[2] + (*this)[2][3] * v[3];
497  v1[3] = (*this)[3][0] * v[0] + (*this)[3][1] * v[1] + (*this)[3][2] * v[2] + (*this)[3][3] * v[3];
498 
499  v1 /= v1[3];
500 
501  // v1 = M*v ;
502  aP.set_X(v1[0]);
503  aP.set_Y(v1[1]);
504  aP.set_Z(v1[2]);
505  aP.set_W(v1[3]);
506 
507  aP.set_oX(v1[0]);
508  aP.set_oY(v1[1]);
509  aP.set_oZ(v1[2]);
510  aP.set_oW(v1[3]);
511 
512  return aP;
513 }
514 
526 {
527  vpTranslationVector t_out;
528  t_out[0] = (*this)[0][0] * t[0] + (*this)[0][1] * t[1] + (*this)[0][2] * t[2] + (*this)[0][3];
529  t_out[1] = (*this)[1][0] * t[0] + (*this)[1][1] * t[1] + (*this)[1][2] * t[2] + (*this)[1][3];
530  t_out[2] = (*this)[2][0] * t[0] + (*this)[2][1] * t[1] + (*this)[2][2] * t[2] + (*this)[2][3];
531 
532  return t_out;
533 }
534 
535 /*********************************************************************/
536 
542 {
544  extract(R);
545 
546  return R.isARotationMatrix();
547 }
548 
554 {
555  for (unsigned int i = 0; i < 3; i++)
556  for (unsigned int j = 0; j < 3; j++)
557  R[i][j] = (*this)[i][j];
558 }
559 
564 {
565  t[0] = (*this)[0][3];
566  t[1] = (*this)[1][3];
567  t[2] = (*this)[2][3];
568 }
573 {
575  (*this).extract(R);
576  tu.buildFrom(R);
577 }
578 
583 {
585  (*this).extract(R);
586  q.buildFrom(R);
587 }
588 
593 {
594  for (unsigned int i = 0; i < 3; i++)
595  for (unsigned int j = 0; j < 3; j++)
596  (*this)[i][j] = R[i][j];
597 }
598 
606 {
607  vpRotationMatrix R(tu);
608  insert(R);
609 }
610 
615 {
616  (*this)[0][3] = t[0];
617  (*this)[1][3] = t[1];
618  (*this)[2][3] = t[2];
619 }
620 
628 
644 {
646 
648  extract(R);
650  extract(T);
651 
653  RtT = -(R.t() * T);
654 
655  Mi.insert(R.t());
656  Mi.insert(RtT);
657 
658  return Mi;
659 }
660 
665 {
666  (*this)[0][0] = 1;
667  (*this)[1][1] = 1;
668  (*this)[2][2] = 1;
669  (*this)[3][3] = 1;
670 
671  (*this)[0][1] = (*this)[0][2] = (*this)[0][3] = 0;
672  (*this)[1][0] = (*this)[1][2] = (*this)[1][3] = 0;
673  (*this)[2][0] = (*this)[2][1] = (*this)[2][3] = 0;
674  (*this)[3][0] = (*this)[3][1] = (*this)[3][2] = 0;
675 }
676 
692 
715 void vpHomogeneousMatrix::save(std::ofstream &f) const
716 {
717  if (!f.fail()) {
718  f << *this;
719  } else {
720  throw(vpException(vpException::ioError, "Cannot save homogeneous matrix: ostream not open"));
721  }
722 }
723 
742 void vpHomogeneousMatrix::load(std::ifstream &f)
743 {
744  if (!f.fail()) {
745  for (unsigned int i = 0; i < 4; i++) {
746  for (unsigned int j = 0; j < 4; j++) {
747  f >> (*this)[i][j];
748  }
749  }
750  } else {
751  throw(vpException(vpException::ioError, "Cannot laad homogeneous matrix: ifstream not open"));
752  }
753 }
754 
757 {
758  vpPoseVector r(*this);
759  std::cout << r.t();
760 }
761 
766 void vpHomogeneousMatrix::convert(std::vector<float> &M)
767 {
768  M.resize(12);
769  for (unsigned int i = 0; i < 12; i++)
770  M[i] = (float)(this->data[i]);
771 }
772 
777 void vpHomogeneousMatrix::convert(std::vector<double> &M)
778 {
779  M.resize(12);
780  for (unsigned int i = 0; i < 12; i++)
781  M[i] = this->data[i];
782 }
783 
788 {
790  this->extract(tr);
791  return tr;
792 }
793 
798 {
800  this->extract(R);
801  return R;
802 }
803 
809 {
810  vpThetaUVector tu;
811  this->extract(tu);
812  return tu;
813 }
814 
843 vpColVector vpHomogeneousMatrix::getCol(const unsigned int j) const
844 {
845  if (j >= getCols())
846  throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the homogeneous matrix"));
847  unsigned int nb_rows = getRows();
848  vpColVector c(nb_rows);
849  for (unsigned int i = 0; i < nb_rows; i++)
850  c[i] = (*this)[i][j];
851  return c;
852 }
853 
863 vpHomogeneousMatrix vpHomogeneousMatrix::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
864 {
865  vpMatrix meanR(3, 3);
866  vpColVector meanT(3);
868  for (size_t i = 0; i < vec_M.size(); i++) {
869  R = vec_M[i].getRotationMatrix();
870  meanR += (vpMatrix) R;
871  meanT += (vpColVector) vec_M[i].getTranslationVector();
872  }
873  meanR /= static_cast<double>(vec_M.size());
874  meanT /= static_cast<double>(vec_M.size());
875 
876  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
877  vpMatrix M, U, V;
878  vpColVector sv;
879  meanR.pseudoInverse(M, sv, 1e-6, U, V);
880  double det = sv[0]*sv[1]*sv[2];
881  if (det > 0) {
882  meanR = U * V.t();
883  }
884  else {
885  vpMatrix D(3,3);
886  D = 0.0;
887  D[0][0] = D[1][1] = 1.0; D[2][2] = -1;
888  meanR = U * D * V.t();
889  }
890 
891  R = meanR;
892 
893  vpTranslationVector t(meanT);
895  return mean;
896 }
897 
898 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
899 
907 
908 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
void set_oZ(const double oZ)
Set the point Z coordinate in the object frame.
Definition: vpPoint.cpp:467
Implementation of an homogeneous matrix and operations on such kind of matrices.
vp_deprecated void setIdentity()
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpRotationMatrix t() const
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 getCols() const
Definition: vpArray2D.h:279
void set_X(const double X)
Set the point X coordinate in the camera frame.
Definition: vpPoint.cpp:454
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
double get_W() const
Get the point W coordinate in the camera frame.
Definition: vpPoint.cpp:419
vpThetaUVector getThetaUVector() const
void load(std::ifstream &f)
Class that defines what is a point.
Definition: vpPoint.h:58
bool isARotationMatrix() const
Implementation of a rotation matrix and operations on such kind of matrices.
vpHomogeneousMatrix & operator=(const vpHomogeneousMatrix &M)
void insert(const vpRotationMatrix &R)
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
void set_oW(const double oW)
Set the point W coordinate in the object frame.
Definition: vpPoint.cpp:469
void set_Z(const double Z)
Set the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:458
vpRotationMatrix getRotationMatrix() const
void set_W(const double W)
Set the point W coordinate in the camera frame.
Definition: vpPoint.cpp:460
vpTranslationVector getTranslationVector() const
void set_oX(const double oX)
Set the point X coordinate in the object frame.
Definition: vpPoint.cpp:463
void set_Y(const double Y)
Set the point Y coordinate in the camera frame.
Definition: vpPoint.cpp:456
void extract(vpRotationMatrix &R) const
Implementation of a rotation vector as quaternion angle minimal representation.
unsigned int getRows() const
Definition: vpArray2D.h:289
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
double get_Y() const
Get the point Y coordinate in the camera frame.
Definition: vpPoint.cpp:415
double get_Z() const
Get the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:417
bool isAnHomogeneousMatrix() const
void save(std::ofstream &f) const
vpMatrix t() const
Definition: vpMatrix.cpp:375
vpHomogeneousMatrix operator*(const vpHomogeneousMatrix &M) const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void set_oY(const double oY)
Set the point Y coordinate in the object frame.
Definition: vpPoint.cpp:465
static vpHomogeneousMatrix mean(const std::vector< vpHomogeneousMatrix > &vec_M)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
vpHomogeneousMatrix inverse() const
vpColVector getCol(const unsigned int j) const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2021
vpHomogeneousMatrix & operator*=(const vpHomogeneousMatrix &M)
void convert(std::vector< float > &M)
void print() const
Print the matrix as a pose vector .
double get_X() const
Get the point X coordinate in the camera frame.
Definition: vpPoint.cpp:413
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
vpRowVector t() const