Main MRPT website > C++ reference for MRPT 1.5.9
SE_traits.cpp
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 
10 #include "base-precomp.h" // Precompiled headers
11 
12 #include <mrpt/poses/SE_traits.h>
13 
14 using namespace mrpt;
15 using namespace mrpt::math;
16 using namespace mrpt::utils;
17 using namespace mrpt::poses;
18 
19 /** A pseudo-Logarithm map in SE(3), where the output = [X,Y,Z, Ln(ROT)], that is, the normal
20  * SO(3) logarithm is used for the rotation components, but the translation is left unmodified.
21  */
22 void SE_traits<3>::pseudo_ln(const CPose3D &P, array_t &x)
23 {
24  x[0] = P.m_coords[0];
25  x[1] = P.m_coords[1];
26  x[2] = P.m_coords[2];
27  CArrayDouble<3> ln_rot = P.ln_rotation();
28  x[3] = ln_rot[0];
29  x[4] = ln_rot[1];
30  x[5] = ln_rot[2];
31 }
32 
33 
34 /** Return one or both of the following 6x6 Jacobians, useful in graph-slam problems...
35  */
37  const CPose3D &P1DP2inv,
38  matrix_VxV_t *df_de1,
39  matrix_VxV_t *df_de2)
40 {
41  const CMatrixDouble33 & R = P1DP2inv.getRotationMatrix(); // The rotation matrix.
42 
43  // Common part: d_Ln(R)_dR:
45  CPose3D::ln_rot_jacob(R, dLnRot_dRot);
46 
47  if (df_de1)
48  {
49  matrix_VxV_t & J1 = *df_de1;
50  // This Jacobian has the structure:
51  // [ I_3 | -[d_t]_x ]
52  // Jacob1 = [ ---------+------------------- ]
53  // [ 0_3x3 | dLnR_dR * (...) ]
54  //
55  J1.zeros();
56  J1(0,0) = 1;
57  J1(1,1) = 1;
58  J1(2,2) = 1;
59 
60  J1(0,4) = P1DP2inv.z(); J1(0,5) = -P1DP2inv.y();
61  J1(1,3) = -P1DP2inv.z(); J1(1,5) = P1DP2inv.x();
62  J1(2,3) = P1DP2inv.y(); J1(2,4) =-P1DP2inv.x();
63 
64  MRPT_ALIGN16 const double aux_vals[] = {
65  0, R(2,0), -R(1,0),
66  -R(2,0), 0, R(0,0),
67  R(1,0), -R(0,0), 0,
68  // -----------------------
69  0, R(2,1), -R(1,1),
70  -R(2,1), 0, R(0,1),
71  R(1,1), -R(0,1), 0,
72  // -----------------------
73  0, R(2,2), -R(1,2),
74  -R(2,2), 0, R(0,2),
75  R(1,2), -R(0,2), 0
76  };
77  const CMatrixFixedNumeric<double,9,3> aux(aux_vals);
78 
79  // right-bottom part = dLnRot_dRot * aux
80  J1.block(3,3,3,3) = (dLnRot_dRot * aux).eval();
81  }
82  if (df_de2)
83  {
84  // This Jacobian has the structure:
85  // [ -R | 0_3x3 ]
86  // Jacob2 = [ ---------+------------------- ]
87  // [ 0_3x3 | dLnR_dR * (...) ]
88  //
89  matrix_VxV_t & J2 = *df_de2;
90  J2.zeros();
91 
92  for (int i=0;i<3;i++)
93  for (int j=0;j<3;j++)
94  J2.set_unsafe(i,j, -R.get_unsafe(i,j));
95 
96  MRPT_ALIGN16 const double aux_vals[] = {
97  0, R(0,2), -R(0,1),
98  0, R(1,2), -R(1,1),
99  0, R(2,2), -R(2,1),
100  // -----------------------
101  -R(0,2), 0, R(0,0),
102  -R(1,2), 0, R(1,0),
103  -R(2,2), 0, R(2,0),
104  // -----------------------
105  R(0,1), -R(0,0), 0,
106  R(1,1), -R(1,0), 0,
107  R(2,1), -R(2,0), 0
108  };
109  const CMatrixFixedNumeric<double,9,3> aux(aux_vals);
110 
111  // right-bottom part = dLnRot_dRot * aux
112  J2.block(3,3,3,3) = (dLnRot_dRot * aux).eval();
113  }
114 }
115 
117  const CPose3D& Dinv, const CPose3D& P1, const CPose3D& P2,
118  matrix_VxV_t* df_de1, matrix_VxV_t* df_de2)
119 {
120  MRPT_TODO("Implement me!");
121  THROW_EXCEPTION("Implement me!");
122 }
123 
124 
125 /** Return one or both of the following 6x6 Jacobians, useful in graph-slam problems...
126  */
128  const CPose2D &P1DP2inv,
129  matrix_VxV_t *df_de1,
130  matrix_VxV_t *df_de2)
131 {
132  if (df_de1)
133  {
134  matrix_VxV_t & J1 = *df_de1;
135  // This Jacobian has the structure:
136  // [ I_2 | -[d_t]_x ]
137  // Jacob1 = [ ---------+--------------- ]
138  // [ 0 | 1 ]
139  //
140  J1.unit(VECTOR_SIZE,1.0);
141  J1(0,2) = -P1DP2inv.y();
142  J1(1,2) = P1DP2inv.x();
143  }
144  if (df_de2)
145  {
146  // This Jacobian has the structure:
147  // [ -R | 0 ]
148  // Jacob2 = [ ---------+------- ]
149  // [ 0 | -1 ]
150  //
151  matrix_VxV_t & J2 = *df_de2;
152 
153  const double ccos = cos(P1DP2inv.phi());
154  const double csin = sin(P1DP2inv.phi());
155 
156  const double vals[] = {
157  -ccos, csin, 0,
158  -csin,-ccos, 0,
159  0, 0, -1
160  };
162  }
163 }
164 
166  const CPose2D& Dinv, const CPose2D& P1, const CPose2D& P2,
167  matrix_VxV_t* df_de1, matrix_VxV_t* df_de2)
168 {
169  using mrpt::math::TPoint2D;
170  const double phi1 = P1.phi();
171 
172  const TPoint2D dt(P2.x() - P1.x(), P2.y() - P1.y());
173  const double si = std::sin(phi1), ci = std::cos(phi1);
174 
175  CMatrixDouble22 RotDinv;
176  Dinv.getRotationMatrix(RotDinv);
177  CMatrixDouble33 K; // zeros
178  K.block<2, 2>(0, 0) = RotDinv;
179  K(2, 2) = 1.0;
180 
181  if (df_de1)
182  {
183  (*df_de1)(0, 0) = -ci;
184  (*df_de1)(0, 1) = -si;
185  (*df_de1)(0, 2) = -si * dt.x + ci * dt.y;
186  (*df_de1)(1, 0) = si;
187  (*df_de1)(1, 1) = -ci;
188  (*df_de1)(1, 2) = -ci * dt.x - si * dt.y;
189  (*df_de1)(2, 0) = 0;
190  (*df_de1)(2, 1) = 0;
191  (*df_de1)(2, 2) = -1;
192  (*df_de1) = K * (*df_de1);
193  }
194 
195  if (df_de2)
196  {
197  (*df_de2)(0, 0) = ci;
198  (*df_de2)(0, 1) = si;
199  (*df_de2)(0, 2) = 0;
200  (*df_de2)(1, 0) = -si;
201  (*df_de2)(1, 1) = ci;
202  (*df_de2)(1, 2) = 0;
203  (*df_de2)(2, 0) = 0;
204  (*df_de2)(2, 1) = 0;
205  (*df_de2)(2, 2) = 1;
206  (*df_de2) = K * (*df_de2);
207  }
208 }
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
#define THROW_EXCEPTION(msg)
mrpt::math::CArrayDouble< 3 > ln_rotation() const
Take the logarithm of the 3x3 rotation matrix, generating the corresponding vector in the Lie Algebra...
Definition: CPose3D.cpp:793
#define MRPT_TODO(x)
static void jacobian_dP1DP2inv_depsilon(const CPose3D &P1DP2inv, matrix_VxV_t *df_de1, matrix_VxV_t *df_de2)
Return one or both of the following 6x6 Jacobians, useful in graph-slam problems: With and being ...
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods...
Definition: CPose3D.h:81
void getRotationMatrix(mrpt::math::CMatrixDouble22 &R) const
Returns the SE(2) 2x2 rotation matrix.
Definition: CPose2D.cpp:139
static void jacobian_dDinvP1invP2_depsilon(const CPose2D &Dinv, const CPose2D &P1, const CPose2D &P2, matrix_VxV_t *df_de1, matrix_VxV_t *df_de2)
Return one or both of the following 3x3 Jacobians, useful in graph-slam problems: With and being ...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
static void jacobian_dDinvP1invP2_depsilon(const CPose3D &Dinv, const CPose3D &P1, const CPose3D &P2, matrix_VxV_t *df_de1, matrix_VxV_t *df_de2)
Return one or both of the following 3x3 Jacobians, useful in graph-slam problems: With and being ...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
static void ln_rot_jacob(const mrpt::math::CMatrixDouble33 &R, mrpt::math::CMatrixFixedNumeric< double, 3, 9 > &M)
Static function to compute the Jacobian of the SO(3) Logarithm function, evaluated at a given 3x3 rot...
Definition: CPose3D.cpp:977
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
static void pseudo_ln(const CPose3D &P, array_t &x)
A pseudo-Logarithm map in SE(3), where the output = [X,Y,Z, Ln(ROT)], that is, the normal SO(3) logar...
GLenum GLint x
Definition: glext.h:3516
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:176
Lightweight 2D point.
#define MRPT_ALIGN16
static void jacobian_dP1DP2inv_depsilon(const CPose2D &P1DP2inv, matrix_VxV_t *df_de1, matrix_VxV_t *df_de2)
Return one or both of the following 3x3 Jacobians, useful in graph-slam problems: With and being ...



Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020