R is not orthogonal about fast_gicp HOT 15 CLOSED

I meet this problem too. But you may avoid it. In my experience, it happens when the point clouds have little overlap or wrong initial guess. Anyway I think it means failed to get a proper solution. So when this happens, means track failed.

from fast_gicp.

Normalizing the rotation part of your initial guess before running registration would fix that error.

Eigen::Matrix4f guess = ...;
guess.block<3, 3>(0, 0) = Eigen::Quaternionf(guess.block<3, 3>(0, 0)).normalized().toRotationMatrix();

from fast_gicp.

guess.block<3, 3>(0, 0) = Eigen::Quaternionf(guess.block<3, 3>(0, 0)).normalized().toRotationMatrix();

Hi, I use identity as init guess; it still happens occasionally.

    Eigen::Matrix4f initial_guess = Eigen::Matrix4f::Identity();
    reg.align(*tmpCloud, initial_guess);

optimize fail? maybe.

from fast_gicp.

Can you check if putting normalization code just below the following line would help?

https://github.com/SMRT-AIST/fast_gicp/blob/8e0d2cfde3bf1fc2a9d7f2851ce31d3ee8be6197/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp#L138

    // normalize x1_
    x1_.linear() = Eigen::Quaternionf(x1_.linear()).normalized().toRotationMatrix();

from fast_gicp.

Normalizing the rotation part of your initial guess before running registration would fix that error.

Eigen::Matrix4f guess = ...;
guess.block<3, 3>(0, 0) = Eigen::Quaternionf(guess.block<3, 3>(0, 0)).normalized().toRotationMatrix();

Actually, I didn't really understand where I should add this code. If you could write it like above, it would be great.

from fast_gicp.

Thanks. Please try to put the following code here

Eigen::Isometry3f x1_ = delta_.inverse() * x0_;
x1_.linear() = Eigen::Quaternionf(x1_.linear()).normalized().toRotationMatrix();

from fast_gicp.

It still gives the same error.

from fast_gicp.

Thanks for your coorperation. The cause is probably the unstable optimization as you said. In the current implementation, it uses a naive GN optimizer that can be unreliable when the initial guess is far from the true solution. I'll implement a more robust LM optimizer in next weeks.

from fast_gicp.

Thank you for you effort and help. I’ll wait.

from fast_gicp.

thanks. I'll try and share the results.

from fast_gicp.

I tried it and now it gives another error which is
[pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!
Applying surface meshing...Using surface method: gp3 ...
[pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!

from fast_gicp.

@gulfemakpinar
I pushed updated implementation of FastGICP to devel branch. The updated code uses a more robust LM optimizer and avoids SO3::log() to prevent the non-orthogonal R error. I would be glad if you could check if it works well in your environment at your convenience.

from fast_gicp.

@gulfemakpinar
I pushed updated implementation of FastGICP to devel branch. The updated code uses a more robust LM optimizer and avoids SO3::log() to prevent the non-orthogonal R error. I would be glad if you could check if it works well in your environment at your convenience.

Hi, I find it works perfectly on cpu version. No more SO3 errors again.but cuda version still meet this problem. I think cuda version still use gauss newton .Any schedule to fix the cuda version?

from fast_gicp.

@cdb0y511 Thanks for testing it. I gonna improve the optimization of the cuda version as well in next week.

from fast_gicp.

Sorry for the late response. It works perfectly on my environment. Thank you.

from fast_gicp.