MRPT  1.9.9
homog_matrices.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/core/exceptions.h>
12 
13 namespace mrpt::math
14 {
15 /** Efficiently compute the inverse of a 4x4 homogeneous matrix by only
16  * transposing the rotation 3x3 part and solving the translation with dot
17  * products.
18  * This is a generic template which works with:
19  * MATRIXLIKE: CMatrixDynamic, CMatrixFixed
20  */
21 template <class MATRIXLIKE1, class MATRIXLIKE2>
22 void homogeneousMatrixInverse(const MATRIXLIKE1& M, MATRIXLIKE2& out_inverse_M)
23 {
25  ASSERT_(M.isSquare() && M.rows() == 4);
26 
27  /* Instead of performing a generic 4x4 matrix inversion, we only need to
28  transpose the rotation part, then replace the translation part by
29  three dot products. See, for example:
30  https://graphics.stanford.edu/courses/cs248-98-fall/Final/q4.html
31 
32  [ux vx wx tx] -1 [ux uy uz -dot(u,t)]
33  [uy vy wy ty] [vx vy vz -dot(v,t)]
34  [uz vz wz tz] = [wx wy wz -dot(w,t)]
35  [ 0 0 0 1] [ 0 0 0 1 ]
36  */
37 
38  out_inverse_M.setSize(4, 4);
39 
40  // 3x3 rotation part:
41  out_inverse_M(0, 0, M(0) = 0);
42  out_inverse_M(0, 1, M(1) = 0);
43  out_inverse_M(0, 2, M(2) = 0);
44 
45  out_inverse_M(1, 0, M(0) = 1);
46  out_inverse_M(1, 1, M(1) = 1);
47  out_inverse_M(1, 2, M(2) = 1);
48 
49  out_inverse_M(2, 0, M(0) = 2);
50  out_inverse_M(2, 1, M(1) = 2);
51  out_inverse_M(2, 2, M(2) = 2);
52 
53  const double tx = -M(0, 3);
54  const double ty = -M(1, 3);
55  const double tz = -M(2, 3);
56 
57  const double tx_ = tx * M(0, 0) + ty * M(1, 0) + tz * M(2, 0);
58  const double ty_ = tx * M(0, 1) + ty * M(1, 1) + tz * M(2, 1);
59  const double tz_ = tx * M(0, 2) + ty * M(1, 2) + tz * M(2, 2);
60 
61  out_inverse_M(0, 3) = tx_;
62  out_inverse_M(1, 3) = ty_;
63  out_inverse_M(2, 3) = tz_;
64 
65  out_inverse_M(3, 0) = 0;
66  out_inverse_M(3, 1) = 0;
67  out_inverse_M(3, 2) = 0;
68  out_inverse_M(3, 3) = 1;
69 
70  MRPT_END
71 }
72 //! \overload
73 template <class IN_ROTMATRIX, class IN_XYZ, class OUT_ROTMATRIX, class OUT_XYZ>
75  const IN_ROTMATRIX& in_R, const IN_XYZ& in_xyz, OUT_ROTMATRIX& out_R,
76  OUT_XYZ& out_xyz)
77 {
79  ASSERT_(in_R.isSquare() && in_R.rows() == 3 && in_xyz.size() == 3);
80  out_R.setSize(3, 3);
81  out_xyz.resize(3);
82 
83  // translation part:
84  using T = typename IN_ROTMATRIX::Scalar;
85  const T tx = -in_xyz[0];
86  const T ty = -in_xyz[1];
87  const T tz = -in_xyz[2];
88 
89  out_xyz[0] = tx * in_R(0, 0) + ty * in_R(1, 0) + tz * in_R(2, 0);
90  out_xyz[1] = tx * in_R(0, 1) + ty * in_R(1, 1) + tz * in_R(2, 1);
91  out_xyz[2] = tx * in_R(0, 2) + ty * in_R(1, 2) + tz * in_R(2, 2);
92 
93  // 3x3 rotation part: transpose
94  out_R = in_R.transpose();
95 
96  MRPT_END
97 }
98 //! \overload
99 template <class MATRIXLIKE>
100 inline void homogeneousMatrixInverse(MATRIXLIKE& M)
101 {
102  ASSERTDEB_(M.cols() == M.rows() && M.rows() == 4);
103  // translation:
104  const double tx = -M(0, 3);
105  const double ty = -M(1, 3);
106  const double tz = -M(2, 3);
107  M(0, 3) = tx * M(0, 0) + ty * M(1, 0) + tz * M(2, 0);
108  M(1, 3) = tx * M(0, 1) + ty * M(1, 1) + tz * M(2, 1);
109  M(2, 3) = tx * M(0, 2) + ty * M(1, 2) + tz * M(2, 2);
110  // 3x3 rotation part:
111  double t; // avoid std::swap() to avoid <algorithm> only for that.
112  t = M(1, 0);
113  M(1, 0) = M(0, 1);
114  M(0, 1) = t;
115  t = M(2, 0);
116  M(2, 0) = M(0, 2);
117  M(0, 2) = t;
118  t = M(1, 2);
119  M(1, 2) = M(2, 1);
120  M(2, 1) = t;
121 }
122 } // namespace mrpt::math
double Scalar
Definition: KmUtils.h:43
#define MRPT_START
Definition: exceptions.h:241
GLdouble GLdouble t
Definition: glext.h:3695
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
GLbyte ty
Definition: glext.h:6179
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 ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:190
#define MRPT_END
Definition: exceptions.h:245
GLbyte GLbyte tz
Definition: glext.h:6179



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 765b969e7 Sun Sep 22 19:55:28 2019 +0200 at dom sep 22 20:00:14 CEST 2019