Running the graph_slam algorithm with registration_method set to FAST_GICP, the map builder tries to align loop closure keyframes and the program crashes with a similar "R is not orthogonal" error to the issue in the fast_gicp repository:
https://github.com/SMRT-AIST/fast_gicp/issues/25
--- loop detection ---
num_candidates: 1
matchingSophus 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 '/home/micho/ros/class/src/fast_gicp/thirdparty/Sophus/sophus/so3.hpp', line 472.
R is not orthogonal:
1.00001 3.27826e-07 4.48897e-07
3.27826e-07 1.00001 -1.05239e-06
4.48897e-07 -1.05239e-06 1
[hdl_slam_nodelet_manager-7] process has died [pid 55734, exit code -6, cmd /opt/ros/noetic/lib/nodelet/nodelet manager __name:=hdl_slam_nodelet_manager __log:=/home/
After some debugging, I discovered the crash happens when calling the align method of the FastGICP class from the matching method of LoopDetector, and the crash always correlates with either the new keyframe Isometry3d or the candidate Isometry3d being unnormalized. Whenever the crash happens, the magnitude of the equivalent quaternions are always larger than 1, but never larger than about 1.00001.
It appears FastGICP inherits align directly from pcl::Registration, so it doesn't seem to be fixable from FastGICP.
Therefore, I applied the same technique you recommended in the referenced issue to normalize the orientation of both the new keyframe and the candidates; convert the un-orthogonal rotation matrix to a quaternion, normalize it, then convert back to a rotation matrix. This PR reflects these changes. In my test case, I can reliably reproduce the crash without the orientation normalization, and reliably prevent the crash with the normalization.
If it would help, I could provide the bag file and my launch/parameter configuration. I am running this in ROS Noetic with PCL version 1.10.
koide3
commented
3 years ago
Running the graph_slam algorithm with
registration_methodset toFAST_GICP, the map builder tries to align loop closure keyframes and the program crashes with a similar "R is not orthogonal" error to the issue in thefast_gicprepository: https://github.com/SMRT-AIST/fast_gicp/issues/25After some debugging, I discovered the crash happens when calling the
alignmethod of theFastGICPclass from thematchingmethod ofLoopDetector, and the crash always correlates with either the new keyframeIsometry3dor the candidateIsometry3dbeing unnormalized. Whenever the crash happens, the magnitude of the equivalent quaternions are always larger than 1, but never larger than about 1.00001.It appears
FastGICPinheritsaligndirectly frompcl::Registration, so it doesn't seem to be fixable fromFastGICP. Therefore, I applied the same technique you recommended in the referenced issue to normalize the orientation of both the new keyframe and the candidates; convert the un-orthogonal rotation matrix to a quaternion, normalize it, then convert back to a rotation matrix. This PR reflects these changes. In my test case, I can reliably reproduce the crash without the orientation normalization, and reliably prevent the crash with the normalization.If it would help, I could provide the bag file and my launch/parameter configuration. I am running this in ROS Noetic with PCL version 1.10.