icp_odometry with LOAM crash

classic Classic list List threaded Threaded
16 messages Options
Reply | Threaded
Open this post in threaded view
|

icp_odometry with LOAM crash

huibuh
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.launch
on 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
Reply | Threaded
Open this post in threaded view
|

Re: icp_odometry with LOAM crash

matlabbe
Administrator
Hi,

For the LOAM build error, a commit has been added recently: https://github.com/introlab/rtabmap/commit/ae92ec40d74abde8925a6d047c281062319e430a

For the crash, LOAM for the paper was tested using this version: https://github.com/introlab/rtabmap/blob/ae92ec40d74abde8925a6d047c281062319e430a/docker/jfr2018/Dockerfile#L133

To have faster icp_odometry, you can also try Frame-To-Frame (F2F) approach (Odom/Strategy=1). If you are based on test_ouster_gen2.launch, increase voxel_size. Icp/PointToPlane could be set to false (this will avoid to compute normals). OdomF2M/ScanMaxSize could be decreased.

cheers,
Mathieu
Reply | Threaded
Open this post in threaded view
|

Re: icp_odometry with LOAM crash

huibuh
Hi,

Your LOAM build error commit works as expected, master branch now compiles with LOAM support.

I checked out commit a4c364 of package loam_velodyne and recompiled, but still no luck: With Odom/Strategy=7, the icp_odometry node crashes immediately.

I also tried out your suggestions to make icp_odometry faster, trying out the following in separate tests:
- Switching to Frame-To-Frame (F2F) approach (Odom/Strategy=1) doesn't work at all with my dataset, generated map becomes messy very quickly once the robot starts moving.
- I increased Icp/VoxelSize from 0.3 to 0.6 and also tried 1.0. This does positively affect the frame rate, but at the same time notably increases the robot position error (large base_footprint position jitter even when robot is stationary).
- Setting Icp/PointToPlane="false" also results in very bad map quality, localization is very poor
- Decreasing OdomF2M/ScanMaxSize from 15000 to 7500 and later 1500 did not result in much of a measurable improvement of icp_odometry frequency.

For now, I get the best results running the Ouster at only 10 Hz. At this input rate, icp_odometry can keep up 10Hz, too (using Icp/VoxelSize=0.3 for indoor setting, 0.5 outdoors).

Please let me know if you have any more ideas what else I could try out.

Cheers,
Heiko

Reply | Threaded
Open this post in threaded view
|

Re: icp_odometry with LOAM crash

matlabbe
Administrator
Hi,

For LOAM, I tested with current local build on my focal/noetic machine and it also crashes with this error in gdb:
Thread 1 "rtabmap-kitti_d" received signal SIGSEGV, Segmentation fault.
__GI___libc_free (mem=0xaa91) at malloc.c:3102
3102	malloc.c: No such file or directory.
(gdb) bt
#0  __GI___libc_free (mem=0xaa91) at malloc.c:3102
#1  0x00007fffefd424d5 in loam::BasicScanRegistration::extractFeatures(unsigned short const&) ()
    at /home/mathieu/catkin_ws/devel/lib/libloam.so
#2  0x00007fffefd42dc4 in loam::BasicScanRegistration::processScanlines(std::chrono::time_point<std::chrono::_V2::system_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > > const&, std::vector<pcl::PointCloud<pcl::PointXYZI>, std::allocator<pcl::PointCloud<pcl::PointXYZI> > > const&) ()
    at /home/mathieu/catkin_ws/devel/lib/libloam.so
#3  0x00007ffff776a319 in rtabmap::OdometryLOAM::computeTransform(rtabmap::SensorData&, rtabmap::Transform const&, rtabmap::OdometryInfo*) () at /home/mathieu/catkin_ws/devel/lib/librtabmap_core.so.0.20
#4  0x00007ffff773b141 in rtabmap::Odometry::process(rtabmap::SensorData&, rtabmap::Transform const&, rtabmap::OdometryInfo*) () at /home/mathieu/catkin_ws/devel/lib/librtabmap_core.so.0.20
#5  0x00007ffff773ed1b in rtabmap::Odometry::process(rtabmap::SensorData&, rtabmap::OdometryInfo*) ()
    at /home/mathieu/catkin_ws/devel/lib/librtabmap_core.so.0.20
