errorTerms.cpp
Go to the documentation of this file.
1 /* -------------------------------------------------------------------------
2  * A repertory of multi primitive-to-primitive (MP2P) ICP algorithms in C++
3  * Copyright (C) 2018-2021 Jose Luis Blanco, University of Almeria
4  * See LICENSE for license information.
5  * ------------------------------------------------------------------------- */
13 #include <mp2p_icp/errorTerms.h>
14 #include <mp2p_icp/optimal_tf_gauss_newton.h>
15 #include <mrpt/math/CVectorFixed.h>
16 #include <mrpt/math/TPoint3D.h>
17 #include <mrpt/math/geometry.h>
18 #include <mrpt/math/ops_containers.h> // dotProduct()
19 #include <mrpt/poses/CPose3D.h>
20 #include <mrpt/poses/Lie/SE.h>
21 
22 #include <Eigen/Dense>
23 #include <iostream>
24 
25 using namespace mp2p_icp;
26 using namespace mrpt::math;
27 
28 mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_point2point(
29  const mrpt::tfest::TMatchingPair& pairing,
30  const mrpt::poses::CPose3D& relativePose,
31  mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian)
32 {
33  MRPT_START
34  mrpt::math::CVectorFixedDouble<3> error;
35  const mrpt::math::TPoint3D& l = pairing.local;
36 
37  const mrpt::math::TPoint3D g = relativePose.composePoint(l);
38 
39  error[0] = g.x - pairing.global.x;
40  error[1] = g.y - pairing.global.y;
41  error[2] = g.z - pairing.global.z;
42 
43  // It's possible change the error to scalar with the function
44  // g.DistanceTo(l) Eval Jacobian:
45  if (jacobian)
46  {
47  mrpt::math::CMatrixFixed<double, 3, 12>& J_aux = jacobian.value().get();
48  // clang-format off
49  J_aux = (Eigen::Matrix<double, 3, 12>() <<
50  l.x, 0, 0, l.y, 0, 0, l.z, 0, 0, 1, 0, 0,
51  0, l.x, 0, 0, l.y, 0, 0, l.z, 0, 0, 1, 0,
52  0, 0, l.x, 0, 0, l.y, 0, 0, l.z, 0, 0, 1
53  ).finished();
54  // clang-format on
55  }
56 
57  return error;
58  MRPT_END
59 }
60 
61 mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_point2line(
62  const mp2p_icp::point_line_pair_t& pairing,
63  const mrpt::poses::CPose3D& relativePose,
64  mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian)
65 {
66  MRPT_START
67  mrpt::math::CVectorFixedDouble<3> error;
68  const auto& lnGlob = pairing.ln_global;
69 
70  // local and global point:
71  const mrpt::math::TPoint3D l = pairing.pt_local;
72  const mrpt::math::TPoint3D g = relativePose.composePoint(l);
73 
74  // Module of vector director of line
75  const auto& u = lnGlob.director;
76  const auto q = (g - lnGlob.pBase);
77 
78  const auto uq = mrpt::math::dotProduct<3, double>(u, q);
79 
80  error[0] = q[0] - u.x * uq;
81  error[1] = q[1] - u.y * uq;
82  error[2] = q[2] - u.z * uq;
83 
84  if (jacobian)
85  {
86  // J1
87  const Eigen::Matrix<double, 3, 3> J1 =
88  (Eigen::Matrix<double, 3, 3>() << 1 - mrpt::square(u.x), -u.x * u.y,
89  -u.x * u.z, -u.y * u.x, 1 - mrpt::square(u.y), -u.y * u.z,
90  -u.z * u.x, -u.z * u.y, 1 - mrpt::square(u.z))
91  .finished();
92  // J2
93  // clang-format off
94  Eigen::Matrix<double, 3, 12> J2 =
95  (Eigen::Matrix<double, 3, 12>() <<
96  l.x, 0, 0, l.y, 0, 0, l.z, 0, 0, 1, 0, 0,
97  0, l.x, 0, 0, l.y, 0, 0, l.z, 0, 0, 1, 0,
98  0, 0, l.x, 0, 0, l.y, 0, 0, l.z, 0, 0, 1
99  ).finished();
100  // clang-format on
101  mrpt::math::CMatrixFixed<double, 3, 12>& J_aux = jacobian.value().get();
102 
103  J_aux = J1 * J2;
104  }
105  return error;
106  MRPT_END
107 }
108 
109 mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_point2plane(
110  const mp2p_icp::point_plane_pair_t& pairing,
111  const mrpt::poses::CPose3D& relativePose,
112  mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian)
113 {
114  MRPT_START
115  mrpt::math::CVectorFixedDouble<3> error;
116  const auto& p = pairing.pt_local;
117  const auto& pl_aux = pairing.pl_global.plane;
118  const mrpt::math::TPoint3D l = TPoint3D(p.x, p.y, p.z);
119  mrpt::math::TPoint3D g;
120  relativePose.composePoint(l, g);
121  double mod_n = mrpt::square(pl_aux.coefs[0]) +
122  mrpt::square(pl_aux.coefs[1]) +
123  mrpt::square(pl_aux.coefs[2]);
124 
125  error[0] = -(pl_aux.coefs[0] / mod_n) *
126  (pl_aux.coefs[0] * g.x + pl_aux.coefs[1] * g.y +
127  pl_aux.coefs[2] * g.z + pl_aux.coefs[3]);
128  error[1] = -(pl_aux.coefs[1] / mod_n) *
129  (pl_aux.coefs[0] * g.x + pl_aux.coefs[1] * g.y +
130  pl_aux.coefs[2] * g.z + pl_aux.coefs[3]);
131  error[2] = -(pl_aux.coefs[2] / mod_n) *
132  (pl_aux.coefs[0] * g.x + pl_aux.coefs[1] * g.y +
133  pl_aux.coefs[2] * g.z + pl_aux.coefs[3]);
134  if (jacobian)
135  {
136  // Eval Jacobian:
137  // J1
138  const Eigen::Matrix<double, 3, 3> J1 =
139  (Eigen::Matrix<double, 3, 3>()
140  << -mrpt::square(pl_aux.coefs[0]) / mod_n,
141  -pl_aux.coefs[0] * pl_aux.coefs[1] / mod_n,
142  -pl_aux.coefs[0] * pl_aux.coefs[2] / mod_n,
143  -pl_aux.coefs[1] * pl_aux.coefs[0] / mod_n,
144  -mrpt::square(pl_aux.coefs[1]) / mod_n,
145  -pl_aux.coefs[1] * pl_aux.coefs[2] / mod_n,
146  -pl_aux.coefs[2] * pl_aux.coefs[0] / mod_n,
147  -pl_aux.coefs[2] * pl_aux.coefs[1] / mod_n,
148  -mrpt::square(pl_aux.coefs[2]) / mod_n)
149  .finished();
150  // J2
151  // clang-format off
152  const Eigen::Matrix<double, 3, 12> J2 =
153  (Eigen::Matrix<double, 3, 12>() <<
154  l.x, 0, 0, l.y, 0, 0, l.z, 0, 0, 1, 0, 0,
155  0, l.x, 0, 0, l.y, 0, 0, l.z, 0, 0, 1, 0,
156  0, 0, l.x, 0, 0, l.y, 0, 0, l.z, 0, 0, 1
157  ).finished();
158  // clang-format on
159 
160  mrpt::math::CMatrixFixed<double, 3, 12>& J_aux = jacobian.value().get();
161  J_aux = J1 * J2;
162  }
163  return error;
164  MRPT_END
165 }
166 
167 mrpt::math::CVectorFixedDouble<4> mp2p_icp::error_line2line(
168  const mp2p_icp::matched_line_t& pairing,
169  const mrpt::poses::CPose3D& relativePose,
170  mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 4, 12>> jacobian)
171 {
172  MRPT_START
173  mrpt::math::CVectorFixedDouble<4> error;
174  mrpt::math::TLine3D ln_aux;
175  mrpt::math::TPoint3D g;
176 
177  const auto& p0 = pairing.ln_local.pBase;
178  const auto& u0 = pairing.ln_local.director;
179  const auto& p1 = pairing.ln_global.pBase;
180  const auto& u1 = pairing.ln_global.director;
181 
182  relativePose.composePoint(p0, g);
183  ln_aux.pBase = mrpt::math::TPoint3D(g);
184 
185  // Homogeneous matrix calculation
186  mrpt::math::CMatrixDouble44 aux;
187  relativePose.getHomogeneousMatrix(aux);
188  const Eigen::Matrix<double, 4, 4> T = aux.asEigen();
189 
190  // Projection of the director vector for the new pose
191  const Eigen::Matrix<double, 1, 4> U =
192  (Eigen::Matrix<double, 1, 4>() << u0[0], u0[1], u0[2], 1).finished();
193  const Eigen::Matrix<double, 1, 4> U_T = U * T;
194  ln_aux.director = {U_T[0], U_T[1], U_T[2]};
195 
196  // Angle formed between the lines
197  double alfa = getAngle(pairing.ln_global, ln_aux) * 180 / (2 * 3.14159265);
198  /*
199  std::cout << "\nLine 1:\n"
200  << pairing.ln_this << "\nLine 2:\n"
201  << pairing.ln_other << "\nLine 2':\n"
202  << ln_aux << "\nAngle:\n"
203  << alfa << "\nT:\n"
204  << T << "\n";
205  */
206 
207  // p_r0 = (p-r_{0,r}). Ec.20
208  const Eigen::Matrix<double, 1, 3> p_r2 =
209  (Eigen::Matrix<double, 1, 3>() << ln_aux.pBase.x - p1.x,
210  ln_aux.pBase.y - p1.y, ln_aux.pBase.z - p1.z)
211  .finished();
212 
213  const Eigen::Matrix<double, 1, 3> rv =
214  (Eigen::Matrix<double, 1, 3>() << u1[0], u1[1], u1[2]).finished();
215 
216  // Relationship between lines
217  const double tolerance = 0.01;
218  if (abs(alfa) < tolerance)
219  { // Parallel
220  // Error: Ec.20
221  error[0] = mrpt::square(pairing.ln_global.distance(ln_aux.pBase));
222  if (jacobian)
223  {
224  // Module of vector director of line
225  double mod_rv = rv * rv.transpose();
226 
227  // J1: Ec.22
228  Eigen::Matrix<double, 1, 3> J1 =
229  2 * p_r2 - (2 / mod_rv) * (p_r2 * rv.transpose()) * rv;
230  // J2: Ec.23
231  // clang-format off
232  const Eigen::Matrix<double, 3, 12> J2 =
233  (Eigen::Matrix<double, 3, 12>() <<
234  p0.x, 0, 0, p0.y, 0, 0, p0.z, 0, 0, 1, 0, 0,
235  0, p0.x, 0, 0, p0.y, 0, 0, p0.z, 0, 0, 1, 0,
236  0, 0, p0.x, 0, 0, p0.y, 0, 0, p0.z, 0, 0, 1
237  ).finished();
238  // clang-format on
239  // Build Jacobian
240  mrpt::math::CMatrixFixed<double, 4, 12>& J_auxp =
241  jacobian.value().get();
242  J_auxp.block<1, 12>(0, 0) = J1 * J2;
243  }
244  }
245  else
246  { // Rest
247  // Error:
248  // Cross product (r_u x r_2,v)
249  const double rw_x = U_T[1] * u1[2] - U_T[2] * u1[1];
250  const double rw_y = -(U_T[0] * u1[2] - U_T[2] * u1[0]);
251  const double rw_z = U_T[0] * u1[1] - U_T[1] * u1[0];
252 
253  const Eigen::Matrix<double, 1, 3> r_w =
254  (Eigen::Matrix<double, 1, 3>() << rw_x, rw_y, rw_z).finished();
255  double aux_rw = r_w * r_w.transpose();
256  // Error 1. Ec.26
257  error[0] = p_r2.dot(r_w) / sqrt(aux_rw);
258  // Error 2. Ec.27
259  error[1] = U_T[0] - u1[0];
260  error[2] = U_T[1] - u1[1];
261  error[3] = U_T[2] - u1[2];
262  if (jacobian)
263  {
264  // J1.1: Ec.32
265  Eigen::Matrix<double, 1, 3> J1_1 = r_w / sqrt(aux_rw);
266 
267  // J1.2:
268  // A
269  const double A =
270  p_r2[0] * r_w[0] + p_r2[1] * r_w[1] + p_r2[2] * r_w[2];
271  const double Ax = -u1[2] * p_r2[1] + u1[1] * p_r2[2];
272  const double Ay = u1[2] * p_r2[0] - u1[0] * p_r2[2];
273  const double Az = -u1[1] * p_r2[0] + u1[0] * p_r2[1];
274  // B
275  const double B = sqrt(aux_rw);
276  const double Bx = (-u1[2] * r_w[1] + u1[1] * r_w[2]) / B;
277  const double By = (u1[2] * r_w[0] + u1[0] * r_w[2]) / B;
278  const double Bz = (-u1[1] * r_w[0] + u1[0] * r_w[1]) / B;
279 
280 #if 0
281  std::cout << "\nA: " << A << "\nAx: " << Ax << "\nAy: " << Ay
282  << "\nAz: " << Az << "\nB: " << B << "\nBx: " << Bx
283  << "\nBy: " << By << "\nBz: " << Bz << "\n";
284 #endif
285 
286  // Ec.36
287  // clang-format off
288  Eigen::Matrix<double, 1, 3> J1_2 =
289  (Eigen::Matrix<double, 1, 3>() <<
290  (Ax * B - A * Bx) / mrpt::square(B),
291  (Ay * B - A * By) / mrpt::square(B),
292  (Az * B - A * Bz) / mrpt::square(B)
293  ).finished();
294  // clang-format on
295 
296  // J1.3: Ec.37-38
297  // clang-format off
298  Eigen::Matrix<double, 3, 6> J1_3 =
299  (Eigen::Matrix<double, 3, 6>() <<
300  0, 0, 0, 1, 0, 0,
301  0, 0, 0, 0, 1, 0,
302  0, 0, 0, 0, 0, 1
303  ).finished();
304  // clang-format on
305 
306  // J1: Ec.29
307  Eigen::Matrix<double, 4, 6> J1;
308  J1.block<1, 3>(0, 0) = J1_1;
309  J1.block<1, 3>(0, 3) = J1_2;
310  J1.block<3, 6>(1, 0) = J1_3;
311 
312  // J2: Ec.39-41
313  // clang-format off
314  const Eigen::Matrix<double, 6, 12> J2 =
315  (Eigen::Matrix<double, 6, 12>() <<
316  p0.x, 0, 0, p0.y, 0, 0, p0.z, 0, 0, 1, 0, 0,
317  0, p0.x, 0, 0, p0.y, 0, 0, p0.z, 0, 0, 1, 0,
318  0, 0, p0.x, 0, 0, p0.y, 0, 0, p0.z, 0, 0, 1,
319  -u0[0], 0, 0, -u0[1], 0, 0, -u0[2], 0, 0, 0, 0, 0,
320  0, -u0[0], 0, 0, -u0[1], 0, 0, -u0[2], 0, 0, 0, 0,
321  0, 0, -u0[0], 0, 0, -u0[1], 0, 0, -u0[2], 0, 0, 0
322  ).finished();
323  // clang-format on
324  // Build Jacobian
325  mrpt::math::CMatrixFixed<double, 4, 12>& J_aux =
326  jacobian.value().get();
327  J_aux.block<4, 12>(0, 0) = J1 * J2;
328 
329  std::cout << "\nJ1:\n" << J1 << "\nJ2:\n" << J2 << "\n";
330  }
331  }
332  // std::cout<<"\nError:\n"<<error;
333  return error;
334  MRPT_END
335 }
336 
337 mrpt::math::CVectorFixedDouble<3> mp2p_icp::error_plane2plane(
338  const mp2p_icp::matched_plane_t& pairing,
339  const mrpt::poses::CPose3D& relativePose,
340  mrpt::optional_ref<mrpt::math::CMatrixFixed<double, 3, 12>> jacobian)
341 {
342  MRPT_START
343  mrpt::math::CVectorFixedDouble<3> error;
344 
345  const auto nl = pairing.p_local.plane.getNormalVector();
346  const auto ng = pairing.p_global.plane.getNormalVector();
347 
348  const auto p_oplus_nl = relativePose.rotateVector(nl);
349 
350  for (int i = 0; i < 3; i++) error[i] = p_oplus_nl[i] - ng[i];
351 
352  if (jacobian)
353  {
354  // Eval Jacobian:
355 
356  // df_oplus(A,p)/d_A. Section 7.3.2 tech. report:
357  // "A tutorial on SE(3) transformation parameterizations and
358  // on-manifold optimization"
359  // Modified, to discard the last I_3 block, since this particular
360  // cost function is insensible to translations.
361 
362  // clang-format off
363  mrpt::math::CMatrixFixed<double, 3, 12>& J_aux = jacobian.value().get();
364  J_aux = (Eigen::Matrix<double, 3, 12>() <<
365  nl.x, 0, 0, nl.y, 0, 0, nl.z, 0, 0, 0, 0, 0,
366  0, nl.x, 0, 0, nl.y, 0, 0, nl.z, 0, 0, 0, 0,
367  0, 0, nl.x, 0, 0, nl.y, 0, 0, nl.z, 0, 0, 0
368  ).finished();
369  // clang-format on
370  }
371  return error;
372  MRPT_END
373 }
mp2p_icp
Definition: covariance.h:17
mp2p_icp::matched_plane_t::p_global
plane_patch_t p_global
Definition: Pairings.h:30
mp2p_icp::point_plane_pair_t
Definition: Pairings.h:52
mp2p_icp::point_plane_pair_t::pl_global
plane_patch_t pl_global
Definition: Pairings.h:54
mp2p_icp::matched_plane_t::p_local
plane_patch_t p_local
Definition: Pairings.h:30
mp2p_icp::point_line_pair_t::ln_global
mrpt::math::TLine3D ln_global
Definition: Pairings.h:71
optimal_tf_gauss_newton.h
Simple non-linear optimizer to find the SE(3) optimal transformation.
errorTerms.h
mp2p_icp::error_plane2plane
mrpt::math::CVectorFixedDouble< 3 > error_plane2plane(const mp2p_icp::matched_plane_t &pairing, const mrpt::poses::CPose3D &relativePose, mrpt::optional_ref< mrpt::math::CMatrixFixed< double, 3, 12 >> jacobian=std::nullopt)
Definition: errorTerms.cpp:337
mp2p_icp::matched_line_t::ln_global
mrpt::math::TLine3D ln_global
Definition: Pairings.h:45
mp2p_icp::matched_line_t
Definition: Pairings.h:43
mp2p_icp::error_point2plane
mrpt::math::CVectorFixedDouble< 3 > error_point2plane(const mp2p_icp::point_plane_pair_t &pairing, const mrpt::poses::CPose3D &relativePose, mrpt::optional_ref< mrpt::math::CMatrixFixed< double, 3, 12 >> jacobian=std::nullopt)
Definition: errorTerms.cpp:109
mp2p_icp::error_point2point
mrpt::math::CVectorFixedDouble< 3 > error_point2point(const mrpt::tfest::TMatchingPair &pairing, const mrpt::poses::CPose3D &relativePose, mrpt::optional_ref< mrpt::math::CMatrixFixed< double, 3, 12 >> jacobian=std::nullopt)
Definition: errorTerms.cpp:28
mp2p_icp::point_line_pair_t
Definition: Pairings.h:69
mp2p_icp::point_line_pair_t::pt_local
mrpt::math::TPoint3D pt_local
Definition: Pairings.h:72
mp2p_icp::point_plane_pair_t::pt_local
mrpt::math::TPoint3Df pt_local
Definition: Pairings.h:55
mp2p_icp::matched_plane_t
Definition: Pairings.h:28
mp2p_icp::matched_line_t::ln_local
mrpt::math::TLine3D ln_local
Definition: Pairings.h:45
mp2p_icp::plane_patch_t::plane
mrpt::math::TPlane plane
Definition: plane_patch.h:24
test.q
q
Definition: test.py:8
mp2p_icp::error_point2line
mrpt::math::CVectorFixedDouble< 3 > error_point2line(const mp2p_icp::point_line_pair_t &pairing, const mrpt::poses::CPose3D &relativePose, mrpt::optional_ref< mrpt::math::CMatrixFixed< double, 3, 12 >> jacobian=std::nullopt)
Definition: errorTerms.cpp:61
mp2p_icp::error_line2line
mrpt::math::CVectorFixedDouble< 4 > error_line2line(const mp2p_icp::matched_line_t &pairing, const mrpt::poses::CPose3D &relativePose, mrpt::optional_ref< mrpt::math::CMatrixFixed< double, 4, 12 >> jacobian=std::nullopt)
Definition: errorTerms.cpp:167

mrpt_local_obstacles
Author(s): Jose-Luis Blanco-Claraco
autogenerated on Mon Aug 14 2023 02:09:02