MRPT  2.0.1
SE.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, 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 
10 #include "poses-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/geometry.h>
13 #include <mrpt/poses/CPose2D.h>
14 #include <mrpt/poses/CPose3D.h>
15 #include <mrpt/poses/Lie/SE.h>
16 #include <mrpt/poses/Lie/SO.h>
17 #include <Eigen/Dense>
18 
19 using namespace mrpt;
20 using namespace mrpt::math;
21 using namespace mrpt::poses;
22 using namespace mrpt::poses::Lie;
23 
24 // See .h for documentation
25 
26 // ====== SE(3) ===========
28 {
29  return CPose3D(Lie::SO<3>::exp(x.tail<3>()), x.head<3>());
30 }
31 
33 {
34  tangent_vector v;
35  const auto log_R = mrpt::poses::Lie::SO<3>::log(P.getRotationMatrix());
36  v[0] = P.x();
37  v[1] = P.y();
38  v[2] = P.z();
39  v[3] = log_R[0];
40  v[4] = log_R[1];
41  v[5] = log_R[2];
42  return v;
43 }
44 
46 {
47  manifold_vector v;
48  pose.getAs12Vector(v);
49  return v;
50 }
51 
53 {
54  return type(v);
55 }
56 
57 // See 10.3.1 in \cite blanco_se3_tutorial
59 {
60  // 12x6 Jacobian:
61  tang2mat_jacob J = tang2mat_jacob::Zero();
62  J.block<3, 3>(9, 0) = Eigen::Matrix3d::Identity();
63  const auto w = SO<3>::tangent_vector(x.tail<3>());
64  J.block<9, 3>(0, 3) = SO<3>::jacob_dexpe_de(w).asEigen();
65  return J;
66 }
67 
69 {
71  J.setZero();
72  const CMatrixDouble33& R = P.getRotationMatrix();
73  J.block<3, 9>(3, 0) = SO<3>::jacob_dlogv_dv(R).asEigen();
74  J(0, 9) = J(1, 10) = J(2, 11) = 1.0;
75  return J;
76 }
77 
78 // Section 10.3.3 in tech report
79 // http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
81 {
83  jacob.block<9, 3>(0, 0).setZero();
84  jacob.block<3, 3>(9, 0).setIdentity();
85  for (int i = 0; i < 3; i++)
86  {
87  auto trg_blc = jacob.block<3, 3>(3 * i, 3);
89  D.getRotationMatrix().blockCopy<3, 1>(0, i), trg_blc);
90  }
91  {
92  auto trg_blc = jacob.block<3, 3>(9, 3);
94  }
95  return jacob;
96 }
97 
98 // Section 10.3.4 in tech report
99 // http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
101 {
103  const auto& dRot = D.getRotationMatrix();
104  jacob.setZero();
105  jacob.block<3, 3>(9, 0) = dRot.asEigen();
106 
107  jacob.block<3, 1>(3, 5) = -dRot.col(0);
108  jacob.block<3, 1>(6, 4) = dRot.col(0);
109 
110  jacob.block<3, 1>(0, 5) = dRot.col(1);
111  jacob.block<3, 1>(6, 3) = -dRot.col(1);
112 
113  jacob.block<3, 1>(0, 4) = -dRot.col(2);
114  jacob.block<3, 1>(3, 3) = dRot.col(2);
115  return jacob;
116 }
117 
118 // Eq. 10.3.7 in tech report
119 // http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
121  const CPose3D& A, const CPose3D& D)
122 {
123  const auto& Arot = A.getRotationMatrix();
124 
126  jacob.block<9, 3>(0, 0).setZero();
127  jacob.block<3, 3>(9, 0) = A.getRotationMatrix().asEigen();
129  for (int i = 0; i < 3; i++)
130  {
132  D.getRotationMatrix().blockCopy<3, 1>(0, i), aux);
133  jacob.block<3, 3>(3 * i, 3) = Arot.asEigen() * aux;
134  }
136  jacob.block<3, 3>(9, 3) = Arot.asEigen() * aux;
137  return jacob;
138 }
139 
141  const CPose3D& Dinv, const CPose3D& P1, const CPose3D& P2,
144 {
145  using namespace mrpt::math;
146 
147  // The rotation matrix of the overall error expression:
148  const CPose3D P1inv = -P1;
149  const CPose3D DinvP1invP2 = Dinv + P1inv + P2;
150 
151  // Common part: d_PseudoLn(T)_dT:
152  // (6x12 matrix)
153  const auto dLnT_dT = SE<3>::jacob_dlogv_dv(DinvP1invP2);
154 
155  // See section 10.3.10 of Tech. report:
156  // "A tutorial on SE(3) transformation parameterizations and on-manifold
157  // optimization"
158  if (df_de1)
159  {
160  matrix_TxT& J1 = df_de1.value().get();
161 
162  const CMatrixFixed<double, 12, 12> J1a =
163  SE<3>::jacob_dAB_dA(Dinv, P1inv + P2);
164  const auto J1b = CMatrixDouble12_6(-SE<3>::jacob_dDexpe_de(Dinv));
165 
166  J1 = dLnT_dT.asEigen() * J1a.asEigen() * J1b.asEigen();
167  }
168  if (df_de2)
169  {
170  matrix_TxT& J2 = df_de2.value().get();
171  const auto dAe_de = SE<3>::jacob_dDexpe_de(DinvP1invP2);
172  J2 = dLnT_dT * dAe_de;
173  }
174 }
175 
177  const SE<3>::type& A, const SE<3>::type& B)
178 {
179  using namespace mrpt::math;
180 
181  matrix_MxM J = matrix_MxM::Zero();
182  // J_wrt_A = kron(B,eye(3));
183  const auto B_HM =
184  B.getHomogeneousMatrixVal<CMatrixDouble44>().transpose().eval();
185  for (int c = 0; c < 4; c++)
186  for (int r = 0; r < 4; r++)
187  for (int q = 0; q < 3; q++) J(r * 3 + q, c * 3 + q) = B_HM(r, c);
188 
189  return J;
190 }
191 
193  const SE<3>::type& A, const SE<3>::type& B)
194 {
195  matrix_MxM J = matrix_MxM::Zero();
196  // J_wrt_B = kron(eye(3),A_rot);
197  const auto& AR = A.getRotationMatrix();
198  for (int c = 0; c < 4; c++) J.block<3, 3>(c * 3, c * 3) = AR.asEigen();
199  return J;
200 }
201 
202 // See .h for documentation
203 // ====== SE(2) ===========
205 {
206  SE<2>::type P;
207  P.x(x[0]);
208  P.y(x[1]);
209  P.phi(x[2]);
210  return P;
211 }
212 
214 {
216  x[0] = P.x();
217  x[1] = P.y();
218  x[2] = mrpt::math::wrapToPi(P.phi());
219  return x;
220 }
221 
223 {
224  manifold_vector v;
225  v[0] = pose.x();
226  v[1] = pose.y();
227  v[2] = mrpt::math::wrapToPi(pose.phi());
228  return v;
229 }
230 
232 {
233  return type(v[0], v[1], mrpt::math::wrapToPi(v[2]));
234 }
235 
237  const SE<2>::type& A, const SE<2>::type& B)
238 {
239  const auto bx = B.x(), by = B.y();
240  const auto cphia = A.phi_cos(), sphia = A.phi_sin();
241 
242  matrix_MxM J = matrix_MxM::Identity();
243  J(0, 2) = -bx * sphia - by * cphia;
244  J(1, 2) = +bx * cphia - by * sphia;
245  return J;
246 }
247 
249  const SE<2>::type& A, const SE<2>::type& B)
250 {
251  matrix_MxM J = matrix_MxM::Identity();
252  const auto cphia = A.phi_cos(), sphia = A.phi_sin();
253  J(0, 0) = cphia;
254  J(0, 1) = -sphia;
255  J(1, 0) = sphia;
256  J(1, 1) = cphia;
257  return J;
258 }
259 
261 {
262  const auto c = D.phi_cos(), s = D.phi_sin();
263 
264  // clang-format off
265  return SE<2>::tang2mat_jacob((Eigen::Matrix3d() <<
266  c, -s, 0,
267  s, c, 0,
268  0, 0, 1
269  ).finished());
270  // clang-format on
271 }
272 
274  const CPose2D& Dinv, const CPose2D& P1, const CPose2D& P2,
277 {
278  const CPose2D P1inv = -P1;
279  const CPose2D P1invP2 = P1inv + P2;
280  const CPose2D DinvP1invP2 = Dinv + P1invP2;
281 
282  if (df_de1)
283  {
284  auto& J1 = df_de1.value().get();
285  J1 = jacob_dAB_dA(Dinv, P1invP2).asEigen() * (-jacob_dDexpe_de(Dinv));
286  }
287 
288  if (df_de2)
289  {
290  auto& J2 = df_de2.value().get();
291  J2 = SE<2>::jacob_dDexpe_de(DinvP1invP2);
292  }
293 }
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
mrpt::math::CVectorFixedDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods...
Definition: CPose3D.h:97
std::optional< std::reference_wrapper< T > > optional_ref
Shorter name for std::optional<std::reference_wrapper<T>>
Definition: optional_ref.h:20
void skew_symmetric3_neg(const VECTOR &v, MATRIX &M)
Computes the negative version of a 3x3 skew symmetric matrix from a 3-vector or 3-array: ...
Definition: geometry.h:836
This base provides a set of functions for maths stuff.
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
CMatrixFixed< double, 12, 6 > CMatrixDouble12_6
Definition: CMatrixFixed.h:386
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Traits for SE(n), rigid-body transformations in R^n space.
Definition: SE.h:30
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:39
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:225
Traits for SO(n), rotations in R^n space.
Definition: SO.h:21



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020