#6  0x0000555555564d3c in main ()

From the kinetic docker image corresponding to the related paper, I could make LOAM working even with latest rtabmap version. Here is the log of rtabmap build with working loam from inside the image `/root/rtabmap_loam/build`:
cmake -DWITH_MSCKF_VIO=OFF -DWITH_LOAM=ON -DWITH_G2O=ON -DWITH_FOVIS=OFF -DWITH_DVO=OFF -DWITH_OKVIS=OFF -DWITH_VISO2=OFF -DWITH_CERES=OFF ..
-- The C compiler identification is GNU 5.4.0
-- The CXX compiler identification is GNU 5.4.0
-- Check for working C compiler: /usr/bin/cc
-- Check for working C compiler: /usr/bin/cc -- works
-- Detecting C compiler ABI info
-- Detecting C compiler ABI info - done
-- Detecting C compile features
-- Detecting C compile features - done
-- Check for working CXX compiler: /usr/bin/c++
-- Check for working CXX compiler: /usr/bin/c++ -- works
-- Detecting CXX compiler ABI info
-- Detecting CXX compiler ABI info - done
-- Detecting CXX compile features
-- Detecting CXX compile features - done
-- MOBILE_BUILD=OFF
-- Checking for module 'eigen3'
--   Found eigen3, version 3.2.92
-- Found eigen: /usr/include/eigen3  
-- Checking for module 'libopenni'
--   Found libopenni, version 1.5.4.0
-- Found openni: /usr/lib/libOpenNI.so  
-- Checking for module 'libopenni2'
--   Found libopenni2, version 2.2.0.3
-- Found OpenNI2: /usr/lib/libOpenNI2.so  
-- The imported target "vtkRenderingPythonTkWidgets" references the file
   "/usr/lib/x86_64-linux-gnu/libvtkRenderingPythonTkWidgets.so"
but this file does not exist.  Possible reasons include:
* The file was deleted, renamed, or moved to another location.
* An install or uninstall procedure did not complete successfully.
* The installation package was faulty and contained
   "/usr/lib/cmake/vtk-6.2/VTKTargets.cmake"
but not all the files it references.

-- The imported target "vtk" references the file
   "/usr/bin/vtk"
but this file does not exist.  Possible reasons include:
* The file was deleted, renamed, or moved to another location.
* An install or uninstall procedure did not complete successfully.
* The installation package was faulty and contained
   "/usr/lib/cmake/vtk-6.2/VTKTargets.cmake"
but not all the files it references.

