test-mp2p_error_terms_jacobians.cpp
Go to the documentation of this file.
1 /* -------------------------------------------------------------------------
2  * A Modular Optimization framework for Localization and mApping (MOLA)
3  * Copyright (C) 2018-2024 University of Almeria
4  * See LICENSE for license information.
5  * ------------------------------------------------------------------------- */
6 
14 #include <mp2p_icp/errorTerms.h>
15 #include <mrpt/core/exceptions.h>
16 #include <mrpt/math/num_jacobian.h> // finite difference method
17 #include <mrpt/poses/CPose3D.h>
18 #include <mrpt/poses/Lie/SE.h>
19 #include <mrpt/random.h>
20 
21 #include <Eigen/Dense>
22 #include <iostream> // cerr
23 
24 using namespace mrpt; // for the "_deg" suffix
25 using namespace mrpt::math;
26 using namespace mrpt::poses;
27 
28 auto& rnd = mrpt::random::getRandomGenerator();
29 
30 static double normald(const double sigma) { return rnd.drawGaussian1D_normalized() * sigma; }
31 static float normalf(const float sigma) { return rnd.drawGaussian1D_normalized() * sigma; }
32 
33 // ===========================================================================
34 // Test: error_point2point
35 // ===========================================================================
36 
37 static void test_Jacob_error_point2point()
38 {
39  const CPose3D p = CPose3D(
40  // x y z
41  normald(10), normald(10), normald(10),
42  // Yaw pitch roll
43  rnd.drawUniform(-M_PI, M_PI), rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5),
44  rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5));
45 
46  mrpt::tfest::TMatchingPair pair;
47  pair.global = {normalf(20), normalf(20), normalf(20)};
48  pair.local = {normalf(20), normalf(20), normalf(20)};
49 
50  // Implemented values:
51  mrpt::math::CMatrixFixed<double, 3, 12> J1;
52  // const mrpt::math::CVectorFixed<double, 3> error = // (Ignored here)
53  mp2p_icp::error_point2point(pair, p, J1);
54 
55  // (12x6 Jacobian)
56  const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);
57 
58  const mrpt::math::CMatrixFixed<double, 3, 6> jacob(J1 * dDexpe_de);
59 
60  // Numerical Jacobian:
61  CMatrixDouble numJacob;
62  {
63  CVectorFixedDouble<6> x_mean;
64  x_mean.setZero();
65 
66  CVectorFixedDouble<6> x_incrs;
67  x_incrs.fill(1e-6);
68  mrpt::math::estimateJacobian(
69  x_mean,
70  /* Error function to evaluate */
71  std::function<void(
72  const CVectorFixedDouble<6>& eps, const CPose3D& D, CVectorFixedDouble<3>& err)>(
73  /* Lambda, capturing the pair data */
74  [pair](
75  const CVectorFixedDouble<6>& eps, const CPose3D& D, CVectorFixedDouble<3>& err)
76  {
77  // SE(3) pose increment on the manifold:
78  const CPose3D incr = Lie::SE<3>::exp(eps);
79  const CPose3D D_expEpsilon = D + incr;
80  err = mp2p_icp::error_point2point(pair, D_expEpsilon);
81  }),
82  x_incrs, p, numJacob);
83  }
84 
85  if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
86  {
87  std::cerr << "numJacob:\n"
88  << numJacob.asEigen() << "\njacob:\n"
89  << jacob.asEigen() << "\nDiff:\n"
90  << (numJacob - jacob) << "\nJ1:\n"
91  << J1.asEigen() << "\n";
92  THROW_EXCEPTION("Jacobian mismatch, see above.");
93  }
94 }
95 
96 // ===========================================================================
97 // Test: error_point2line
98 // ===========================================================================
99 
100 static void test_Jacob_error_point2line()
101 {
102  const CPose3D p = CPose3D(
103  // x y z
104  normald(10), normald(10), normald(10),
105  // Yaw pitch roll
106  rnd.drawUniform(-M_PI, M_PI), rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5),
107  rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5));
108 
109  mp2p_icp::point_line_pair_t pair;
110 
111  pair.ln_global.pBase.x = normalf(20);
112  pair.ln_global.pBase.y = normalf(20);
113  pair.ln_global.pBase.z = normalf(20);
114  pair.ln_global.director = mrpt::math::TPoint3D(normald(1), normald(1), normald(1)).unitarize();
115 
116  pair.pt_local.x = normalf(10);
117  pair.pt_local.y = normalf(10);
118  pair.pt_local.z = normalf(10);
119 
120  // Implemented values:
121  mrpt::math::CMatrixFixed<double, 3, 12> J1;
122 
123  mp2p_icp::error_point2line(pair, p, J1);
124 
125  // (12x6 Jacobian)
126  const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);
127 
128  const mrpt::math::CMatrixFixed<double, 3, 6> jacob(J1 * dDexpe_de);
129 
130  // Numerical Jacobian:
131  CMatrixDouble numJacob;
132  {
133  CVectorFixedDouble<6> x_mean;
134  x_mean.setZero();
135 
136  CVectorFixedDouble<6> x_incrs;
137  x_incrs.fill(1e-5);
138  mrpt::math::estimateJacobian(
139  x_mean,
140  /* Error function to evaluate */
141  std::function<void(
142  const CVectorFixedDouble<6>& eps, const CPose3D& D, CVectorFixedDouble<3>& err)>(
143  /* Lambda, capturing the pair data */
144  [pair](
145  const CVectorFixedDouble<6>& eps, const CPose3D& D, CVectorFixedDouble<3>& err)
146  {
147  // SE(3) pose increment on the manifold:
148  const CPose3D incr = Lie::SE<3>::exp(eps);
149  const CPose3D D_expEpsilon = D + incr;
150  err = mp2p_icp::error_point2line(pair, D_expEpsilon);
151  }),
152  x_incrs, p, numJacob);
153  }
154 
155  if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
156  {
157  std::cerr << "relativePose: " << p
158  << "\n"
159  "numJacob:"
160  << numJacob.asEigen()
161  << "\n"
162  "jacob :"
163  << jacob.asEigen()
164  << "\n"
165  "diff :"
166  << (numJacob - jacob)
167  << "\n"
168  "J1 :"
169  << J1.asEigen() << "\n";
170  THROW_EXCEPTION("Jacobian mismatch, see above.");
171  }
172 }
173 
174 // ===========================================================================
175 // Test: error_point2plane
176 // ===========================================================================
177 
178 static void test_Jacob_error_point2plane()
179 {
180  const CPose3D p = CPose3D(
181  // x y z
182  normald(10), normald(10), normald(10),
183  // Yaw pitch roll
184  rnd.drawUniform(-M_PI, M_PI), rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5),
185  rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5));
186 
187  mp2p_icp::point_plane_pair_t pair;
188 
189  pair.pl_global.centroid.x = normalf(20);
190  pair.pl_global.centroid.y = normalf(20);
191  pair.pl_global.centroid.z = normalf(20);
192  pair.pl_global.plane.coefs[0] = normald(20);
193  pair.pl_global.plane.coefs[1] = normald(20);
194  pair.pl_global.plane.coefs[2] = normald(20);
195 
196  pair.pt_local.x = normalf(10);
197  pair.pt_local.y = normalf(10);
198  pair.pt_local.z = normalf(10);
199 
200  // Implemented values:
201  mrpt::math::CMatrixFixed<double, 3, 12> J1;
202 
203  mp2p_icp::error_point2plane(pair, p, J1);
204 
205  // (12x6 Jacobian)
206  const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);
207 
208  const mrpt::math::CMatrixFixed<double, 3, 6> jacob(J1 * dDexpe_de);
209 
210  // Numerical Jacobian:
211  CMatrixDouble numJacob;
212  {
213  CVectorFixedDouble<6> x_mean;
214  x_mean.setZero();
215 
216  CVectorFixedDouble<6> x_incrs;
217  x_incrs.fill(1e-6);
218  mrpt::math::estimateJacobian(
219  x_mean,
220  /* Error function to evaluate */
221  std::function<void(
222  const CVectorFixedDouble<6>& eps, const CPose3D& D, CVectorFixedDouble<3>& err)>(
223  /* Lambda, capturing the pair data */
224  [pair](
225  const CVectorFixedDouble<6>& eps, const CPose3D& D, CVectorFixedDouble<3>& err)
226  {
227  // SE(3) pose increment on the manifold:
228  const CPose3D incr = Lie::SE<3>::exp(eps);
229  const CPose3D D_expEpsilon = D + incr;
230  err = mp2p_icp::error_point2plane(pair, D_expEpsilon);
231  }),
232  x_incrs, p, numJacob);
233  }
234 
235  if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
236  {
237  std::cerr << "numJacob:\n"
238  << numJacob.asEigen() << "\njacob:\n"
239  << jacob.asEigen() << "\nDiff:\n"
240  << (numJacob - jacob) << "\nJ1:\n"
241  << J1.asEigen() << "\n";
242  THROW_EXCEPTION("Jacobian mismatch, see above.");
243  }
244 }
245 
246 // ===========================================================================
247 // Test: error_line2line
248 // ===========================================================================
249 #if 0
250 static void test_Jacob_error_line2line()
251 {
252  const CPose3D p = CPose3D(
253  // x y z
254  normald(10), normald(10), normald(10),
255  // Yaw pitch roll
256  rnd.drawUniform(-M_PI, M_PI), rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5),
257  rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5));
258 
259  mp2p_icp::matched_line_t pair;
260 
261  pair.ln_global.pBase.x = normalf(10);
262  pair.ln_global.pBase.y = normalf(10);
263  pair.ln_global.pBase.z = normalf(10);
264  pair.ln_global.director[0] = normald(10);
265  pair.ln_global.director[1] = normald(10);
266  pair.ln_global.director[2] = normald(10);
267 
268  pair.ln_local.pBase.x = normalf(10);
269  pair.ln_local.pBase.y = normalf(10);
270  pair.ln_local.pBase.z = normalf(10);
271  pair.ln_local.director[0] = normald(10);
272  pair.ln_local.director[1] = normald(10);
273  pair.ln_local.director[2] = normald(10);
274 
275  // Implemented values:
276  mrpt::math::CMatrixFixed<double, 4, 12> J1;
277 
278  mp2p_icp::error_line2line(pair, p, J1);
279 
280  // (12x6 Jacobian)
281  const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);
282 
283  const mrpt::math::CMatrixFixed<double, 4, 6> jacob(J1 * dDexpe_de);
284 
285  // Numerical Jacobian:
286  CMatrixDouble numJacob;
287  {
288  CVectorFixedDouble<6> x_mean;
289  x_mean.setZero();
290 
291  CVectorFixedDouble<6> x_incrs;
292  x_incrs.fill(1e-6);
293  mrpt::math::estimateJacobian(
294  x_mean,
295  /* Error function to evaluate */
296  std::function<void(
297  const CVectorFixedDouble<6>& eps, const CPose3D& D,
298  CVectorFixedDouble<4>& err)>(
299  /* Lambda, capturing the pair data */
300  [pair](
301  const CVectorFixedDouble<6>& eps, const CPose3D& D,
302  CVectorFixedDouble<4>& err) {
303  // SE(3) pose increment on the manifold:
304  const CPose3D incr = Lie::SE<3>::exp(eps);
305  const CPose3D D_expEpsilon = D + incr;
306  err = mp2p_icp::error_line2line(pair, D_expEpsilon);
307  }),
308  x_incrs, p, numJacob);
309  }
310 
311  if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
312  {
313  std::cerr << "numJacob:\n"
314  << numJacob.asEigen() << "\njacob:\n"
315  << jacob.asEigen() << "\nDiff:\n"
316  << (numJacob - jacob) << "\nJ1:\n"
317  << J1.asEigen() << "\n";
318  THROW_EXCEPTION("Jacobian mismatch, see above.");
319  }
320 }
321 #endif
322 
323 // ===========================================================================
324 // Test: error_plane2plane
325 // ===========================================================================
326 
327 static void test_Jacob_error_plane2plane()
328 {
329  const CPose3D p = CPose3D(
330  // x y z
331  normald(10), normald(10), normald(10),
332  // Yaw pitch roll
333  rnd.drawUniform(-M_PI, M_PI), rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5),
334  rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5));
335 
336  mp2p_icp::matched_plane_t pair;
337 
338  pair.p_global.centroid.x = normalf(20);
339  pair.p_global.centroid.y = normalf(20);
340  pair.p_global.centroid.z = normalf(20);
341  pair.p_global.plane.coefs[0] = normald(20);
342  pair.p_global.plane.coefs[1] = normald(20);
343  pair.p_global.plane.coefs[2] = normald(20);
344 
345  pair.p_local.centroid.x = normalf(10);
346  pair.p_local.centroid.y = normalf(10);
347  pair.p_local.centroid.z = normalf(10);
348  pair.p_local.plane.coefs[0] = normald(10);
349  pair.p_local.plane.coefs[1] = normald(10);
350  pair.p_local.plane.coefs[2] = normald(10);
351 
352  // Implemented values:
353  mrpt::math::CMatrixFixed<double, 3, 12> J1;
354 
355  mp2p_icp::error_plane2plane(pair, p, J1);
356 
357  // (12x6 Jacobian)
358  const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);
359 
360  const mrpt::math::CMatrixFixed<double, 3, 6> jacob(J1 * dDexpe_de);
361 
362  // Numerical Jacobian:
363  CMatrixDouble numJacob;
364  {
365  CVectorFixedDouble<6> x_mean;
366  x_mean.setZero();
367 
368  CVectorFixedDouble<6> x_incrs;
369  x_incrs.fill(1e-6);
370  mrpt::math::estimateJacobian(
371  x_mean,
372  /* Error function to evaluate */
373  std::function<void(
374  const CVectorFixedDouble<6>& eps, const CPose3D& D, CVectorFixedDouble<3>& err)>(
375  /* Lambda, capturing the pair data */
376  [pair](
377  const CVectorFixedDouble<6>& eps, const CPose3D& D, CVectorFixedDouble<3>& err)
378  {
379  // SE(3) pose increment on the manifold:
380  const CPose3D incr = Lie::SE<3>::exp(eps);
381  const CPose3D D_expEpsilon = D + incr;
382  err = mp2p_icp::error_plane2plane(pair, D_expEpsilon);
383  }),
384  x_incrs, p, numJacob);
385  }
386 
387  if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
388  {
389  std::cerr << "numJacob:\n"
390  << numJacob.asEigen() << "\njacob:\n"
391  << jacob.asEigen() << "\nDiff:\n"
392  << (numJacob - jacob) << "\nJ1:\n"
393  << J1.asEigen() << "\n";
394  THROW_EXCEPTION("Jacobian mismatch, see above.");
395  }
396 }
397 
398 // ===========================================================================
399 // Test: error_line2line
400 // ===========================================================================
401 
402 static void test_error_line2line()
403 {
404  const CPose3D p = CPose3D(
405  // x y z
406  1, 0.5, 0.1,
407  // Yaw pitch roll
408  0, 0, 0);
409 
410  mp2p_icp::matched_line_t pair;
411 
412  pair.ln_global.pBase.x = 0;
413  pair.ln_global.pBase.y = 1;
414  pair.ln_global.pBase.z = -4;
415  pair.ln_global.director[0] = 0.4364;
416  pair.ln_global.director[1] = 0.8729;
417  pair.ln_global.director[2] = -0.2182;
418 
419  pair.ln_local.pBase.x = 2;
420  pair.ln_local.pBase.y = 1;
421  pair.ln_local.pBase.z = -0.5;
422  pair.ln_local.director[0] = 0.2357;
423  pair.ln_local.director[1] = 0.2357;
424  pair.ln_local.director[2] = 0.9428;
425 
426  // Implemented values:
427  mrpt::math::CMatrixFixed<double, 4, 12> J1;
428 
429  mrpt::math::CVectorFixedDouble<4> ref_error;
430  ref_error[0] = 0.0517;
431  ref_error[1] = 0.2007;
432  ref_error[2] = 0.6372;
433  ref_error[3] = -1.1610;
434 
435 #if 0
436  mrpt::math::CVectorFixedDouble<4> error =
437  mp2p_icp::error_line2line(pair, p, J1);
438 
439  std::cout << "\nResultado: \n"
440  << error << "\nRecta A:\n"
441  << pair.ln_global << "\nRecta B:\n"
442  << pair.ln_local << "\n";
443 #endif
444 
445  // (12x6 Jacobian)
446  const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);
447 
448  const mrpt::math::CMatrixFixed<double, 4, 6> jacob(J1 * dDexpe_de);
449 
450  // Numerical Jacobian:
451  CMatrixDouble numJacob;
452  {
453  CVectorFixedDouble<6> x_mean;
454  x_mean.setZero();
455 
456  CVectorFixedDouble<6> x_incrs;
457  x_incrs.fill(1e-6);
458  mrpt::math::estimateJacobian(
459  x_mean,
460  /* Error function to evaluate */
461  std::function<void(
462  const CVectorFixedDouble<6>& eps, const CPose3D& D, CVectorFixedDouble<4>& err)>(
463  /* Lambda, capturing the pair data */
464  [pair](
465  const CVectorFixedDouble<6>& eps, const CPose3D& D, CVectorFixedDouble<4>& err)
466  {
467  // SE(3) pose increment on the manifold:
468  const CPose3D incr = Lie::SE<3>::exp(eps);
469  const CPose3D D_expEpsilon = D + incr;
470  err = mp2p_icp::error_line2line(pair, D_expEpsilon);
471  }),
472  x_incrs, p, numJacob);
473  }
474 #if 0
475  std::cout << "numJacob:\n"
476  << numJacob.asEigen() << "\njacob:\n"
477  << jacob.asEigen() << "\nDiff:\n"
478  << (numJacob - jacob) << "\nJ1:\n"
479  << J1.asEigen() << "\ndDexp_de:\n"
480  << dDexpe_de.asEigen() << "\n";
481 #endif
482 }
483 
484 static void test_against_ground_truth_error_point2line()
485 {
486  const CPose3D p = CPose3D::Identity();
487 
488  mp2p_icp::point_line_pair_t pair;
489 
490  pair.ln_global.pBase.x = 10;
491  pair.ln_global.pBase.y = 11;
492  pair.ln_global.pBase.z = 12;
493  pair.ln_global.director = mrpt::math::TPoint3D(0, 0, 1).unitarize();
494 
495  pair.pt_local.x = 10;
496  pair.pt_local.y = 11;
497  pair.pt_local.z = -1.0;
498 
499  // Implemented values:
500  mrpt::math::CMatrixFixed<double, 3, 12> J1;
501 
502  CVectorFixedDouble<3> err = mp2p_icp::error_point2line(pair, p, J1);
503  ASSERT_NEAR_(err[0], 0.0, 1e-6);
504 }
505 
506 int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv)
507 {
508  try
509  {
510  // test Jacobians:
511  // ----------------------------------------------
512  rnd.randomize(1234); // for reproducible tests
513 
514  // Run for many different random conditions:
515  for (int i = 0; i < 1000; i++)
516  {
517  test_Jacob_error_point2point();
518  test_Jacob_error_point2line();
519  test_Jacob_error_point2plane();
520  test_Jacob_error_plane2plane();
521  // TODO: Fix this one:
522  // test_Jacob_error_line2line();
523 
524  test_error_line2line();
525  }
526 
527  // Test for known fixed conditions:
528  // ----------------------------------------------
529  test_against_ground_truth_error_point2line();
530  }
531  catch (std::exception& e)
532  {
533  std::cerr << mrpt::exception_to_str(e) << "\n";
534  return 1;
535  }
536 }
test_error_line2line
static void test_error_line2line()
Definition: test-mp2p_error_terms_jacobians.cpp:402
mp2p_icp::plane_patch_t::centroid
mrpt::math::TPoint3D centroid
Definition: plane_patch.h:25
mp2p_icp::matched_plane_t::p_global
plane_patch_t p_global
Definition: Pairings.h:31
test_Jacob_error_plane2plane
static void test_Jacob_error_plane2plane()
Definition: test-mp2p_error_terms_jacobians.cpp:327
mp2p_icp::point_plane_pair_t
Definition: point_plane_pair_t.h:26
mp2p_icp::point_plane_pair_t::pl_global
plane_patch_t pl_global
Definition: point_plane_pair_t.h:28
mp2p_icp::matched_plane_t::p_local
plane_patch_t p_local
Definition: Pairings.h:31
mp2p_icp::point_line_pair_t::ln_global
mrpt::math::TLine3D ln_global
Definition: Pairings.h:55
mrpt
Definition: LogRecord.h:100
test_Jacob_error_point2line
static void test_Jacob_error_point2line()
Definition: test-mp2p_error_terms_jacobians.cpp:100
test_Jacob_error_point2point
static void test_Jacob_error_point2point()
Definition: test-mp2p_error_terms_jacobians.cpp:37
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:320
mp2p_icp::matched_line_t::ln_global
mrpt::math::TLine3D ln_global
Definition: Pairings.h:46
rnd
auto & rnd
Definition: test-mp2p_error_terms_jacobians.cpp:28
mp2p_icp::matched_line_t
Definition: Pairings.h:44
main
int main([[maybe_unused]] int argc, [[maybe_unused]] char **argv)
Definition: test-mp2p_error_terms_jacobians.cpp:506
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:107
test_Jacob_error_point2plane
static void test_Jacob_error_point2plane()
Definition: test-mp2p_error_terms_jacobians.cpp:178
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:53
mp2p_icp::point_line_pair_t::pt_local
mrpt::math::TPoint3D pt_local
Definition: Pairings.h:56
mp2p_icp::point_plane_pair_t::pt_local
mrpt::math::TPoint3Df pt_local
Definition: point_plane_pair_t.h:29
test_against_ground_truth_error_point2line
static void test_against_ground_truth_error_point2line()
Definition: test-mp2p_error_terms_jacobians.cpp:484
mp2p_icp::matched_plane_t
Definition: Pairings.h:29
mp2p_icp::matched_line_t::ln_local
mrpt::math::TLine3D ln_local
Definition: Pairings.h:46
mp2p_icp::plane_patch_t::plane
mrpt::math::TPlane plane
Definition: plane_patch.h:24
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:60
normald
static double normald(const double sigma)
Definition: test-mp2p_error_terms_jacobians.cpp:30
normalf
static float normalf(const float sigma)
Definition: test-mp2p_error_terms_jacobians.cpp:31
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:155

mp2p_icp
Author(s):
autogenerated on Mon May 26 2025 02:45:50