Posted by
huibuh on
URL: http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261.html
Hi Mat
Let me first give you some context why I am trying out rtabmap LOAM integration:
When running a Ouster OS0-64 at 1024x20 mode (20Hz) with a setup similar to this
https://github.com/introlab/rtabmap_ros/blob/master/launch/tests/test_ouster_gen2.launchon a tracked mobile robot I am getting great mapping results. No matter what I do though, I cannot get consistent odometry update rates from icp_odometry using the default algorithm Odom/Strategy=0 (Frame-to-Map). I tried increasing Icp/VoxelSize, but this also does not help much. Most of the time, rate of "rostopic hz /icp_odometry/odom" starts off with 20Hz, but then quickly drops to 8-12Hz when the robot is moving in more complex environments. After that, the rate never recovers and stays low, no matter of the robot's environment. This becomes an issue when the robot is moving fast (it can do up to 11km/h) and does quick turns.
Having read your latest paper in Journal of Field Robotics, I figured I'd try whether LOAM can give be more Hz and hopefully similar localization performance as Frame-to-Map. So I added the loam_velodyne ROS package and recompiled rtabmap. There, I noticed a compile error, which I fixed (see below). Once I had rtabmap with LOAM working, I switched to "Odom/Strategy=7 (LOAM)". When running the same bag file that worked nicely with Odom/Strategy=0, the icp_odometry node repeatably crashes hard when Odom/Strategy=7 is used (no error in log file).
I was hoping you could take a look what the issue might be or provide general advice how the 3D LIDAR icp odom rate could be increased. (Btw., I also experimented with using robot_localization, fusing in wheel odometry and/or IMU, but this just decreases the map result.)
Here are the details of my setup:
$ rtabmap --version
RTAB-Map: 0.20.9
PCL: 1.8.1
With VTK: 6.3.0
OpenCV: 3.2.0
With OpenCV xfeatures2d: false
With OpenCV nonfree: false
With ORB OcTree: true
With SuperPoint Torch: false
With Python3: false
With FastCV: false
With Madgwick: true
With TORO: true
With g2o: true
With GTSAM: true
With Vertigo: true
With CVSBA: false
With Ceres: true
With OpenNI2: true
With Freenect: true
With Freenect2: false
With K4W2: false
With K4A: true
With DC1394: true
With FlyCapture2: false
With ZED: false
With ZED Open Capture: false
With RealSense: false
With RealSense SLAM: false
With RealSense2: false
With MYNT EYE S: false
With DepthAI: false
With libpointmatcher: true
With CCCoreLib: false
With octomap: true
With cpu-tsdf: false
With open chisel: false
With Alice Vision: false
With LOAM: true
With FOVIS: false
With Viso2: false
With DVO: false
With ORB_SLAM: false
With OKVIS: false
With MSCKF_VIO: false
With VINS-Fusion: false
rtabmap_ros: 0.20.9-melodic
loam_velodyne: latest commit on master (25db5)
Ubuntu Linux 18.04 + ROS up to date
Hardware: Intel NUC 8 Pro (NUC8v7PNB with i7-8665U @4.8GHz)
Contents of icp_odometry node log file after crash:
$ cat /home/nuc/.ros/log/c3b6e4e2-db3b-11eb-8e3c-08719055bced/rtabmap-icp_odometry-icp_odometry-21-stdout.log
[ INFO] [1625233546.255426022]: Initializing nodelet with 12 worker threads.
[ INFO] [1625233546.361902031]: Odometry: frame_id = base_footprint
[ INFO] [1625233546.361925564]: Odometry: odom_frame_id = odom
[ INFO] [1625233546.361933626]: Odometry: publish_tf = true
[ INFO] [1625233546.361943417]: Odometry: wait_for_transform = true
[ INFO] [1625233546.361960542]: Odometry: wait_for_transform_duration = 0.100000
[ INFO] [1625233546.362003503]: Odometry: initial_pose = xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000
[ INFO] [1625233546.362015784]: Odometry: ground_truth_frame_id =
[ INFO] [1625233546.362031641]: Odometry: ground_truth_base_frame_id = base_footprint
[ INFO] [1625233546.362042170]: Odometry: config_path =
[ INFO] [1625233546.362056669]: Odometry: publish_null_when_lost = true
[ INFO] [1625233546.362066856]: Odometry: guess_frame_id = base_footprint
[ INFO] [1625233546.362077421]: Odometry: guess_min_translation = 0.000000
[ INFO] [1625233546.362088826]: Odometry: guess_min_rotation = 0.000000
[ INFO] [1625233546.362099402]: Odometry: guess_min_time = 0.000000
[ INFO] [1625233546.362108958]: Odometry: expected_update_rate = 25.000000 Hz
[ INFO] [1625233546.362123697]: Odometry: max_update_rate = 0.000000 Hz
[ INFO] [1625233546.362138203]: Odometry: wait_imu_to_init = true
[ INFO] [1625233546.362169913]: Odometry: stereoParams_=0 visParams_=0 icpParams_=1
[ INFO] [1625233546.374944006]: Setting odometry parameter "Icp/CorrespondenceRatio"="0.01"
[ INFO] [1625233546.375467998]: Setting odometry parameter "Icp/DownsamplingStep"="1"
[ INFO] [1625233546.376838971]: Setting odometry parameter "Icp/Epsilon"="0.001"
[ INFO] [1625233546.379977099]: Setting odometry parameter "Icp/Iterations"="10"
[ INFO] [1625233546.381118010]: Setting odometry parameter "Icp/MaxCorrespondenceDistance"="1"
[ INFO] [1625233546.387446313]: Setting odometry parameter "Icp/MaxTranslation"="2"
[ INFO] [1625233546.412559558]: Setting odometry parameter "Icp/PointToPlane"="true"
[ INFO] [1625233546.417987661]: Setting odometry parameter "Icp/PointToPlaneK"="20"
[ INFO] [1625233546.426043618]: Setting odometry parameter "Icp/PointToPlaneRadius"="0"
[ INFO] [1625233546.438621417]: Setting odometry parameter "Icp/VoxelSize"="0.3"
[ INFO] [1625233546.487629519, 1618412014.285835309]: Setting odometry parameter "Odom/ScanKeyFrameThr"="0.95"
[ INFO] [1625233546.488459546, 1618412014.285835309]: Setting odometry parameter "Odom/Strategy"="7"
[ INFO] [1625233546.501578222, 1618412014.295894469]: Setting odometry parameter "OdomF2M/ScanMaxSize"="15000"
[ INFO] [1625233546.507687755, 1618412014.305949106]: Setting odometry parameter "OdomF2M/ScanSubtractRadius"="0.3"
[ INFO] [1625233546.838164195, 1618412014.628925352]: Setting odometry parameter "Reg/Force3DoF"="false"
[ INFO] [1625233547.041578799, 1618412014.832256708]: odometry: Subscribing to IMU topic /imu_filter/data
[ INFO] [1625233547.046826128, 1618412014.842273122]: IcpOdometry: scan_cloud_max_points = 0
[ INFO] [1625233547.046864977, 1618412014.842273122]: IcpOdometry: scan_downsampling_step = 1
[ INFO] [1625233547.046890486, 1618412014.842273122]: IcpOdometry: scan_range_min = 0.000000 m
[ INFO] [1625233547.046931574, 1618412014.842273122]: IcpOdometry: scan_range_max = 0.000000 m
[ INFO] [1625233547.046953270, 1618412014.842273122]: IcpOdometry: scan_voxel_size = 0.300000 m
[ INFO] [1625233547.046970077, 1618412014.842273122]: IcpOdometry: scan_normal_k = 20
[ INFO] [1625233547.046988445, 1618412014.842273122]: IcpOdometry: scan_normal_radius = 0.000000 m
Output command line:
[rtabmap/icp_odometry/icp_odometry-21] process has died [pid 10383, exit code -11, cmd /home/nuc/catkin_ws/devel/lib/rtabmap_ros/icp_odometry scan_cloud:=/ouster_filter/cropbox/output imu:=/imu_filter/data __name:=icp_odometry __log:=/home/nuc/.ros/log/b8e386b6-dd74-11eb-8778-08719055bced/rtabmap-icp_odometry-icp_odometry-21.log].
log file: /home/nuc/.ros/log/b8e386b6-dd74-11eb-8778-08719055bced/rtabmap-icp_odometry-icp_odometry-21*.log
As mentioned above, when trying to compile rtabmap 0.20.9 I get a compile error:
/home/nuc/code/rtabmap/corelib/src/odometry/OdometryLOAM.cpp: In member function ‘virtual rtabmap::Transform rtabmap::OdometryLOAM::computeTransform(rtabmap::SensorData&, const rtabmap::Transform&, rtabmap::OdometryInfo*)’:
/home/nuc/code/rtabmap/corelib/src/odometry/OdometryLOAM.cpp:286:169: error: no matching function for call to ‘rtabmap::LaserScan::backwardCompatibility(rtabmap::LaserScan, int, float, rtabmap::Transform)’
n::backwardCompatibility(util3d::laserScanFromPointCloud(out), 0, data.laserScanRaw().rangeMax(), data.laserScanRaw().localTransform());
This is how I fixed it:
~/code/rtabmap$ git diff corelib/src/odometry/OdometryLOAM.cpp
diff --git a/corelib/src/odometry/OdometryLOAM.cpp b/corelib/src/odometry/OdometryLOAM.cpp
index b57a2b76..f87ea4a2 100644
--- a/corelib/src/odometry/OdometryLOAM.cpp
+++ b/corelib/src/odometry/OdometryLOAM.cpp
@@ -283,7 +283,7 @@ Transform OdometryLOAM::computeTransform(
Transform rot(0,0,1,0,1,0,0,0,0,1,0,0);
pcl::PointCloud<pcl::PointXYZI> out;
pcl::transformPointCloud(laserMapping_->laserCloudSurroundDS(), out, rot.toEigen3f());
- info->localScanMap = LaserScan::backwardCompatibility(util3d::laserScanFromPointCloud(out), 0, data.laserScanRaw().rangeMax(), data.laserScanRaw().localTransform());
+ info->localScanMap = LaserScan::backwardCompatibility(util3d::laserScanFromPointCloud(out)
.data(), 0, data.laserScanRaw().rangeMax(), data.laserScanRaw().localTransform());
}
}
}
As always, your help is highly appreciated and thank you for your great work!
Regards,
Heiko