-- Found libusb-1.0: /usr/include  
-- Checking for module 'flann'
--   Found flann, version 1.8.4
-- Found Flann: /usr/lib/x86_64-linux-gnu/libflann_cpp_s.a  
-- Found qhull: /usr/lib/x86_64-linux-gnu/libqhull.so  
-- Found PCL_COMMON: /usr/lib/x86_64-linux-gnu/libpcl_common.so  
-- Found PCL_OCTREE: /usr/lib/x86_64-linux-gnu/libpcl_octree.so  
-- Found PCL_IO: /usr/lib/x86_64-linux-gnu/libpcl_io.so  
-- Found PCL_KDTREE: /usr/lib/x86_64-linux-gnu/libpcl_kdtree.so  
-- Found PCL_SEARCH: /usr/lib/x86_64-linux-gnu/libpcl_search.so  
-- Found PCL_SURFACE: /usr/lib/x86_64-linux-gnu/libpcl_surface.so  
-- Found PCL_FILTERS: /usr/lib/x86_64-linux-gnu/libpcl_filters.so  
-- Found PCL_FEATURES: /usr/lib/x86_64-linux-gnu/libpcl_features.so  
-- Found PCL_REGISTRATION: /usr/lib/x86_64-linux-gnu/libpcl_registration.so  
-- Found PCL_SAMPLE_CONSENSUS: /usr/lib/x86_64-linux-gnu/libpcl_sample_consensus.so  
-- Found PCL_GEOMETRY: /usr/include/pcl-1.7  
-- Found PCL_SEGMENTATION: /usr/lib/x86_64-linux-gnu/libpcl_segmentation.so  
-- Found PCL_VISUALIZATION: /usr/lib/x86_64-linux-gnu/libpcl_visualization.so  
-- PCL definitions don't contain "-march=native", make sure all libraries using Eigen are also compiled without that flag to avoid some segmentation faults (with gdb referring to some Eigen functions).
-- Found Sqlite3: /usr/include /usr/lib/x86_64-linux-gnu/libsqlite3.so
-- Try OpenMP C flag = [-fopenmp]
-- Performing Test OpenMP_FLAG_DETECTED
-- Performing Test OpenMP_FLAG_DETECTED - Success
-- Try OpenMP CXX flag = [-fopenmp]
-- Performing Test OpenMP_FLAG_DETECTED
-- Performing Test OpenMP_FLAG_DETECTED - Success
-- Found OpenMP: -fopenmp  
-- Found OpenMP: 
-- Found OpenCV: /usr/local/include/opencv;/usr/local/include
-- Found PCL: /usr/include/pcl-1.7;/usr/include/eigen3;/usr/include;/usr/include/ni;/usr/include/openni2;/usr/include/vtk-6.2;/usr/include/x86_64-linux-gnu;/usr/include/freetype2;/usr/include/x86_64-linux-gnu/freetype2;/usr/lib/openmpi/include/openmpi/opal/mca/event/libevent2021/libevent;/usr/lib/openmpi/include/openmpi/opal/mca/event/libevent2021/libevent/include;/usr/lib/openmpi/include;/usr/lib/openmpi/include/openmpi;/usr/include/python2.7;/usr/include/jsoncpp;/usr/include/hdf5/openmpi;/usr/include/libxml2;/usr/include/tcl
-- Found ZLIB: /usr/include
-- VTK_RENDERING_BACKEND=OpenGL
-- Found Freenect: /usr/include
-- Found OpenNI2: /usr/include/openni2
-- Found CSPARSE: /usr/include/suitesparse  
-- Old g2o version detected with c++03 interface (config file: /usr/local/include/g2o/config.h).
-- Found g2o: /usr/local/include;/usr/include/suitesparse;/usr/include/suitesparse
-- GTSAM include directory:  /usr/local/lib/cmake/GTSAM/../../../include;/usr/include/eigen3
-- FlyCapture2_INCLUDE_DIR=FlyCapture2_INCLUDE_DIR-NOTFOUND
-- FlyCapture2_LIBRARY=FlyCapture2_LIBRARY-NOTFOUND
-- Triclops_INCLUDE_DIR=Triclops_INCLUDE_DIR-NOTFOUND
-- Triclops_LIBRARY=Triclops_LIBRARY-NOTFOUND
-- FlyCaptureBridge_LIBRARY=FlyCaptureBridge_LIBRARY-NOTFOUND
-- Found libpointmatcher: /usr/local/include;/usr/include;/usr/include/eigen3;/usr/local/include;/root/libpointmatcher/pointmatcher
-- Boost version: 1.58.0
-- Found the following Boost libraries:
--   thread
--   filesystem
--   system
--   program_options
--   date_time
--   chrono
--   atomic
-- Boost version: 1.58.0
-- Found the following Boost libraries:
--   thread
--   filesystem
--   system
--   program_options
--   date_time
--   chrono
--   timer
--   atomic
-- Using CATKIN_DEVEL_PREFIX: /root/rtabmap_loam/build/devel
-- Using CMAKE_PREFIX_PATH: /root/catkin_ws/devel;/opt/ros/kinetic
-- This workspace overlays: /root/catkin_ws/devel;/opt/ros/kinetic
-- Found PythonInterp: /usr/bin/python (found version "2.7.12") 
-- Using PYTHON_EXECUTABLE: /usr/bin/python
-- Using Debian Python package layout
-- Using empy: /usr/bin/empy
-- Using CATKIN_ENABLE_TESTING: ON
-- Call enable_testing()
-- Using CATKIN_TEST_RESULTS_DIR: /root/rtabmap_loam/build/test_results
-- Found gmock sources under '/usr/src/gmock': gmock will be built
-- Found Threads: TRUE  
-- Found gtest sources under '/usr/src/gmock': gtests will be built
-- Using Python nosetests: /usr/bin/nosetests-2.7
-- catkin 0.7.14
-- Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
-- Found loam_velodyne: /root/catkin_ws/src/loam_velodyne/include;/usr/include/eigen3;/usr/include/pcl-1.7;/usr/include;/usr/include/ni;/usr/include/openni2;/usr/include/vtk-6.2;/usr/include/hdf5/openmpi;/usr/lib/openmpi/include/openmpi/opal/mca/event/libevent2021/libevent;/usr/lib/openmpi/include/openmpi/opal/mca/event/libevent2021/libevent/include;/usr/lib/openmpi/include;/usr/lib/openmpi/include/openmpi;/usr/include/jsoncpp;/usr/include/libxml2;/usr/include/x86_64-linux-gnu;/usr/include/freetype2;/usr/include/x86_64-linux-gnu/freetype2;/usr/include/python2.7;/usr/include/tcl;/opt/ros/kinetic/include;/opt/ros/kinetic/share/xmlrpcpp/cmake/../../../include/xmlrpcpp
-- Found octomap 1.8.1: /opt/ros/kinetic/include
-- Architecture: x86_64
-- Performing Test COMPILER_SUPPORTS_CXX14
-- Performing Test COMPILER_SUPPORTS_CXX14 - Success
-- Found Pthreads
-- --------------------------------------------
-- Info :
--   RTAB-Map Version =     0.20.13
--   CMAKE_VERSION =        3.5.1
--   CMAKE_INSTALL_PREFIX = /usr/local
--   CMAKE_BUILD_TYPE =     Release
--   CMAKE_INSTALL_LIBDIR = lib
--   BUILD_APP =            ON
--   BUILD_TOOLS =          ON
--   BUILD_EXAMPLES =       ON
--   BUILD_SHARED_LIBS =    ON
--   CMAKE_CXX_FLAGS =  -fmessage-length=0  -fopenmp -std=c++14
--   FLANN_KDTREE_MEM_OPT = OFF
--   PCL_DEFINITIONS = -DEIGEN_USE_NEW_STDVECTOR;-DEIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET;-DFLANN_STATIC;-Dqh_QHpointer
--   PCL_VERSION = 1.7.2
-- 
-- Optional dependencies ('*' affects some default parameters) :
--  *With OpenCV 3.1.0 xfeatures2d = YES, nonfree = YES (License: Non commercial)
--   With Qt 5.5.1            = YES (License: Open Source or Commercial)
--   With VTK 6.2              = YES (License: BSD)
--   With external SQLite3     = YES (License: Public Domain)
--   With ORB OcTree           = YES (License: GPLv3)
--   With SupertPoint          = NO (WITH_TORCH=OFF)
--   With Python3              = NO (WITH_PYTHON=OFF)
--   With Madgwick             = YES (License: GPL)
--   With FastCV               = NO (FastCV not found)
--   With PDAL                 = NO (PDAL not found)
-- 
--  Solvers:
--   With TORO                 = YES (License: Creative Commons [Attribution-NonCommercial-ShareAlike])
--  *With g2o                  = YES (License: BSD)
--  *With GTSAM                = YES (License: BSD)
--  *With Ceres                = NO (WITH_CERES=OFF)
--   With VERTIGO              = YES (License: GPLv3)
--   With cvsba                = NO (cvsba not found)
--  *With libpointmatcher      = YES (License: BSD)
--   With CCCoreLib            = NO (CCCoreLib not found)
-- 
--  Reconstruction Approaches:
--   With OCTOMAP              = YES (License: BSD)
--   With CPUTSDF              = NO (CPUTSDF not found)
--   With OpenChisel           = NO (open_chisel not found)
--   With AliceVision          = NO (WITH_ALICE_VISION=OFF)
-- 
--  Camera Drivers:
--   With Freenect             = YES (License: Apache v2 and/or GPLv2)
--   With OpenNI2              = YES (License: Apache v2)
--   With Freenect2            = NO (libfreenect2 not found)
--   With Kinect for Windows 2 = NO (Kinect for Windows 2 SDK not found)
--   With Kinect for Azure     = NO (Kinect for Azure SDK not found)
--   With dc1394               = NO (dc1394 not found)
--   With FlyCapture2/Triclops = NO (Point Grey SDK not found)
--   With ZED                  = NO (ZED sdk and/or cuda not found)
--   With ZEDOC                = NO (ZED Open Capture not found)
--   With RealSense            = NO (librealsense not found)
--   With RealSense2           = NO (librealsense2 not found)
--   With MyntEyeS             = NO (mynteye s sdk not found)
--   With DepthAI              = NO (depthai-core not found)
-- 
--  Odometry Approaches:
--   With loam_velodyne        = YES (License: BSD)
--   With libfovis             = NO (WITH_FOVIS=OFF)
--   With libviso2             = NO (WITH_VISO2=OFF)
--   With dvo_core             = NO (WITH_DVO=OFF)
--   With okvis                = NO (WITH_OKVIS=OFF)
--   With msckf_vio            = NO (WITH_MSCKF_VIO=OFF)
--   With VINS-Fusion          = NO (VINS-Fusion not found)
--   With OpenVINS             = NO (ov_msckf not found)
--   With ORB_SLAM             = NO (WITH_G2O should be OFF as ORB_SLAM uses its own g2o version)
-- Show all options with: cmake -LA | grep WITH_
-- --------------------------------------------
-- Configuring done
-- Generating done

