Visual Servoing Platform  version 3.3.1 under development (2020-02-21)
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(double tx, double ty, double tz, double tux, double tuy, double tuz)
202  : vpArray2D<double>(4, 4)
203 {
204  buildFrom(tx, ty, tz, tux, tuy, tuz);
205  (*this)[3][3] = 1.;
206 }
207 
213 {
214  insert(tu);
215  insert(t);
216 }
217 
223 {
224  insert(R);
225  insert(t);
226 }
227 
232 {
233  vpTranslationVector tv(p[0], p[1], p[2]);
234  vpThetaUVector tu(p[3], p[4], p[5]);
235 
236  insert(tu);
237  insert(tv);
238 }
239 
245 {
246  insert(t);
247  insert(q);
248 }
249 
255 void vpHomogeneousMatrix::buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
256 {
257  vpRotationMatrix R(tux, tuy, tuz);
258  vpTranslationVector t(tx, ty, tz);
259 
260  insert(R);
261  insert(t);
262 }
263 
304 void vpHomogeneousMatrix::buildFrom(const std::vector<float> &v)
305 {
306  if (v.size() != 12 && v.size() != 16) {
307  throw(vpException(vpException::dimensionError, "Cannot convert std::vector<float> to vpHomogeneousMatrix"));
308  }
309 
310  for (unsigned int i = 0; i < 12; i++)
311  this->data[i] = (double)v[i];
312 }
313 
354 void vpHomogeneousMatrix::buildFrom(const std::vector<double> &v)
355 {
356  if (v.size() != 12 && v.size() != 16) {
357  throw(vpException(vpException::dimensionError, "Cannot convert std::vector<double> to vpHomogeneousMatrix"));
358  }
359 
360  for (unsigned int i = 0; i < 12; i++)
361  this->data[i] = v[i];
362 }
363 
370 {
371  for (int i = 0; i < 4; i++) {
372  for (int j = 0; j < 4; j++) {
373  rowPtrs[i][j] = M.rowPtrs[i][j];
374  }
375  }
376  return *this;
377 }
378 
397 {
399 
400  vpRotationMatrix R1, R2, R;
401  vpTranslationVector T1, T2, T;
402 
403  extract(T1);
404  M.extract(T2);
405 
406  extract(R1);
407  M.extract(R2);
408 
409  R = R1 * R2;
410 
411  T = R1 * T2 + T1;
412 
413  p.insert(T);
414  p.insert(R);
415 
416  return p;
417 }
418 
437 {
438  (*this) = (*this) * M;
439  return (*this);
440 }
441 
450 {
451  if (v.getRows() != 4) {
453  "Cannot multiply a (4x4) homogeneous matrix by a "
454  "(%dx1) column vector",
455  v.getRows()));
456  }
457  vpColVector p(rowNum);
458 
459  p = 0.0;
460 
461  for (unsigned int j = 0; j < 4; j++) {
462  for (unsigned int i = 0; i < 4; i++) {
463  p[i] += rowPtrs[i][j] * v[j];
464  }
465  }
466 
467  return p;
468 }
469 
482 {
483  vpPoint aP;
484 
485  vpColVector v(4), v1(4);
486 
487  v[0] = bP.get_X();
488  v[1] = bP.get_Y();
489  v[2] = bP.get_Z();
490  v[3] = bP.get_W();
491 
492  v1[0] = (*this)[0][0] * v[0] + (*this)[0][1] * v[1] + (*this)[0][2] * v[2] + (*this)[0][3] * v[3];
493  v1[1] = (*this)[1][0] * v[0] + (*this)[1][1] * v[1] + (*this)[1][2] * v[2] + (*this)[1][3] * v[3];
494  v1[2] = (*this)[2][0] * v[0] + (*this)[2][1] * v[1] + (*this)[2][2] * v[2] + (*this)[2][3] * v[3];
495  v1[3] = (*this)[3][0] * v[0] + (*this)[3][1] * v[1] + (*this)[3][2] * v[2] + (*this)[3][3] * v[3];
496 
497  v1 /= v1[3];
498 
499  // v1 = M*v ;
500  aP.set_X(v1[0]);
501  aP.set_Y(v1[1]);
502  aP.set_Z(v1[2]);
503  aP.set_W(v1[3]);
504 
505  aP.set_oX(v1[0]);
506  aP.set_oY(v1[1]);
507  aP.set_oZ(v1[2]);
508  aP.set_oW(v1[3]);
509 
510  return aP;
511 }
512 
524 {
525  vpTranslationVector t_out;
526  t_out[0] = (*this)[0][0] * t[0] + (*this)[0][1] * t[1] + (*this)[0][2] * t[2] + (*this)[0][3];
527  t_out[1] = (*this)[1][0] * t[0] + (*this)[1][1] * t[1] + (*this)[1][2] * t[2] + (*this)[1][3];
528  t_out[2] = (*this)[2][0] * t[0] + (*this)[2][1] * t[1] + (*this)[2][2] * t[2] + (*this)[2][3];
529 
530  return t_out;
531 }
532 
533 /*********************************************************************/
534 
540 {
542  extract(R);
543 
544  return R.isARotationMatrix();
545 }
546 
552 {
553  for (unsigned int i = 0; i < 3; i++)
554  for (unsigned int j = 0; j < 3; j++)
555  R[i][j] = (*this)[i][j];
556 }
557 
562 {
563  t[0] = (*this)[0][3];
564  t[1] = (*this)[1][3];
565  t[2] = (*this)[2][3];
566 }
571 {
573  (*this).extract(R);
574  tu.buildFrom(R);
575 }
576 
581 {
583  (*this).extract(R);
584  q.buildFrom(R);
585 }
586 
591 {
592  for (unsigned int i = 0; i < 3; i++)
593  for (unsigned int j = 0; j < 3; j++)
594  (*this)[i][j] = R[i][j];
595 }
596 
604 {
605  vpRotationMatrix R(tu);
606  insert(R);
607 }
608 
613 {
614  (*this)[0][3] = t[0];
615  (*this)[1][3] = t[1];
616  (*this)[2][3] = t[2];
617 }
618 
626 
642 {
644 
646  extract(R);
648  extract(T);
649 
651  RtT = -(R.t() * T);
652 
653  Mi.insert(R.t());
654  Mi.insert(RtT);
655 
656  return Mi;
657 }
658 
663 {
664  (*this)[0][0] = 1;
665  (*this)[1][1] = 1;
666  (*this)[2][2] = 1;
667  (*this)[3][3] = 1;
668 
669  (*this)[0][1] = (*this)[0][2] = (*this)[0][3] = 0;
670  (*this)[1][0] = (*this)[1][2] = (*this)[1][3] = 0;
671  (*this)[2][0] = (*this)[2][1] = (*this)[2][3] = 0;
672  (*this)[3][0] = (*this)[3][1] = (*this)[3][2] = 0;
673 }
674 
690 
713 void vpHomogeneousMatrix::save(std::ofstream &f) const
714 {
715  if (!f.fail()) {
716  f << *this;
717  } else {
718  throw(vpException(vpException::ioError, "Cannot save homogeneous matrix: ostream not open"));
719  }
720 }
721 
740 void vpHomogeneousMatrix::load(std::ifstream &f)
741 {
742  if (!f.fail()) {
743  for (unsigned int i = 0; i < 4; i++) {
744  for (unsigned int j = 0; j < 4; j++) {
745  f >> (*this)[i][j];
746  }
747  }
748  } else {
749  throw(vpException(vpException::ioError, "Cannot load homogeneous matrix: ifstream not open"));
750  }
751 }
752 
755 {
756  vpPoseVector r(*this);
757  std::cout << r.t();
758 }
759 
764 void vpHomogeneousMatrix::convert(std::vector<float> &M)
765 {
766  M.resize(12);
767  for (unsigned int i = 0; i < 12; i++)
768  M[i] = (float)(this->data[i]);
769 }
770 
775 void vpHomogeneousMatrix::convert(std::vector<double> &M)
776 {
777  M.resize(12);
778  for (unsigned int i = 0; i < 12; i++)
779  M[i] = this->data[i];
780 }
781 
786 {
788  this->extract(tr);
789  return tr;
790 }
791 
796 {
798  this->extract(R);
799  return R;
800 }
801 
807 {
808  vpThetaUVector tu;
809  this->extract(tu);
810  return tu;
811 }
812 
842 {
843  if (j >= getCols())
844  throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the homogeneous matrix"));
845  unsigned int nb_rows = getRows();
846  vpColVector c(nb_rows);
847  for (unsigned int i = 0; i < nb_rows; i++)
848  c[i] = (*this)[i][j];
849  return c;
850 }
851 
861 vpHomogeneousMatrix vpHomogeneousMatrix::mean(const std::vector<vpHomogeneousMatrix> &vec_M)
862 {
863  vpMatrix meanR(3, 3);
864  vpColVector meanT(3);
866  for (size_t i = 0; i < vec_M.size(); i++) {
867  R = vec_M[i].getRotationMatrix();
868  meanR += (vpMatrix) R;
869  meanT += (vpColVector) vec_M[i].getTranslationVector();
870  }
871  meanR /= static_cast<double>(vec_M.size());
872  meanT /= static_cast<double>(vec_M.size());
873 
874  // Euclidean mean of the rotation matrix following Moakher's method (SIAM 2002)
875  vpMatrix M, U, V;
876  vpColVector sv;
877  meanR.pseudoInverse(M, sv, 1e-6, U, V);
878  double det = sv[0]*sv[1]*sv[2];
879  if (det > 0) {
880  meanR = U * V.t();
881  }
882  else {
883  vpMatrix D(3,3);
884  D = 0.0;
885  D[0][0] = D[1][1] = 1.0; D[2][2] = -1;
886  meanR = U * D * V.t();
887  }
888 
889  R = meanR;
890 
891  vpTranslationVector t(meanT);
893  return mean;
894 }
895 
896 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
897 
905 
906 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2206
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
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
void extract(vpRotationMatrix &R) const
void set_Y(double Y)
Set the point Y coordinate in the camera frame.
Definition: vpPoint.cpp:456
vpThetaUVector getThetaUVector() const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
vpRotationMatrix t() const
void load(std::ifstream &f)
double get_W() const
Get the point W coordinate in the camera frame.
Definition: vpPoint.cpp:419
vpColVector getCol(unsigned int j) const
Class that defines what is a point.
Definition: vpPoint.h:58
Implementation of a rotation matrix and operations on such kind of matrices.
unsigned int getCols() const
Definition: vpArray2D.h:279
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:507
void set_W(double W)
Set the point W coordinate in the camera frame.
Definition: vpPoint.cpp:460
void set_oY(double oY)
Set the point Y coordinate in the object frame.
Definition: vpPoint.cpp:465
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 W coordinate in the object frame.
Definition: vpPoint.cpp:469
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void set_oZ(double oZ)
Set the point Z coordinate in the object frame.
Definition: vpPoint.cpp:467
vpTranslationVector getTranslationVector() const
double get_X() const
Get the point X coordinate in the camera frame.
Definition: vpPoint.cpp:413
void set_Z(double Z)
Set the point Z coordinate in the camera frame.
Definition: vpPoint.cpp:458
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void set_oX(double oX)
Set the point X coordinate in the object frame.
Definition: vpPoint.cpp:463
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 Z coordinate in the camera frame.
Definition: vpPoint.cpp:417
double get_Y() const
Get the point Y coordinate in the camera frame.
Definition: vpPoint.cpp:415
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
void set_X(double X)
Set the point X coordinate in the camera frame.
Definition: vpPoint.cpp:454