scan matching has not converged!! about fast_gicp

[liuyf5231]

another bug,
matching.Sophus ensure failed in function 'Sophus::SO3<Scalar_, Options>::SO3(const Transformation&) [with Scalar_ = float; int Options = 0; Sophus::SO3<Scalar_, Options>::Transformation = Eigen::Matrix<float, 3, 3>]', file '/..../src/fast_gicp/thirdparty/Sophus/sophus/so3.hpp', line 473. R is not orthogonal: 1.00001 -1.83471e-07 -1.13947e-06 -1.83471e-07 1.00001 4.81601e-07 -1.13947e-06 4.81601e-07

[hitbg-yjm]

How do you solve the problem？ we have the same problem.

[koide3]

This error happens when the rotation part of the initial guess is not orthogonal. A workaround is to do normalize the initial guess like:

Eigen::Matrix4d initial_guess = ...;
initial_guess.block<3, 3>(0, 0) = Eigen::Quaterniond(initial_guess.block<3, 3>(0, 0)).normalized().toRotationMatrix();
gicp.align(aligned_cloud, inigial_guess.cast<float>());

[hitbg-yjm]

This error happens when the rotation part of the initial guess is not orthogonal. A workaround is to do normalize the initial guess like:

Eigen::Matrix4d initial_guess = ...;
initial_guess.block<3, 3>(0, 0) = Eigen::Quaterniond(initial_guess.block<3, 3>(0, 0)).normalized().toRotationMatrix();
gicp.align(aligned_cloud, inigial_guess.cast<float>());

Thank you for your response. I will try this way ,it helps a lot.

[gulfemakpinar]

I am using FastVGICP and I faced with the same problem. Could you help me? matching.Sophus ensure failed in function 'Sophus::SO3<Scalar_, Options>::SO3(const Transformation&) [with Scalar_ = float; int Options = 0; Sophus::SO3<Scalar_, Options>::Transformation = Eigen::Matrix<float, 3, 3>]', file '/..../src/fast_gicp/thirdparty/Sophus/sophus/so3.hpp', line 473. I got this message and the system is terminated.