Then log of kitti benchmark script using the modified docker above with latest rtabmap version:
./run_kitti_datasets.sh test_loam 7 1 07
[ WARN] (2021-08-18 20:14:19.642) Parameters.cpp:1112::parseArguments() Parameter migration from "OdomORBSLAM2/VocPath" to "OdomORBSLAM/VocPath" (value=/root/ORBvoc.txt).
[ WARN] (2021-08-18 20:14:19.642) Parameters.cpp:1112::parseArguments() Parameter migration from "OdomORBSLAM2/ThDepth" to "OdomORBSLAM/ThDepth" (value=40).
[ WARN] (2021-08-18 20:14:19.642) Parameters.cpp:1112::parseArguments() Parameter migration from "OdomORBSLAM2/Fps" to "OdomORBSLAM/Fps" (value=10).
[ WARN] (2021-08-18 20:14:19.642) Parameters.cpp:1112::parseArguments() Parameter migration from "OdomORBSLAM2/MaxFeatures" to "OdomORBSLAM/MaxFeatures" (value=2000).
[ WARN] (2021-08-18 20:14:19.642) Parameters.cpp:1112::parseArguments() Parameter migration from "Icp/PM" to "Icp/Strategy" (value=true).
[ WARN] (2021-08-18 20:14:19.642) Parameters.cpp:1112::parseArguments() Parameter migration from "Icp/PMOutlierRatio" to "Icp/OutlierRatio" (value=0.7).
Paths:
   Sequence number:  07
   Sequence path:    /root/datasets/kitti/dataset/sequences/07
   Output:           /root/results/kitti/07
   Output name:      test_loam
   left images:      /root/datasets/kitti/dataset/sequences/07/image_0
   right images:     /root/datasets/kitti/dataset/sequences/07/image_1
   calib.txt:        /root/datasets/kitti/dataset/sequences/07/calib.txt
   times.txt:        /root/datasets/kitti/dataset/sequences/07/times.txt
   Ground Truth:      /root/datasets/kitti/devkit/cpp/data/odometry/poses/07.txt
   Exposure Compensation: false
   Disparity:         false
   Scan:               /root/datasets/kitti/dataset/sequences/07/velodyne
   Scan only:          false
   Scan step:          1
   Scan voxel:         0.000000m
   Scan normal k:      0
   Scan normal radius: 0.000000
