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-2021 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 
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 
332 // ===========================================================================
333 // Test: error_plane2plane
334 // ===========================================================================
335 
336 static void test_Jacob_error_plane2plane()
337 {
338  const CPose3D p = CPose3D(
339  // x y z
340  normald(10), normald(10), normald(10),
341  // Yaw pitch roll
342  rnd.drawUniform(-M_PI, M_PI), rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5),
343  rnd.drawUniform(-M_PI * 0.5, M_PI * 0.5));
344 
345  mp2p_icp::matched_plane_t pair;
346 
347  pair.p_global.centroid.x = normalf(20);
348  pair.p_global.centroid.y = normalf(20);
349  pair.p_global.centroid.z = normalf(20);
350  pair.p_global.plane.coefs[0] = normald(20);
351  pair.p_global.plane.coefs[1] = normald(20);
352  pair.p_global.plane.coefs[2] = normald(20);
353 
354  pair.p_local.centroid.x = normalf(10);
355  pair.p_local.centroid.y = normalf(10);
356  pair.p_local.centroid.z = normalf(10);
357  pair.p_local.plane.coefs[0] = normald(10);
358  pair.p_local.plane.coefs[1] = normald(10);
359  pair.p_local.plane.coefs[2] = normald(10);
360 
361  // Implemented values:
362  mrpt::math::CMatrixFixed<double, 3, 12> J1;
363 
364  mp2p_icp::error_plane2plane(pair, p, J1);
365 
366  // (12x6 Jacobian)
367  const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);
368 
369  const mrpt::math::CMatrixFixed<double, 3, 6> jacob(J1 * dDexpe_de);
370 
371  // Numerical Jacobian:
372  CMatrixDouble numJacob;
373  {
374  CVectorFixedDouble<6> x_mean;
375  x_mean.setZero();
376 
377  CVectorFixedDouble<6> x_incrs;
378  x_incrs.fill(1e-6);
379  mrpt::math::estimateJacobian(
380  x_mean,
381  /* Error function to evaluate */
382  std::function<void(
383  const CVectorFixedDouble<6>& eps, const CPose3D& D,
384  CVectorFixedDouble<3>& err)>(
385  /* Lambda, capturing the pair data */
386  [pair](
387  const CVectorFixedDouble<6>& eps, const CPose3D& D,
388  CVectorFixedDouble<3>& err) {
389  // SE(3) pose increment on the manifold:
390  const CPose3D incr = Lie::SE<3>::exp(eps);
391  const CPose3D D_expEpsilon = D + incr;
392  err = mp2p_icp::error_plane2plane(pair, D_expEpsilon);
393  }),
394  x_incrs, p, numJacob);
395  }
396 
397  if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
398  {
399  std::cerr << "numJacob:\n"
400  << numJacob.asEigen() << "\njacob:\n"
401  << jacob.asEigen() << "\nDiff:\n"
402  << (numJacob - jacob) << "\nJ1:\n"
403  << J1.asEigen() << "\n";
404  THROW_EXCEPTION("Jacobian mismatch, see above.");
405  }
406 }
407 
408 // ===========================================================================
409 // Test: error_line2line
410 // ===========================================================================
411 
412 static void test_error_line2line()
413 {
414  const CPose3D p = CPose3D(
415  // x y z
416  1, 0.5, 0.1,
417  // Yaw pitch roll
418  0, 0, 0);
419 
420  mp2p_icp::matched_line_t pair;
421 
422  pair.ln_global.pBase.x = 0;
423  pair.ln_global.pBase.y = 1;
424  pair.ln_global.pBase.z = -4;
425  pair.ln_global.director[0] = 0.4364;
426  pair.ln_global.director[1] = 0.8729;
427  pair.ln_global.director[2] = -0.2182;
428 
429  pair.ln_local.pBase.x = 2;
430  pair.ln_local.pBase.y = 1;
431  pair.ln_local.pBase.z = -0.5;
432  pair.ln_local.director[0] = 0.2357;
433  pair.ln_local.director[1] = 0.2357;
434  pair.ln_local.director[2] = 0.9428;
435 
436  // Implemented values:
437  mrpt::math::CMatrixFixed<double, 4, 12> J1;
438 
439  mrpt::math::CVectorFixedDouble<4> ref_error;
440  ref_error[0] = 0.0517;
441  ref_error[1] = 0.2007;
442  ref_error[2] = 0.6372;
443  ref_error[3] = -1.1610;
444 
445 #if 0
446  mrpt::math::CVectorFixedDouble<4> error =
447  mp2p_icp::error_line2line(pair, p, J1);
448 
449  std::cout << "\nResultado: \n"
450  << error << "\nRecta A:\n"
451  << pair.ln_global << "\nRecta B:\n"
452  << pair.ln_local << "\n";
453 #endif
454 
455  // (12x6 Jacobian)
456  const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);
457 
458  const mrpt::math::CMatrixFixed<double, 4, 6> jacob(J1 * dDexpe_de);
459 
460  // Numerical Jacobian:
461  CMatrixDouble numJacob;
462  {
463  CVectorFixedDouble<6> x_mean;
464  x_mean.setZero();
465 
466  CVectorFixedDouble<6> x_incrs;
467  x_incrs.fill(1e-6);
468  mrpt::math::estimateJacobian(
469  x_mean,
470  /* Error function to evaluate */
471  std::function<void(
472  const CVectorFixedDouble<6>& eps, const CPose3D& D,
473  CVectorFixedDouble<4>& err)>(
474  /* Lambda, capturing the pair data */
475  [pair](
476  const CVectorFixedDouble<6>& eps, const CPose3D& D,
477  CVectorFixedDouble<4>& err) {
478  // SE(3) pose increment on the manifold:
479  const CPose3D incr = Lie::SE<3>::exp(eps);
480  const CPose3D D_expEpsilon = D + incr;
481  err = mp2p_icp::error_line2line(pair, D_expEpsilon);
482  }),
483  x_incrs, p, numJacob);
484  }
485 #if 0
486  std::cout << "numJacob:\n"
487  << numJacob.asEigen() << "\njacob:\n"
488  << jacob.asEigen() << "\nDiff:\n"
489  << (numJacob - jacob) << "\nJ1:\n"
490  << J1.asEigen() << "\ndDexp_de:\n"
491  << dDexpe_de.asEigen() << "\n";
492 #endif
493 }
494 
495 static void test_against_ground_truth_error_point2line()
496 {
497  const CPose3D p = CPose3D::Identity();
498 
499  mp2p_icp::point_line_pair_t pair;
500 
501  pair.ln_global.pBase.x = 10;
502  pair.ln_global.pBase.y = 11;
503  pair.ln_global.pBase.z = 12;
504  pair.ln_global.director = mrpt::math::TPoint3D(0, 0, 1).unitarize();
505 
506  pair.pt_local.x = 10;
507  pair.pt_local.y = 11;
508  pair.pt_local.z = -1.0;
509 
510  // Implemented values:
511  mrpt::math::CMatrixFixed<double, 3, 12> J1;
512 
513  CVectorFixedDouble<3> err = mp2p_icp::error_point2line(pair, p, J1);
514  ASSERT_NEAR_(err[0], 0.0, 1e-6);
515 }
516 
517 int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv)
518 {
519  try
520  {
521  // test Jacobians:
522  // ----------------------------------------------
523  rnd.randomize(1234); // for reproducible tests
524 
525  // Run for many different random conditions:
526  for (int i = 0; i < 1000; i++)
527  {
528  test_Jacob_error_point2point();
529  test_Jacob_error_point2line();
530  test_Jacob_error_point2plane();
531  test_Jacob_error_plane2plane();
532  // TODO: Fix this one:
533  // test_Jacob_error_line2line();
534 
535  test_error_line2line();
536  }
537 
538  // Test for known fixed conditions:
539  // ----------------------------------------------
540  test_against_ground_truth_error_point2line();
541  }
542  catch (std::exception& e)
543  {
544  std::cerr << mrpt::exception_to_str(e) << "\n";
545  return 1;
546  }
547 }
test_Jacob_error_point2line
static void test_Jacob_error_point2line()
Definition: test-mp2p_error_terms_jacobians.cpp:107
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::point_line_pair_t::pt_local
mrpt::math::TPoint3D pt_local
Definition: Pairings.h:72
test_error_line2line
static void test_error_line2line()
Definition: test-mp2p_error_terms_jacobians.cpp:412
test_against_ground_truth_error_point2line
static void test_against_ground_truth_error_point2line()
Definition: test-mp2p_error_terms_jacobians.cpp:495
mp2p_icp::matched_plane_t::p_global
plane_patch_t p_global
Definition: Pairings.h:30
mp2p_icp::matched_plane_t
Definition: Pairings.h:28
M_PI
#define M_PI
test_Jacob_error_point2point
static void test_Jacob_error_point2point()
Definition: test-mp2p_error_terms_jacobians.cpp:43
rnd
auto & rnd
Definition: test-mp2p_error_terms_jacobians.cpp:28
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
main
int main([[maybe_unused]] int argc, [[maybe_unused]] char **argv)
Definition: test-mp2p_error_terms_jacobians.cpp:517
mp2p_icp::matched_line_t::ln_local
mrpt::math::TLine3D ln_local
Definition: Pairings.h:45
mp2p_icp::matched_line_t::ln_global
mrpt::math::TLine3D ln_global
Definition: Pairings.h:45
mp2p_icp::point_plane_pair_t::pl_global
plane_patch_t pl_global
Definition: Pairings.h:54
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
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
test_Jacob_error_point2plane
static void test_Jacob_error_point2plane()
Definition: test-mp2p_error_terms_jacobians.cpp:187
normald
static double normald(const double sigma)
Definition: test-mp2p_error_terms_jacobians.cpp:30
mp2p_icp::point_line_pair_t
Definition: Pairings.h:69
test_Jacob_error_line2line
static void test_Jacob_error_line2line()
Definition: test-mp2p_error_terms_jacobians.cpp:260
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::point_line_pair_t::ln_global
mrpt::math::TLine3D ln_global
Definition: Pairings.h:71
test_Jacob_error_plane2plane
static void test_Jacob_error_plane2plane()
Definition: test-mp2p_error_terms_jacobians.cpp:336
mp2p_icp::point_plane_pair_t
Definition: Pairings.h:52
mp2p_icp::plane_patch_t::centroid
mrpt::math::TPoint3D centroid
Definition: plane_patch.h:25
normalf
static float normalf(const float sigma)
Definition: test-mp2p_error_terms_jacobians.cpp:34

mrpt_local_obstacles
Author(s): Jose-Luis Blanco-Claraco
autogenerated on Thu Jun 1 2023 03:06:43