Fix 4DoF PointToPlane error minimizer crash

[kaatrasa]

I encountered a bug where PointToPlane error minimizer crashes when using force4DOF is enabled and the point clouds have very little overlap. Using Debug version of libpointmatcher the crash happens with all point clouds.

I traced the issue down to this matrix multiplication matrixGamma*mPts.reading.features, where matrixGamma and mPts.reading.features are 3x3 and 4xN matrices, respectively. Using inhomogeneous coordinates for the points fixed the issue.

stacktrace:

main: /home/kaatr/dev/vio/target/3rdparty/host/include/eigen3/Eigen/src/Core/CwiseBinaryOp.h:110: Eigen::CwiseBinaryOp<BinaryOp, Lhs, Rhs>::CwiseBinaryOp(const Lhs&, const Rhs&, const BinaryOp&) [with BinaryOp = Eigen::internal::scalar_product_op<double, double>; LhsType = const Eigen::Transpose<const Eigen::Block<const Eigen::Matrix<double, -1, -1>, 1, -1, false> >; RhsType = const Eigen::Block<const Eigen::Matrix<double, -1, -1>, -1, 1, true>; Eigen::CwiseBinaryOp<BinaryOp, Lhs, Rhs>::Lhs = Eigen::Transpose<const Eigen::Block<const Eigen::Matrix<double, -1, -1>, 1, -1, false> >; Eigen::CwiseBinaryOp<BinaryOp, Lhs, Rhs>::Rhs = Eigen::Block<const Eigen::Matrix<double, -1, -1>, -1, 1, true>]: Assertion `aLhs.rows() == aRhs.rows() && aLhs.cols() == aRhs.cols()' failed.
Process 625401 stopped
* thread #25, name = 'main', stop reason = signal SIGABRT
    frame #0: 0x00007ffff757c03b libc.so.6`raise + 203