Saved calibration "test_loam_calib" to "/root/results/kitti/07"
Parameters:
   FAST/Threshold=12
   GFTT/MinDistance=7
   GFTT/QualityLevel=0.01
   Icp/CorrespondenceRatio=0.01
   Icp/Epsilon=0.0001
   Icp/Iterations=10
   Icp/MaxCorrespondenceDistance=1.5
   Icp/MaxTranslation=2
   Icp/OutlierRatio=0.7
   Icp/PMMatcherEpsilon=1
   Icp/PMMatcherKnn=3
   Icp/PointToPlane=false
   Icp/Strategy=true
   Kp/FlannRebalancingFactor=1.0
   Kp/MaxFeatures=750
   Mem/BinDataKept=true
   Mem/LaserScanVoxelSize=0.5
   Mem/STMSize=30
   Mem/UseOdomFeatures=false
   Odom/ScanKeyFrameThr=0.8
   Odom/Strategy=7
   OdomF2M/MaxSize=3000
   OdomF2M/ScanMaxSize=10000
   OdomF2M/ScanSubtractRadius=0.5
   OdomORBSLAM/Fps=10
   OdomORBSLAM/MaxFeatures=2000
   OdomORBSLAM/ThDepth=40
   OdomORBSLAM/VocPath=/root/ORBvoc.txt
   OdomViso2/MatchNmsN=7
   OdomViso2/MatchRadius=300
   RGBD/AngularUpdate=0
   RGBD/LinearUpdate=0
   RGBD/ProximityBySpace=false
   Reg/Strategy=1
   Rtabmap/CreateIntermediateNodes=true
   Rtabmap/DetectionRate=2
   Rtabmap/PublishRAMUsage=true
   Rtabmap/WorkingDirectory=/root/results/kitti/07
   Vis/CorGuessWinSize=20
   Vis/CorNNDR=0.6
   Vis/MaxFeatures=1500
