Visual Servoing Platform  version 3.6.1 under development (2024-06-18)
vpHomogeneousMatrix.h
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Homogeneous matrix.
32  */
33 
39 #ifndef _vpHomogeneousMatrix_h_
40 #define _vpHomogeneousMatrix_h_
41 
42 #include <fstream>
43 #include <vector>
44 
45 #include <visp3/core/vpConfig.h>
46 
48 
50 class vpPoseVector;
51 class vpMatrix;
52 class vpRotationMatrix;
53 class vpPoseVector;
54 class vpThetaUVector;
55 class vpQuaternionVector;
56 class vpPoint;
57 
59 
60 #include <visp3/core/vpArray2D.h>
61 #include <visp3/core/vpRotationMatrix.h>
62 #include <visp3/core/vpThetaUVector.h>
63 #include <visp3/core/vpPoseVector.h>
64 
65 #ifdef VISP_HAVE_NLOHMANN_JSON
66 #include <nlohmann/json.hpp>
67 #endif
68 
204 class VISP_EXPORT vpHomogeneousMatrix : public vpArray2D<double>
205 {
206 public:
212  explicit vpHomogeneousMatrix(const vpPoseVector &p);
213  explicit vpHomogeneousMatrix(const std::vector<float> &v);
214  explicit vpHomogeneousMatrix(const std::vector<double> &v);
215  vpHomogeneousMatrix(double tx, double ty, double tz, double tux, double tuy, double tuz);
216 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
217  explicit vpHomogeneousMatrix(const std::initializer_list<double> &list);
218 #endif
219 
220 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
221  vp_deprecated void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R);
222  vp_deprecated void buildFrom(const vpTranslationVector &t, const vpThetaUVector &tu);
223  vp_deprecated void buildFrom(const vpTranslationVector &t, const vpQuaternionVector &q);
224  vp_deprecated void buildFrom(const vpPoseVector &p);
225  vp_deprecated void buildFrom(const std::vector<float> &v);
226  vp_deprecated void buildFrom(const std::vector<double> &v);
227  vp_deprecated void buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz);
228 #endif
229  vpHomogeneousMatrix &build(const vpTranslationVector &t, const vpRotationMatrix &R);
230  vpHomogeneousMatrix &build(const vpTranslationVector &t, const vpThetaUVector &tu);
232  vpHomogeneousMatrix &build(const vpPoseVector &p);
233  vpHomogeneousMatrix &build(const std::vector<float> &v);
234  vpHomogeneousMatrix &build(const std::vector<double> &v);
235  vpHomogeneousMatrix &build(const double &tx, const double &ty, const double &tz, const double &tux, const double &tuy, const double &tuz);
236 
237  void convert(std::vector<float> &M);
238  void convert(std::vector<double> &M);
239 
240  // Set to identity
241  void eye();
242 
243  vpColVector getCol(unsigned int j) const;
244  vpRotationMatrix getRotationMatrix() const;
245  vpThetaUVector getThetaUVector() const;
246  vpTranslationVector getTranslationVector() const;
247 
248  // Invert the homogeneous matrix.
249  vpHomogeneousMatrix inverse() const;
250  // Invert the homogeneous matrix.
251  void inverse(vpHomogeneousMatrix &Mi) const;
252 
253  // Test if the rotational part of the matrix is a rotation matrix.
254  bool isAnHomogeneousMatrix(double threshold = 1e-6) const;
255  bool isValid() const;
256 
257  void insert(const vpRotationMatrix &R);
258  void insert(const vpThetaUVector &tu);
259  void insert(const vpTranslationVector &t);
260  void insert(const vpQuaternionVector &t);
261 
262  void extract(vpRotationMatrix &R) const;
263  void extract(vpThetaUVector &tu) const;
264  void extract(vpTranslationVector &t) const;
265  void extract(vpQuaternionVector &q) const;
266 
284  void load(std::ifstream &f);
285 
302  void load(const std::string &filename);
303 
325  void save(std::ofstream &f) const;
326 
347  void save(const std::string &filename) const;
348 
351  vpHomogeneousMatrix &operator*=(const vpHomogeneousMatrix &M);
352 
353  vpColVector operator*(const vpColVector &v) const;
356 
357  // Multiply by a point
358  vpPoint operator*(const vpPoint &bP) const;
359 
360  vpHomogeneousMatrix &operator<<(double val);
361  vpHomogeneousMatrix &operator,(double val);
362 
363  void orthogonalizeRotation();
364 
365  void print() const;
366 
372  void resize(unsigned int nrows, unsigned int ncols, bool flagNullify = true)
373  {
374  (void)nrows;
375  (void)ncols;
376  (void)flagNullify;
377  throw(vpException(vpException::fatalError, "Cannot resize an homogeneous matrix"));
378  }
379 
380  static vpHomogeneousMatrix compute3d3dTransformation(const std::vector<vpPoint> &p, const std::vector<vpPoint> &q);
381 
382  static vpHomogeneousMatrix mean(const std::vector<vpHomogeneousMatrix> &vec_M);
383 
384 #ifdef VISP_HAVE_NLOHMANN_JSON
385 public:
386  static const std::string jsonTypeName;
387 private:
388  friend void to_json(nlohmann::json &j, const vpHomogeneousMatrix &T);
389  friend void from_json(const nlohmann::json &j, vpHomogeneousMatrix &T);
390  // Conversion helper function to avoid circular dependencies and MSVC errors that are not exported in the DLL
391  void parse_json(const nlohmann::json &j);
392  void convert_to_json(nlohmann::json &j) const;
393 public:
394 
395 #endif
396 
397 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
406  vp_deprecated void init() { }
410  vp_deprecated void setIdentity();
412 #endif
413 
414 protected:
415  unsigned int m_index;
416 };
417 
418 #ifdef VISP_HAVE_NLOHMANN_JSON
419 inline void to_json(nlohmann::json &j, const vpHomogeneousMatrix &T)
420 {
421  T.convert_to_json(j);
422 }
423 
424 inline void from_json(const nlohmann::json &j, vpHomogeneousMatrix &T)
425 {
426  T.parse_json(j);
427 }
428 #endif
430 #endif
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:127
void insert(const vpArray2D< Type > &A, unsigned int r, unsigned int c)
Definition: vpArray2D.h:487
friend void to_json(nlohmann::json &j, const vpArray2D< T > &array)
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:604
static bool load(const std::string &filename, vpArray2D< Type > &A, bool binary=false, char *header=nullptr)
Definition: vpArray2D.h:659
vpArray2D< Type > & operator=(Type x)
Set all the elements of the array to x.
Definition: vpArray2D.h:516
static bool save(const std::string &filename, const vpArray2D< Type > &A, bool binary=false, const char *header="")
Definition: vpArray2D.h:861
friend void from_json(const nlohmann::json &j, vpArray2D< T > &array)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:171
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ fatalError
Fatal error.
Definition: vpException.h:72
Implementation of an homogeneous matrix and operations on such kind of matrices.
static const std::string jsonTypeName
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true)
vp_deprecated void init()
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:151
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:187
Implementation of a rotation vector as quaternion angle minimal representation.
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.
vpMatrix operator*(const double &x, const vpMatrix &A)