libc.so.6`raise:
->  0x7ffff757c03b <+203>: movq   0x108(%rsp), %rax
    0x7ffff757c043 <+211>: xorq   %fs:0x28, %rax
    0x7ffff757c04c <+220>: jne    0x7ffff757c074            ; <+260>
    0x7ffff757c04e <+222>: movl   %r8d, %eax
(lldb) bt
* thread #25, name = 'main', stop reason = signal SIGABRT
  * frame #0: 0x00007ffff757c03b libc.so.6`raise + 203
    frame #1: 0x00007ffff755b859 libc.so.6`abort + 299
    frame #2: 0x00007ffff755b729 libc.so.6`___lldb_unnamed_symbol8$$libc.so.6 + 15
    frame #3: 0x00007ffff756d006 libc.so.6`__assert_fail + 70
    frame #4: 0x00005555558d0601 main`Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, Eigen::Transpose<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false> const> const, Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, -1, 1, true> const>::CwiseBinaryOp(this=0x00007fffb0fee520, aLhs=0x00007fffb0fee4b0, aRhs=0x00007fffb0fee4e8, func=<unavailable>) at CwiseBinaryOp.h:110:7
    frame #5: 0x00005555558da64b main`Eigen::internal::product_evaluator<Eigen::Product<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, 1>, 8, Eigen::DenseShape, Eigen::DenseShape, double, double>::coeff(long, long) const [inlined] Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, Eigen::internal::traits<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, -1, 1, true> >::Scalar>, Eigen::Transpose<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false> const> const, Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, -1, 1, true> const> const Eigen::MatrixBase<Eigen::Transpose<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, 1, -1, false> const> >::cwiseProduct<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1> const, -1, 1, true> >(other=<unavailable>, this=<unavailable>) const at MatrixCwiseBinaryOps.h:25:97
    frame #6: 0x00005555558da63a main`Eigen::internal::product_evaluator<Eigen::Product<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, 1>, 8, Eigen::DenseShape, Eigen::DenseShape, double, double>::coeff(this=0x00007fffb0fee660, row=<unavailable>, col=<unavailable>) const at ProductEvaluators.h:578
    frame #7: 0x00005555558da73c main`Eigen::internal::generic_dense_assignment_kernel<Eigen::internal::evaluator<Eigen::Matrix<double, -1, -1, 0, -1, -1> >, Eigen::internal::evaluator<Eigen::Product<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, 1> >, Eigen::internal::assign_op<double, double>, 0>::assignCoeff(this=0x00007fffb0fee698, row=2, col=0) at AssignEvaluator.h:631:5
    frame #8: 0x00005555558da80f main`Eigen::internal::dense_assignment_loop<Eigen::internal::generic_dense_assignment_kernel<Eigen::internal::evaluator<Eigen::Matrix<double, -1, -1, 0, -1, -1> >, Eigen::internal::evaluator<Eigen::Product<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, 1> >, Eigen::internal::assign_op<double, double>, 0>, 4, 0>::run(Eigen::internal::generic_dense_assignment_kernel<Eigen::internal::evaluator<Eigen::Matrix<double, -1, -1, 0, -1, -1> >, Eigen::internal::evaluator<Eigen::Product<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, 1> >, Eigen::internal::assign_op<double, double>, 0>&) at AssignEvaluator.h:645:5
    frame #9: 0x00005555558da80a main`Eigen::internal::dense_assignment_loop<Eigen::internal::generic_dense_assignment_kernel<Eigen::internal::evaluator<Eigen::Matrix<double, -1, -1, 0, -1, -1> >, Eigen::internal::evaluator<Eigen::Product<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, 1> >, Eigen::internal::assign_op<double, double>, 0>, 4, 0>::run(kernel=0x00007fffb0fee698) at AssignEvaluator.h:555
    frame #10: 0x00005555559fa2b6 main`void Eigen::internal::generic_product_impl<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::DenseShape, Eigen::DenseShape, 8>::evalTo<Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&) + 198
    frame #11: 0x0000555555b0ac57 main`void Eigen::internal::call_dense_assignment_loop<Eigen::Matrix<double, -1, -1, 1, -1, -1>, Eigen::Transpose<Eigen::Product<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, 0> const>, Eigen::internal::assign_op<double, double> >(Eigen::Matrix<double, -1, -1, 1, -1, -1>&, Eigen::Transpose<Eigen::Product<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, 0> const> const&, Eigen::internal::assign_op<double, double> const&) + 103
    frame #12: 0x0000555555b0a810 main`void Eigen::internal::call_dense_assignment_loop<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Transpose<Eigen::Diagonal<Eigen::Product<Eigen::Transpose<Eigen::Product<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, 0> const>, Eigen::Block<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, -1, -1, false>, -1, -1, false>, 0> const, 0> const>, Eigen::internal::assign_op<double, double> >(Eigen::Matrix<double, -1, -1, 0, -1, -1>&, Eigen::Transpose<Eigen::Diagonal<Eigen::Product<Eigen::Transpose<Eigen::Product<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, 0> const>, Eigen::Block<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, -1, -1, false>, -1, -1, false>, 0> const, 0> const> const&, Eigen::internal::assign_op<double, double> const&) + 240
    frame #13: 0x0000555555aea663 main`PointToPlaneErrorMinimizer<double>::compute_in_place(PointMatcher<double>::ErrorMinimizer::ErrorElements&) + 1363
    frame #14: 0x0000555555aea06e main`PointToPlaneErrorMinimizer<double>::compute(PointMatcher<double>::ErrorMinimizer::ErrorElements const&) + 46
    frame #15: 0x0000555555aa0a30 main`PointMatcher<double>::ErrorMinimizer::compute(PointMatcher<double>::DataPoints const&, PointMatcher<double>::DataPoints const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, PointMatcher<double>::Matches const&) + 64
    frame #16: 0x00005555559df4b5 main`PointMatcher<double>::ICP::computeWithTransformedReference(PointMatcher<double>::DataPoints const&, PointMatcher<double>::DataPoints const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&) + 1365
    frame #17: 0x00005555559debd2 main`PointMatcher<double>::ICP::compute(PointMatcher<double>::DataPoints const&, PointMatcher<double>::DataPoints const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&) + 3090
    frame #18: 0x00005555559ddf8a main`PointMatcher<double>::ICP::operator()(PointMatcher<double>::DataPoints const&, PointMatcher<double>::DataPoints const&) + 74

[ethzasl-jenkins]

Can one of the admins verify this patch?

[pomerlef]

ok to test

[pomerlef]

@kubelvla can you have a look?

[kubelvla]

Thanks for catching this bug, I'll check if it were better to change the Gamma matrix to 4x4 homogeneous version, or whether to use the sub-block. But I definitely need to re-check the sizes of the matrices, my bad.

[kubelvla]

@pomerlef I've checked the fix, the proposed one is the best solution. So far, I've been lucky that Eigen somehow multiplies the wrong sized matrices correctly, but apparently not always. The proposed fix does not change the numerical values of the cross variable, and the mapper works the same (without the crashes, of course). I have verified it on our darpa data. You can merge the pull request.

[pomerlef]

Ok to test

[YoshuaNava]

Also LGTM.

@kubelvla a proposal from my side would be to generalize the approach to optimize ICP in a sub-space with a more flexible method, e.g. http://www.seas.ucla.edu/~vandenbe/133A/lectures/cls.pdf, slide 2.

With this, the DoF to optimize could be a dynamic parameter of the Error Minimizer. A constraint built in this way could be more natural / flexible to implement.

What do you think?

[kubelvla]

@YoshuaNava Hi, good idea. I'll give it a look, although I'm mostly concerned with changing diapers and driving a baby carriage these months, so the results my take some time :D

[YoshuaNava]

@kubelvla No worries, all the best and thank you for looking into it. The 4DOF implementation is very useful :muscle: