Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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
GLdouble GLdouble t
Definition: glext.h:3610
size_t size(const MATRIXLIKE &m, const int dim)
Definition: bits.h:43
#define MRPT_END
GLbyte ty
Definition: glext.h:5441
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
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...
#define ASSERT_(f)
GLbyte GLbyte tz
Definition: glext.h:5441
double Scalar
Definition: KmUtils.h:41



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019