Motion model distortion speed enhancement
Thanks for sharing the great work! I used a portion of the code and found a spot that can use a speedup. In the motion model source file here: https://gitlab.kitware.com/keu-computervision/slam/-/blob/master/slam_lib/src/MotionModel.cxx
When linear interpolation is performed on every point in a pointcloud this becomes somewhat time consuming for large pointclouds. In my tests it appears by changing a few lines we can get a fair speedup.
From:
Eigen::Quaterniond rot(Eigen::Quaterniond(H0.rotation()).slerp(time, Eigen::Quaterniond(H1.rotation())));
Eigen::Translation3d trans(H0.translation() + time * (H1.translation() - H0.translation()));
To:
Eigen::Quaterniond Q0(H0.matrix().block<3,3>(0,0));
Eigen::Quaterniond Q1(H1.matrix().block<3,3>(0,0));
Eigen::Quaterniond rot(Q0.slerp(time, Q1);
Eigen::Translation3d trans(H0.translation() + time * (H1.translation() - H0.translation()));
It seems that the default Quaternion constructor and using the H0.Rotation() matrix extraction is fairly slow. I hope you find the same speedup I did. I don't have exact numbers as it has been a while since I last worked with this chunk of code.
Cheers, -Sam
Edited by Nicolas Cadart