RTAB-Map version: 0.20.13
Processing 1101 images...
..........Iteration 1101/1101: speed=0km/h camera=10ms, odom(quality=0.000000, kfs=0)=365ms, slam=197ms, rmse=0.387183m *
Total time=318.698992s
Saving trajectory ...
Saving /root/results/kitti/07/test_loam_poses.txt... done!
Ground truth comparison:
   KITTI t_err = 0.645350 %
   KITTI r_err = 0.004496 deg/m
   translational_rmse=   0.388508 m
   rotational_rmse=      0.596562 deg
Saving rtabmap database (with all statistics) to "/root/results/kitti/07/test_loam.db"
Do:
 $ rtabmap-databaseViewer /root/results/kitti/07/test_loam.db

$ rtabmap-report results/kitti/07/test_loam.db 
Database: results/kitti/07/test_loam.db
   test_loam.db (1101, s=1.000):	error lin=0.388m (max=1.571m, odom=1.605m) ang=0.6deg, slam: avg=89ms (max=299ms) loops=14, odom: avg=253ms (max=373ms), camera: avg=16ms, map=2289MB

I tried again on my ubuntu focal/noetic computer with exactly the same cmake options and I still have a creash on start. Not sure where the problem is coming from (Xenial versus Focal?).

To increase speed, another parameter could the number of ICP iterations: Icp/Iterations (default 30). I remember I set it to 10 for some 3D lidar demos.

cheers,
Mathieu
Reply | Threaded
Open this post in threaded view
|

Re: icp_odometry with LOAM crash

huibuh
Hi Mat

Thanks for looking into the LOAM crash issue again. As I wasn't able to get LOAM to work, I looked around what other state of the art 3D LIDAR odometry solutions are out there for ROS. I ended up using a fork of the very recently released F-LOAM implementation for ROS, see all the details here: https://github.com/flynneva/floam (also check out this related conversation here: https://github.com/wh200720041/floam/issues/37#issuecomment-915412576).

F-LOAM works extremely well for me. I can run the Ouster OS0-64 at 20Hz and get 20 Hz odom rate from the floam node while it only utilizes approx 60% of one of the NUC cores. It also deals extremely well with difficult scenarios (long, narrow corridors and tight spaces for example).

The maps produced by rtabmap also looks better than ever due to the improved odometry.

Regards,

Heiko
Reply | Threaded
Open this post in threaded view
|

Re: icp_odometry with LOAM crash

matlabbe
Administrator
Hi Heiko,

