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

mp2p_icp
Author(s): Jose-Luis Blanco-Claraco
autogenerated on Tue Jul 2 2024 02:47:26