Main MRPT website > C++ reference for MRPT 1.5.5
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 
116 
117 
118 /** Return one or both of the following 6x6 Jacobians, useful in graph-slam problems...
119  */
121  const CPose2D &P1DP2inv,
122  matrix_VxV_t *df_de1,
123  matrix_VxV_t *df_de2)
124 {
125  if (df_de1)
126  {
127  matrix_VxV_t & J1 = *df_de1;
128  // This Jacobian has the structure:
129  // [ I_2 | -[d_t]_x ]
130  // Jacob1 = [ ---------+--------------- ]
131  // [ 0 | 1 ]
132  //
133  J1.unit(VECTOR_SIZE,1.0);
134  J1(0,2) = -P1DP2inv.y();
135  J1(1,2) = P1DP2inv.x();
136  }
137  if (df_de2)
138  {
139  // This Jacobian has the structure:
140  // [ -R | 0 ]
141  // Jacob2 = [ ---------+------- ]
142  // [ 0 | -1 ]
143  //
144  matrix_VxV_t & J2 = *df_de2;
145 
146  const double ccos = cos(P1DP2inv.phi());
147  const double csin = sin(P1DP2inv.phi());
148 
149  const double vals[] = {
150  -ccos, csin, 0,
151  -csin,-ccos, 0,
152  0, 0, -1
153  };
155  }
156 }
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
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:781
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
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
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:965
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
#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.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019