This is pretty cool, thanks for the link. I'll check if I can make a demo launch file for F-LOAM / RTAB-Map integration.

cheers,
Mathieu
Reply | Threaded
Open this post in threaded view
|

Re: icp_odometry with LOAM crash

matlabbe
Administrator
I added FLOAM support inside rtabmap. If you want to try, you need the original FLOAM approach and apply this patch. Rebuild RTAB-Map from source and cmake should detect it. Set "--Odom/Strategy 11" to use FLOAM. To avoid duplicating more parameters, Icp/VoxelSize is used as the local map resolution (e.g., set it to 0.4 for kitti dataset). Otherwise, see LOAM prameters:
rtabmap --params | grep LOAM
Icp/RangeMax and Icp/RangeMin can be used to set max_dis and min_dis FLOAM parameters.

Here is a result on kitti 07 dataset:
rtabmap-report ./results/kitti/07/test_floam.db
Database: ./results/kitti/07/test_floam.db
   test_floam.db (1101, s=1.000):	error lin=0.468m (max=0.524m, odom=0.505m) ang=0.6deg, slam: avg=97ms (max=234ms) loops=17, odom: avg=55ms (max=180ms), camera: avg=7ms, map=2214MB
Reply | Threaded
Open this post in threaded view
|

Re: icp_odometry with LOAM crash

huibuh
Hi Mat
That's great news! To try out your FLOAM implementation, I removed flynneva/floam from my catkin package path and replaced it with wh200720041/floam. I then applied floam_as_library_support.patch and it compiled OK.

But when I pulled the latest rtabmap master branch and ran cmake and make, I get this error during linking:
[ 80%] Linking CXX executable ../../../bin/rtabmap-reprocess
../../../bin/librtabmap_core.so.0.20.14: undefined reference to `ceres::Problem::SetParameterBlockConstant(double const*)'
../../../bin/librtabmap_core.so.0.20.14: undefined reference to `ceres::Problem::AddResidualBlock(ceres::CostFunction*, ceres::LossFuncti
on*, double* const*, int)'
I suppose this might be an issue with the ceres_solver version? I am using ceres_solver tag 2.0.0, as advised in the floam installation instructions. I also tried the latest ceres_solver master branch, but also get errors when compiling rtabmap. Which version of rtabmap/rtabmap_ros and ceres_solver/floam did you use when you tested?

Regards,
Heiko

P.S.: When I set WITH_FLOAM=OFF, rtabmap compiles without issues.
Reply | Threaded
Open this post in threaded view
|

Re: icp_odometry with LOAM crash

matlabbe
Administrator
I am on ceres 1.14.0. I will upgrade to 2.0.0 and fix the compatibility issues.
Reply | Threaded
Open this post in threaded view
|

Re: icp_odometry with LOAM crash

matlabbe
Administrator
Hi,

I cannot build floam with ceres 2.0.0, I have similar errors than you but with floam:
/usr/bin/ld: /home/mathieu/catkin_ws/devel/lib/libfloam.so: undefined reference to `ceres::Problem::AddResidualBlock(ceres::CostFunction*, ceres::LossFunction*, double*)'
/usr/bin/ld: /home/mathieu/catkin_ws/devel/lib/libfloam.so: undefined reference to `ceres::Solver::Summary::Summary()'

This one is working for me:
git clone https://ceres-solver.googlesource.com/ceres-solver
cd ceres-solver
git checkout 1.14.0
mkdir build
cd build
cmake -DBUILD_SHARED_LIBS=ON -DBUILD_TESTING=OFF -DBUILD_EXAMPLES=OFF ..
make -j8
sudo make install

cheers,
Mathieu
Reply | Threaded
Open this post in threaded view
|

Re: icp_odometry with LOAM crash

huibuh
Hi,

I switched to ceres-solver 1.14.0 and was able to compile rtabmap with FLOAM enabled. However, when I switch from Odom/Strategy=0 to Odom/Strategy=11, the icp_odometry node does not produce any output (topic /icp_odometry/odom is published at correct rate, but all values are 0, same for the published tf transform). No errors are thrown either. Are there any other parameters I need to change?

