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>
16
17namespace 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
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
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
#define ASSERT_(f)
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
#define MRPT_END
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.7 for MRPT 1.4.0 SVN: at Tue Jun 27 15:23:24 UTC 2023