Main MRPT website > C++ reference for MRPT 1.4.0
homog_matrices.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef MRPT_HOMOG_MATRICES_H
10 #define MRPT_HOMOG_MATRICES_H
11 
12 #include <mrpt/config.h>
14 #include <mrpt/utils/boost_join.h>
15 #include <mrpt/utils/mrpt_macros.h>
16 
17 namespace mrpt
18 {
19  namespace math
20  {
21 
22  /** Efficiently compute the inverse of a 4x4 homogeneous matrix by only transposing the rotation 3x3 part and solving the translation with dot products.
23  * This is a generic template which works with:
24  * MATRIXLIKE: CMatrixTemplateNumeric, CMatrixFixedNumeric
25  */
26  template <class MATRIXLIKE1,class MATRIXLIKE2>
27  void homogeneousMatrixInverse(const MATRIXLIKE1 &M, MATRIXLIKE2 &out_inverse_M)
28  {
30  ASSERT_( M.isSquare() && size(M,1)==4);
31 
32  /* Instead of performing a generic 4x4 matrix inversion, we only need to
33  transpose the rotation part, then replace the translation part by
34  three dot products. See, for example:
35  https://graphics.stanford.edu/courses/cs248-98-fall/Final/q4.html
36 
37  [ux vx wx tx] -1 [ux uy uz -dot(u,t)]
38  [uy vy wy ty] [vx vy vz -dot(v,t)]
39  [uz vz wz tz] = [wx wy wz -dot(w,t)]
40  [ 0 0 0 1] [ 0 0 0 1 ]
41  */
42 
43  out_inverse_M.setSize(4,4);
44 
45  // 3x3 rotation part:
46  out_inverse_M.set_unsafe(0,0, M.get_unsafe(0,0));
47  out_inverse_M.set_unsafe(0,1, M.get_unsafe(1,0));
48  out_inverse_M.set_unsafe(0,2, M.get_unsafe(2,0));
49 
50  out_inverse_M.set_unsafe(1,0, M.get_unsafe(0,1));
51  out_inverse_M.set_unsafe(1,1, M.get_unsafe(1,1));
52  out_inverse_M.set_unsafe(1,2, M.get_unsafe(2,1));
53 
54  out_inverse_M.set_unsafe(2,0, M.get_unsafe(0,2));
55  out_inverse_M.set_unsafe(2,1, M.get_unsafe(1,2));
56  out_inverse_M.set_unsafe(2,2, M.get_unsafe(2,2));
57 
58  const double tx = -M.get_unsafe(0,3);
59  const double ty = -M.get_unsafe(1,3);
60  const double tz = -M.get_unsafe(2,3);
61 
62  const double tx_ = tx*M.get_unsafe(0,0)+ty*M.get_unsafe(1,0)+tz*M.get_unsafe(2,0);
63  const double ty_ = tx*M.get_unsafe(0,1)+ty*M.get_unsafe(1,1)+tz*M.get_unsafe(2,1);
64  const double tz_ = tx*M.get_unsafe(0,2)+ty*M.get_unsafe(1,2)+tz*M.get_unsafe(2,2);
65 
66  out_inverse_M.set_unsafe(0,3, tx_ );
67  out_inverse_M.set_unsafe(1,3, ty_ );
68  out_inverse_M.set_unsafe(2,3, tz_ );
69 
70  out_inverse_M.set_unsafe(3,0, 0);
71  out_inverse_M.set_unsafe(3,1, 0);
72  out_inverse_M.set_unsafe(3,2, 0);
73  out_inverse_M.set_unsafe(3,3, 1);
74 
75  MRPT_END
76  }
77  //! \overload
78  template <class IN_ROTMATRIX,class IN_XYZ, class OUT_ROTMATRIX, class OUT_XYZ>
80  const IN_ROTMATRIX & in_R,
81  const IN_XYZ & in_xyz,
82  OUT_ROTMATRIX & out_R,
83  OUT_XYZ & out_xyz
84  )
85  {
87  ASSERT_( in_R.isSquare() && size(in_R,1)==3 && in_xyz.size()==3)
88  out_R.setSize(3,3);
89  out_xyz.resize(3);
90 
91  // translation part:
92  typedef typename IN_ROTMATRIX::Scalar T;
93  const T tx = -in_xyz[0];
94  const T ty = -in_xyz[1];
95  const T tz = -in_xyz[2];
96 
97  out_xyz[0] = tx*in_R.get_unsafe(0,0)+ty*in_R.get_unsafe(1,0)+tz*in_R.get_unsafe(2,0);
98  out_xyz[1] = tx*in_R.get_unsafe(0,1)+ty*in_R.get_unsafe(1,1)+tz*in_R.get_unsafe(2,1);
99  out_xyz[2] = tx*in_R.get_unsafe(0,2)+ty*in_R.get_unsafe(1,2)+tz*in_R.get_unsafe(2,2);
100 
101  // 3x3 rotation part: transpose
102  out_R = in_R.adjoint();
103 
104  MRPT_END
105  }
106  //! \overload
107  template <class MATRIXLIKE>
108  inline void homogeneousMatrixInverse(MATRIXLIKE &M)
109  {
110  ASSERTDEB_( M.isSquare() && size(M,1)==4);
111  // translation:
112  const double tx = -M(0,3);
113  const double ty = -M(1,3);
114  const double tz = -M(2,3);
115  M(0,3) = tx*M(0,0)+ty*M(1,0)+tz*M(2,0);
116  M(1,3) = tx*M(0,1)+ty*M(1,1)+tz*M(2,1);
117  M(2,3) = tx*M(0,2)+ty*M(1,2)+tz*M(2,2);
118  // 3x3 rotation part:
119  double t; // avoid std::swap() to avoid <algorithm> only for that.
120  t=M(1,0); M(1,0) = M(0,1); M(0,1)=t;
121  t=M(2,0); M(2,0) = M(0,2); M(0,2)=t;
122  t=M(1,2); M(1,2) = M(2,1); M(2,1)=t;
123  }
124 
125  }
126 }
127 #endif
EIGEN_STRONG_INLINE const AdjointReturnType t() const
Transpose.
#define MRPT_START
Definition: mrpt_macros.h:349
#define ASSERT_(f)
Definition: mrpt_macros.h:261
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: mrpt_macros.h:283
#define MRPT_END
Definition: mrpt_macros.h:353
size_t size(const MATRIXLIKE &m, int dim)
Definition: bits.h:38
void homogeneousMatrixInverse(const MATRIXLIKE1 &M, MATRIXLIKE2 &out_inverse_M)
Efficiently compute the inverse of a 4x4 homogeneous matrix by only transposing the rotation 3x3 part...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.



Page generated by Doxygen 1.9.1 for MRPT 1.4.0 SVN: at Mon Apr 18 03:56:21 UTC 2022