Cheers,
Heiko
Reply | Threaded
Open this post in threaded view
|

Re: icp_odometry with LOAM crash

matlabbe
Administrator
Hi,

OdomLOAM/Sensor should be set correctly, default is set to 2 (64 rays) matching kitti dataset. With VLP16, it should be set to 0. Note that I just fixed an error when setting it to 0, make sure to update to latest rtabmap version.

For convenience, I updated test_velodyne.launch with floam option:
roslaunch rtabmap_ros test_velodyne.launch floam:=true
It is because when using icp_odometry with FLOAM, we should make also sure that Icp/VoxelSize is 0 (otherwise icp_odometry pre-voxel the scan before sending it to floam, which is causing issues).

cheers,
Mathieu
Reply | Threaded
Open this post in threaded view
|

Re: icp_odometry with LOAM crash

huibuh
Hi,
I have pulled the latest update from rtabmap master (commit 5f6561) and done some more testing.

Using KITTI bag and wh200720041/floam with original test launch files, to make things reproducible:
roslaunch floam floam_velodyne.launch
rosbag play --clock 2011_09_30_0018.bag
... works as expected, rostopic hz /odom is published at 10Hz.
roslaunch rtabmap_ros test_velodyne.launch use_sim_time:=true resolution:=0.4 floam:=true
rosbag play --clock 2011_09_30_0018.bag
... works very badly: the trajectory is completely off, CPU load very high, and rostopic hz /rtabmap/odom is at only 5Hz.

Not sure what I am doing wrong here.

Cheers.
Heiko
Reply | Threaded
Open this post in threaded view
|

Re: icp_odometry with LOAM crash

matlabbe
Administrator
Hi Heiko,

I updated the launch file to set parameters related to kitti bags (the launch is configured by default to test with a real hand-held VLP16). I added floam_sensor option to select 64 rings lidar (required for kitti bags).

To test with a kitti rosbag:
$ roslaunch rtabmap_ros test_velodyne.launch resolution:=0.4 floam:=true floam_sensor:=2 use_sim_time:=true queue_size:=100 loop_ratio:=0.2
$ rosbag play  --clock 2011_09_30_0027.bag

FLOAM should be able to process data under 100 ms, otherwise it drifts a lot. I added queue_size option too to make sure that all scans are processed even if there are some processing time spikes that went over 100 ms. On my laptop, with resolution = 0.4, it is able to process most scans under 100 ms, so it works. With loop_ratio:=0.2, the loop closure can be detected and corrected:



Without loop closure:


Closer, with and without loop closure:


Area of the loop closure (after optimization):


cheers,
Mathieu
Reply | Threaded
Open this post in threaded view
|

Re: icp_odometry with LOAM crash

huibuh
Hi Mat
First of all, thank you for your detailed post, very much appreciated. I was able to reproduce your results from the kitti dataset using the standalone test_velodyne.launch file. Next, I played back by own dataset from a Ouster OS0-64, which also worked like a treat. However, when I then tried to run rtabmap with floam using my own tf tree, icp_odometry with Odom/Strategy=11 did not work, but Odom/Strategy=0 did. Upon closer investigation I came to the conclusion that floam cannot handle an offset in z-direction of the lasers's frame_id ('velodyne' in test_velodyne.launch).

My tf tree roughly looks like this:
map->odom->base_footprint->base_link->os_lidar
where 'os_lidar' is the equivalent to 'velodyne' in test_velodyne.launch

If the z-offset between base_footprint and os_lidar is non-zero, floam produces incorrect output (offset between 0.1-0.7m) or just outputs zero values (offset >0.7m).

Any idea what causes this behavior?

Cheers,
Heiko
Reply | Threaded
Open this post in threaded view
|

Re: icp_odometry with LOAM crash

matlabbe
Administrator
Hi Heiko,

There was indeed a bug, I wrongly copied stuff from OdometryLOAM, the local transform was applied before FLOAM processing as it should be only after. This is fixed in this commit. The result should now be the same than using velodyne frame directly.

Note also that you may have to tune the hard-coded angle here for the ouster 64 in case the field of view is different than HDL-64E.

cheers,
Mathieu