15 #include <mrpt/core/exceptions.h>
16 #include <mrpt/math/num_jacobian.h>
17 #include <mrpt/poses/CPose3D.h>
18 #include <mrpt/poses/Lie/SE.h>
19 #include <mrpt/random.h>
21 #include <Eigen/Dense>
25 using namespace mrpt::math;
26 using namespace mrpt::poses;
28 auto&
rnd = mrpt::random::getRandomGenerator();
32 return rnd.drawGaussian1D_normalized() * sigma;
36 return rnd.drawGaussian1D_normalized() * sigma;
45 const CPose3D p = CPose3D(
52 mrpt::tfest::TMatchingPair pair;
57 mrpt::math::CMatrixFixed<double, 3, 12> J1;
62 const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);
64 const mrpt::math::CMatrixFixed<double, 3, 6> jacob(J1 * dDexpe_de);
67 CMatrixDouble numJacob;
69 CVectorFixedDouble<6> x_mean;
72 CVectorFixedDouble<6> x_incrs;
74 mrpt::math::estimateJacobian(
78 const CVectorFixedDouble<6>& eps,
const CPose3D& D,
79 CVectorFixedDouble<3>& err)>(
82 const CVectorFixedDouble<6>& eps,
const CPose3D& D,
83 CVectorFixedDouble<3>& err) {
85 const CPose3D incr = Lie::SE<3>::exp(eps);
86 const CPose3D D_expEpsilon = D + incr;
89 x_incrs, p, numJacob);
92 if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
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.");
109 const CPose3D p = CPose3D(
129 mrpt::math::CMatrixFixed<double, 3, 12> J1;
134 const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);
136 const mrpt::math::CMatrixFixed<double, 3, 6> jacob(J1 * dDexpe_de);
139 CMatrixDouble numJacob;
141 CVectorFixedDouble<6> x_mean;
144 CVectorFixedDouble<6> x_incrs;
146 mrpt::math::estimateJacobian(
150 const CVectorFixedDouble<6>& eps,
const CPose3D& D,
151 CVectorFixedDouble<3>& err)>(
154 const CVectorFixedDouble<6>& eps,
const CPose3D& D,
155 CVectorFixedDouble<3>& err) {
157 const CPose3D incr = Lie::SE<3>::exp(eps);
158 const CPose3D D_expEpsilon = D + incr;
161 x_incrs, p, numJacob);
164 if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
166 std::cerr <<
"relativePose: " << p
169 << numJacob.asEigen()
175 << (numJacob - jacob)
178 << J1.asEigen() <<
"\n";
179 THROW_EXCEPTION(
"Jacobian mismatch, see above.");
189 const CPose3D p = CPose3D(
210 mrpt::math::CMatrixFixed<double, 3, 12> J1;
215 const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);
217 const mrpt::math::CMatrixFixed<double, 3, 6> jacob(J1 * dDexpe_de);
220 CMatrixDouble numJacob;
222 CVectorFixedDouble<6> x_mean;
225 CVectorFixedDouble<6> x_incrs;
227 mrpt::math::estimateJacobian(
231 const CVectorFixedDouble<6>& eps,
const CPose3D& D,
232 CVectorFixedDouble<3>& err)>(
235 const CVectorFixedDouble<6>& eps,
const CPose3D& D,
236 CVectorFixedDouble<3>& err) {
238 const CPose3D incr = Lie::SE<3>::exp(eps);
239 const CPose3D D_expEpsilon = D + incr;
242 x_incrs, p, numJacob);
245 if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
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.");
262 const CPose3D p = CPose3D(
286 mrpt::math::CMatrixFixed<double, 4, 12> J1;
291 const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);
293 const mrpt::math::CMatrixFixed<double, 4, 6> jacob(J1 * dDexpe_de);
296 CMatrixDouble numJacob;
298 CVectorFixedDouble<6> x_mean;
301 CVectorFixedDouble<6> x_incrs;
303 mrpt::math::estimateJacobian(
307 const CVectorFixedDouble<6>& eps,
const CPose3D& D,
308 CVectorFixedDouble<4>& err)>(
311 const CVectorFixedDouble<6>& eps,
const CPose3D& D,
312 CVectorFixedDouble<4>& err) {
314 const CPose3D incr = Lie::SE<3>::exp(eps);
315 const CPose3D D_expEpsilon = D + incr;
318 x_incrs, p, numJacob);
321 if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
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.");
338 const CPose3D p = CPose3D(
362 mrpt::math::CMatrixFixed<double, 3, 12> J1;
367 const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);
369 const mrpt::math::CMatrixFixed<double, 3, 6> jacob(J1 * dDexpe_de);
372 CMatrixDouble numJacob;
374 CVectorFixedDouble<6> x_mean;
377 CVectorFixedDouble<6> x_incrs;
379 mrpt::math::estimateJacobian(
383 const CVectorFixedDouble<6>& eps,
const CPose3D& D,
384 CVectorFixedDouble<3>& err)>(
387 const CVectorFixedDouble<6>& eps,
const CPose3D& D,
388 CVectorFixedDouble<3>& err) {
390 const CPose3D incr = Lie::SE<3>::exp(eps);
391 const CPose3D D_expEpsilon = D + incr;
394 x_incrs, p, numJacob);
397 if ((numJacob.asEigen() - jacob.asEigen()).array().abs().maxCoeff() > 1e-5)
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.");
414 const CPose3D p = CPose3D(
437 mrpt::math::CMatrixFixed<double, 4, 12> J1;
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;
446 mrpt::math::CVectorFixedDouble<4> error =
449 std::cout <<
"\nResultado: \n"
450 << error <<
"\nRecta A:\n"
456 const auto dDexpe_de = mrpt::poses::Lie::SE<3>::jacob_dDexpe_de(p);
458 const mrpt::math::CMatrixFixed<double, 4, 6> jacob(J1 * dDexpe_de);
461 CMatrixDouble numJacob;
463 CVectorFixedDouble<6> x_mean;
466 CVectorFixedDouble<6> x_incrs;
468 mrpt::math::estimateJacobian(
472 const CVectorFixedDouble<6>& eps,
const CPose3D& D,
473 CVectorFixedDouble<4>& err)>(
476 const CVectorFixedDouble<6>& eps,
const CPose3D& D,
477 CVectorFixedDouble<4>& err) {
479 const CPose3D incr = Lie::SE<3>::exp(eps);
480 const CPose3D D_expEpsilon = D + incr;
483 x_incrs, p, numJacob);
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";
497 const CPose3D p = CPose3D::Identity();
504 pair.
ln_global.director = mrpt::math::TPoint3D(0, 0, 1).unitarize();
511 mrpt::math::CMatrixFixed<double, 3, 12> J1;
514 ASSERT_NEAR_(err[0], 0.0, 1e-6);
517 int main([[maybe_unused]]
int argc, [[maybe_unused]]
char** argv)
526 for (
int i = 0; i < 1000; i++)
542 catch (std::exception& e)
544 std::cerr << mrpt::exception_to_str(e) <<
"\n";