Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 #pragma once
10 
11 #include <mrpt/core/exceptions.h>
12 
13 namespace mrpt
14 {
15 namespace math
16 {
17 /** Efficiently compute the inverse of a 4x4 homogeneous matrix by only
18  * transposing the rotation 3x3 part and solving the translation with dot
19  * products.
20  * This is a generic template which works with:
21  * MATRIXLIKE: CMatrixTemplateNumeric, CMatrixFixedNumeric
22  */
23 template <class MATRIXLIKE1, class MATRIXLIKE2>
24 void homogeneousMatrixInverse(const MATRIXLIKE1& M, MATRIXLIKE2& out_inverse_M)
25 {
27  ASSERT_(M.isSquare() && M.rows() == 4);
28 
29  /* Instead of performing a generic 4x4 matrix inversion, we only need to
30  transpose the rotation part, then replace the translation part by
31  three dot products. See, for example:
32  https://graphics.stanford.edu/courses/cs248-98-fall/Final/q4.html
33 
34  [ux vx wx tx] -1 [ux uy uz -dot(u,t)]
35  [uy vy wy ty] [vx vy vz -dot(v,t)]
36  [uz vz wz tz] = [wx wy wz -dot(w,t)]
37  [ 0 0 0 1] [ 0 0 0 1 ]
38  */
39 
40  out_inverse_M.setSize(4, 4);
41 
42  // 3x3 rotation part:
43  out_inverse_M.set_unsafe(0, 0, M.get_unsafe(0, 0));
44  out_inverse_M.set_unsafe(0, 1, M.get_unsafe(1, 0));
45  out_inverse_M.set_unsafe(0, 2, M.get_unsafe(2, 0));
46 
47  out_inverse_M.set_unsafe(1, 0, M.get_unsafe(0, 1));
48  out_inverse_M.set_unsafe(1, 1, M.get_unsafe(1, 1));
49  out_inverse_M.set_unsafe(1, 2, M.get_unsafe(2, 1));
50 
51  out_inverse_M.set_unsafe(2, 0, M.get_unsafe(0, 2));
52  out_inverse_M.set_unsafe(2, 1, M.get_unsafe(1, 2));
53  out_inverse_M.set_unsafe(2, 2, M.get_unsafe(2, 2));
54 
55  const double tx = -M.get_unsafe(0, 3);
56  const double ty = -M.get_unsafe(1, 3);
57  const double tz = -M.get_unsafe(2, 3);
58 
59  const double tx_ = tx * M.get_unsafe(0, 0) + ty * M.get_unsafe(1, 0) +
60  tz * M.get_unsafe(2, 0);
61  const double ty_ = tx * M.get_unsafe(0, 1) + ty * M.get_unsafe(1, 1) +
62  tz * M.get_unsafe(2, 1);
63  const double tz_ = tx * M.get_unsafe(0, 2) + ty * M.get_unsafe(1, 2) +
64  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, const IN_XYZ& in_xyz, OUT_ROTMATRIX& out_R,
81  OUT_XYZ& out_xyz)
82 {
84  ASSERT_(in_R.isSquare() && in_R.rows() == 3 && in_xyz.size() == 3);
85  out_R.setSize(3, 3);
86  out_xyz.resize(3);
87 
88  // translation part:
89  using T = typename IN_ROTMATRIX::Scalar;
90  const T tx = -in_xyz[0];
91  const T ty = -in_xyz[1];
92  const T tz = -in_xyz[2];
93 
94  out_xyz[0] = tx * in_R.get_unsafe(0, 0) + ty * in_R.get_unsafe(1, 0) +
95  tz * in_R.get_unsafe(2, 0);
96  out_xyz[1] = tx * in_R.get_unsafe(0, 1) + ty * in_R.get_unsafe(1, 1) +
97  tz * in_R.get_unsafe(2, 1);
98  out_xyz[2] = tx * in_R.get_unsafe(0, 2) + ty * in_R.get_unsafe(1, 2) +
99  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.cols() == M.rows() && M.rows() == 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);
121  M(1, 0) = M(0, 1);
122  M(0, 1) = t;
123  t = M(2, 0);
124  M(2, 0) = M(0, 2);
125  M(0, 2) = t;
126  t = M(1, 2);
127  M(1, 2) = M(2, 1);
128  M(2, 1) = t;
129 }
130 }
131 }
exceptions.h
t
GLdouble GLdouble t
Definition: glext.h:3689
mrpt::math::homogeneousMatrixInverse
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...
Definition: homog_matrices.h:24
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
Scalar
double Scalar
Definition: KmUtils.h:44
tz
GLbyte GLbyte tz
Definition: glext.h:6092
ty
GLbyte ty
Definition: glext.h:6092
MRPT_START
#define MRPT_START
Definition: exceptions.h:262
MRPT_END
#define MRPT_END
Definition: exceptions.h:266
ASSERTDEB_
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:205



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST