Skip to content

LeGO-LOAM, LIO-SAM, LVI-SAM, FAST-LIO2, Faster-LIO, VoxelMap, R3LIVE, Point-LIO, KISS-ICP, DLO, DLIO, Ada-LIO, PV-LIO, SLAMesh, ImMesh, FAST-LIO-MULTI, M-LOAM, LOCUS, SLICT, MA-LIO, CT-ICP, GenZ-ICP, iG-LIO, SR-LIO application and comparison on Gazebo and real-world datasets. Installation and config files are provided.

License

Notifications You must be signed in to change notification settings

engcang/SLAM-application

Folders and files

NameName
Last commit message
Last commit date

Latest commit

412e162 · Jan 31, 2025

History

71 Commits
Jan 13, 2025
Jan 31, 2025
Jan 13, 2025
Jan 8, 2025
Jan 13, 2025
Apr 1, 2022
Jan 13, 2025
Jan 13, 2025
Jan 13, 2025
May 6, 2021
May 6, 2021
Sep 12, 2024
May 11, 2021
Sep 12, 2024
Sep 11, 2024
Jul 13, 2022
Jan 5, 2025
Jan 8, 2025
Jul 13, 2022
Sep 13, 2022
Jan 5, 2025
May 6, 2021

Repository files navigation

SLAM-application: installation and test


Results:

● Single-LiDAR

● Multi-LiDARs

  • video14: FAST-LIO-MULTI bundle update vs asynchronous update



Dependencies

  • Common packages

    $ sudo apt-get install -y ros-noetic-navigation ros-noetic-robot-localization ros-noetic-robot-state-publisher
  • [CMake] version should be more recent than 3.20.5 for CT-ICP

    $ wget https://github.com/Kitware/CMake/archive/refs/tags/v3.31.3.tar.gz
    $ tar zxf v3.31.3.tar.gz
    $ cd CMake-3.31.3
    $ ./bootstrap
    $ make
    $ sudo make install
  • GTSAM for LVI-SAM and LIO-SAM

    $ wget -O gtsam.zip https://github.com/borglab/gtsam/archive/4.0.2.zip
    $ unzip gtsam.zip
    $ cd gtsam-4.0.2/
    $ mkdir build && cd build
    $ cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON ..
    $ sudo make install -j8
  • Ceres solver for LVI-SAM, SLAMesh, and SLICT1.0

    $ sudo apt-get install -y cmake libgoogle-glog-dev libatlas-base-dev libsuitesparse-dev
    $ wget http://ceres-solver.org/ceres-solver-1.14.0.tar.gz #LVI-SAM
    $ wget http://ceres-solver.org/ceres-solver-2.1.0.tar.gz #SLAMesh, SLICT1.0
    $ tar zxf ceres-solver-1.14.0.tar.gz #LVI-SAM
    $ tar zxf ceres-solver-2.1.0.tar.gz #SLAMesh, SLICT1.0
    $ mkdir ceres-bin
    $ mkdir solver && cd ceres-bin
    $ cmake ../ceres-solver-1.14.0 -DEXPORT_BUILD_DIR=ON -DCMAKE_INSTALL_PREFIX="../solver"  #good for build without being root privileged and at wanted directory
    $ make -j8 # 8 : number of cores
    $ make test
    $ make install
  • CGAL and pcl-tools for R3LIVE

    $ sudo apt install libcgal-dev pcl-tools
    
    Optionally,
    $ sudo apt install meshlab
  • glog, g++-9 and gcc-9 for Faster-LIO

    $ sudo apt-get install libgoogle-glog-dev
    $ sudo add-apt-repository ppa:ubuntu-toolchain-r/test
    $ sudo apt update
    $ sudo apt install gcc-9 g++-9
    $ sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-9 60 --slave /usr/bin/g++ g++ /usr/bin/g++-9

🌟🌟🌟 Note: Ouster-ros package cannot be built with gcc and g++ with the version higher than 6,

  • When building ouster-ros,
    catkin b -DCMAKE_C_COMPILER=gcc-6 -DCMAKE_CXX_COMPILER=g++-6 -DCMAKE_BUILD_TYPE=Release



Installation (single LiDAR without IMU)

● LeGO-LOAM

$ cd ~/your_workspace/src
$ git clone https://github.com/RobustFieldAutonomyLab/LeGO-LOAM.git
$ cd ..
$ catkin build -DCMAKE_BUILD_TYPE=Release

● DLO

$ sudo apt install libomp-dev libpcl-dev libeigen3-dev 
$ cd ~/your_workspace/src
$ git clone https://github.com/vectr-ucla/direct_lidar_odometry.git
$ cd ..
$ catkin build -DCMAKE_BUILD_TYPE=Release

● CT-ICP

  • [CMake] version should be more recent than 3.20.5 for CT-ICP
    $ wget https://github.com/Kitware/CMake/archive/refs/tags/v3.31.3.tar.gz
    $ tar zxf v3.31.3.tar.gz
    $ cd CMake-3.31.3
    $ ./bootstrap
    $ make
    $ sudo make install
  • Clone and build
    $ cd <OUTSIDE_OF_WORKSPACE_BUT_SOMEWHERE_ELSE>
    $ git clone https://github.com/jedeschaud/ct_icp
    $ cd ct_icp
    $ mkdir .cmake-build-superbuild
    $ cd .cmake-build-superbuild
    $ cmake ../superbuild
    $ cmake --build . --config Release
    $ cd ..
    $ mkdir cmake-build-release 
    $ cd  cmake-build-release
    $ cmake .. -DCMAKE_BUILD_TYPE=Release
    $ cmake --build . --target install --config Release --parallel 12
    
    $ cd ..
    $ cd ros/roscore
    $ mkdir cmake-build-release && cd  cmake-build-release
    $ cmake .. -DCMAKE_BUILD_TYPE=Release
    $ cmake --build . --target install --config Release --parallel 12
    
    $ cd <path-to-your-catkin-workspace>/src
    $ ln -s <path-to-ct_icp-git-project>/ros/catkin_ws/ct_icp_odometry ct_icp_odometry
    $ ln -s <path-to-ct_icp-git-project>/ros/catkin_ws/slam_roscore slam_roscore
    $ cd ..
    $ catkin build -DSUPERBUILD_INSTALL_DIR=<path-to-superbuild-install-dir>
    >> This directory is <CT_ICP_GIT_CLONED_PATH>/ct_icp/install

● KISS-ICP (ROS1)

🌟🌟🌟 Note: KISS-ICP (ROS1) cannot be build with gcc and g++ with the version higher than 9

$ cd ~/your_workspace/src
$ git clone https://github.com/PRBonn/kiss-icp.git
$ git reset --hard 460c6f2
$ cd ..
$ catkin build -DCMAKE_BUILD_TYPE=Release -DCMAKE_C_COMPILER=gcc-9 -DCMAKE_CXX_COMPILER=g++-9

● GenZ-ICP

🌟🌟🌟 Note: GenZ-ICP cannot be build with gcc and g++ with the version higher than 9

$ cd ~/your_workspace/src
$ git clone https://github.com/cocel-postech/genz-icp
$ cd ..
$ catkin build -DCMAKE_BUILD_TYPE=Release -DCMAKE_C_COMPILER=gcc-9 -DCMAKE_CXX_COMPILER=g++-9



Installation (single LiDAR with IMU)

● LIO-SAM

$ cd ~/your_workspace/src
$ git clone https://github.com/TixiaoShan/LIO-SAM.git
$ cd ..
$ catkin build -DCMAKE_BUILD_TYPE=Release

● LVI-SAM

$ cd ~/your_workspace/src
$ git clone https://github.com/TixiaoShan/LVI-SAM.git
$ cd ..
$ catkin build -DCMAKE_BUILD_TYPE=Release

● Trouble shooting for LVI-SAM

  • for OpenCV 4.X, edit LVI-SAM/src/visual_odometry/visual_loop/ThirdParty/DVision/BRIEF.cpp:53
// cv::cvtColor(image, aux, CV_RGB2GRAY);
cv::cvtColor(image, aux, cv::COLOR_RGB2GRAY);

● FAST-LIO2

$ cd ~/your_workspace/src
$ git clone https://github.com/Livox-SDK/livox_ros_driver.git
$ cd ..
$ catkin build -DCMAKE_BUILD_TYPE=Release

$ cd ~/your_workspace/src
$ git clone --recursive https://github.com/hku-mars/FAST_LIO.git
$ cd ..
$ catkin build -DCMAKE_BUILD_TYPE=Release

● Faster-LIO

$ cd ~/your_workspace/src
$ git clone https://github.com/gaoxiang12/faster-lio.git

$ cd faster-lio/thirdparty
$ tar -xvf tbb2018_20170726oss_lin.tgz

$ cd ~/your_workspace
$ catkin build -DCUSTOM_TBB_DIR=$(pwd)/src/faster-lio/thirdparty/tbb2018_20170726oss -DCMAKE_BUILD_TYPE=Release

● Faster-LIO on ARM architecture (e.g., Jetson Xavier)

$ cd ~/your_workspace/src
$ git clone https://github.com/gaoxiang12/faster-lio.git

$ cd faster-lio/thirdparty
$ git clone https://github.com/syoyo/tbb-aarch64
$ cd tbb-aarch64
$ ./scripts/bootstrap-aarch64-linux.sh
$ cd build-aarch64
$ make && make install

$ gedit faster-lio/cmake/packages.cmake

Edit line 13 as:
    #set(TBB2018_LIBRARY_DIR "${CUSTOM_TBB_DIR}/lib/intel64/gcc4.7")
    set(TBB2018_LIBRARY_DIR "${CUSTOM_TBB_DIR}/lib")


$ cd ~/your_workspace
$ catkin build -DCUSTOM_TBB_DIR=$(pwd)/src/faster-lio/thirdparty/tbb-aarch64/dist -DCMAKE_BUILD_TYPE=Release

● VoxelMap

$ cd ~/your_workspace/src
$ git clone https://github.com/Livox-SDK/livox_ros_driver.git
$ cd ..
$ catkin build -DCMAKE_BUILD_TYPE=Release

$ git clone https://github.com/hku-mars/VoxelMap.git
$ cd ..
$ catkin build -DCMAKE_BUILD_TYPE=Release

● Trouble shooting for VoxelMap

  • /usr/include/lz4.h:196:57: error: conflicting declaration ‘typedef struct LZ4_stream_t LZ4_stream_t’ ...
    • You could meet this error in ROS-noetic. Fix as here
$ sudo mv /usr/include/flann/ext/lz4.h /usr/include/flann/ext/lz4.h.bak
$ sudo mv /usr/include/flann/ext/lz4hc.h /usr/include/flann/ext/lz4.h.bak

$ sudo ln -s /usr/include/lz4.h /usr/include/flann/ext/lz4.h
$ sudo ln -s /usr/include/lz4hc.h /usr/include/flann/ext/lz4hc.h

● R3LIVE

$ cd ~/your_workspace/src
$ git clone https://github.com/hku-mars/r3live.git
$ cd ..
$ catkin build -DCMAKE_BUILD_TYPE=Release

● Trouble shooting for R3LIVE

  • LiDAR incoming frame too old ...
    • Original Livox-ros-driver does not publish the data with ROS timestamp, but LiDAR time.
    • So, use modified livox-ros-driver here
    • If that does not solve the problem, edit lddc.cpp yourself, line 563:
    //livox_msg.header.stamp = ros::Time((timestamp - init_lidar_tim - packet_offset_time )  / 1e9 + init_ros_time);
      livox_msg.header.stamp = ros::Time::now();
      /**************** Modified for R2LIVE **********************/
      ros::Publisher *p_publisher = Lddc::GetCurrentPublisher(handle);
      if (kOutputToRos == output_type_)
      {
        p_publisher->publish(livox_msg);
      }

● How to properly set configuration for R3LIVE

  • Camera calibration - use Kalibr or camera_calibration
    • Kalibr - refer original repo
    • camera_calibration - use as here
  • Lidar-Camera calibration
    • Other spinning LiDARs are not supported yet (for RGB mapping), but try to use lidar_camera_calibration repo, if you want to.
    • For LiVOX LiDAR, use livox_camera_calib repo
      • Record a bag file of LiVOX LiDAR data and capture the image from RGB camera you use.
      • Convert a bag file into a PCD file with (change directories in the launch file):
      $ roslaunch livox_camera_calib bag_to_pcd.launch
      • Then, calibrate LiDAR and camera as (change directories in the launch and config files):
      $ roslaunch livox_camera_calib calib.launch

★Note: extrinsic rotational parameter from livox_camera_calib should be transposed in the r3live_config.yaml file. Refer my extrinsic result and r3live config file


Left: Target image. Right: Target PCD


Left: calibrated image and residuals. Right: calibrated image


Sensor configuration of mine: Pixhawk4 mini as an IMU, FLIR Blackfly S USB3 (BFS-U3-23S3C-C), LiVOX MID-70


● Point-LIO

$ cd ~/your_workspace/src
$ git clone https://github.com/Livox-SDK/livox_ros_driver.git
$ cd ..
$ catkin build -DCMAKE_BUILD_TYPE=Release

$ cd ~/your_workspace/src
$ git clone --recursive https://github.com/hku-mars/Point-LIO.git
$ cd ..
$ catkin build -DCMAKE_BUILD_TYPE=Release

● DLIO

$ sudo apt install libomp-dev libpcl-dev libeigen3-dev 
$ cd ~/your_workspace/src
$ git clone https://github.com/vectr-ucla/direct_lidar_inertial_odometry.git
$ cd ..
$ catkin build -DCMAKE_BUILD_TYPE=Release

● PV-LIO

$ cd ~/your_workspace/src
$ git clone https://github.com/Livox-SDK/livox_ros_driver
$ git clone https://github.com/HViktorTsoi/PV-LIO
$ cd ..
$ catkin build -DCMAKE_BUILD_TYPE=Release

● SLAMesh

$ sudo apt-get install build-essential cmake cmake-curses-gui libflann-dev libgsl-dev libeigen3-dev libopenmpi-dev \
     openmpi-bin opencl-c-headers ocl-icd-opencl-dev libboost-all-dev libopencv-dev libyaml-cpp-dev \
     freeglut3-dev libhdf5-dev qtbase5-dev qt5-default libqt5opengl5-dev liblz4-dev

# Ubuntu 18.04
$ sudo apt-get install libvtk6-dev libvtk6-qt-dev
# Ubuntu 20.04
$ sudo apt-get install libvtk7-dev libvtk7-qt-dev

$ git clone https://github.com/uos/lvr2.git
$ cd lvr2 
$ mkdir build && cd build
$ cmake .. && make
$ sudo make install

$ cd ~/your_workspace/src
$ git clone https://github.com/naturerobots/mesh_tools.git
$ git clone https://github.com/RuanJY/SLAMesh.git
$ cd..
$ catkin build -DCMAKE_BUILD_TYPE=Release

● Trouble shooting for SLAMesh

  • With Ceres >= 2.0 version and OpenCV 4.X version, ceres.solve can occur error
  • Delete OpenCV dependency in CMakeLists.txt
#target_link_libraries(slamesh ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS} ${G2O_LIBS} ${CERES_LIBRARIES} ${LVR2_LIBRARIES})
target_link_libraries(slamesh ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${G2O_LIBS} ${CERES_LIBRARIES} ${LVR2_LIBRARIES})

● ImMesh

$ sudo apt-get install ros-noetic-cv-bridge ros-noetic-tf ros-noetic-message-filters ros-noetic-image-transport*
$ sudo apt-get install -y libcgal-dev pcl-tools libgl-dev libglm-dev libglfw3-dev libglew-dev libglw1-mesa-dev libxkbcommon-x11-dev
$ cd ~/your_workspace/src
$ git clone https://github.com/Livox-SDK/livox_ros_driver
$ git clone https://github.com/hku-mars/ImMesh
$ cd ..
$ catkin build -DCMAKE_BUILD_TYPE=Release

● Trouble shooting for ImMesh

  • Use Ubuntu >= 20.04. Otherwise, CGAL version issue will bother you.

● iG-LIO

$ cd ~/your_workspace/src
$ git clone https://github.com/zijiechenrobotics/ig_lio
$ cd ..
$ catkin build -DCMAKE_BUILD_TYPE=Release

● SR-LIO

$ cd ~/your_workspace/src
$ git clone https://github.com/ZikangYuan/sr_lio/
$ cd ..
$ catkin build -DCMAKE_BUILD_TYPE=Release




Installation (Multi-LiDARs)

● FAST-LIO-MULTI

$ cd ~/your_workspace/src
$ git clone https://github.com/Livox-SDK/livox_ros_driver
$ git clone https://github.com/engcang/FAST_LIO_MULTI --recursive
$ cd ..
$ catkin build -DCMAKE_BUILD_TYPE=Release

● SLICT1.0

  • It need Ceres >= 2.1.0
sudo apt install libsuitesparse-dev libtbb-dev
sudo apt install ros-noetic-tf2-sensor-msgs ros-noetic-tf-conversions
sudo apt install libatlas-base-dev libeigen3-dev libgoogle-glog-dev
sudo apt install python3-wstool python3-catkin-tools python3-osrf-pycommon

cd workspace_1
git clone https://github.com/Livox-SDK/Livox-SDK
cd Livox-SDK/build
cmake .. && sudo make install -j12

cd workspace_1
git clone https://github.com/Livox-SDK/Livox-SDK2
cd Livox-SDK2 && mkdir build && cd build
cmake .. && sudo make install -j12

cd workspace_1/src
git clone https://github.com/livox-SDK/livox_ros_driver
git clone https://github.com/livox-SDK/livox_ros_driver2
cd livox_ros_driver2
./build.sh ROS1


>>> Make chained workspace, it will be good for your mental health
>>> because of livox_ros_driver2
cd workspace_2
source workspace_1/devel/setup.bash

cd workspace_2/src
git clone https://github.com/brytsknguyen/ufomap 
cd ufomap && git checkout devel_surfel

cd workspace_2/src
wget https://github.com/brytsknguyen/slict/archive/refs/tags/slict.1.0.tar.gz
tar -zxf slict.1.0.tar.gz
cd ..
catkin build

● MA-LIO

cd workspace/src
git clone https://github.com/minwoo0611/MA-LIO.git
cd..
catkin build

● Trouble shooting for MA-LIO

  • If you get PCL error, just comment the find_package in MA_LIO/CMakeLists.txt
#before
find_package(PCL 1.8 REQUIRED)
#after
#find_package(PCL 1.8 REQUIRED)
  • You might have to fix the laserMapping.cpp file's uncertainty calculation part
// before
...
// for (int i = 0; i < (int)kf.lidar_uncertainty[num].size() - 1; i++)
...
// for (int i = 0; i < (int)kf.lidar_uncertainty[num].size() - 1; i++)

// after
...
for (int i = 0; i < (int)kf.lidar_uncertainty[num].size(); i++)
...
for (int i = 0; i < (int)kf.lidar_uncertainty[num].size(); i++)

● LOCUS2.0

sudo apt-get install ros-noetic-catkin python3-catkin-tools
sudo apt-get install ros-noetic-pcl-ros
sudo apt install ros-noetic-tf2-sensor-msgs ros-noetic-tf2-geometry-msgs ros-noetic-eigen-conversions

cd workspace/src
git clone https://github.com/NeBula-Autonomy/LOCUS.git
git clone https://github.com/NeBula-Autonomy/common_nebula_slam.git
cd ..
catkin build -DCMAKE_BUILD_TYPE=Release

● M-LOAM - Thanks to Chanjoon

  • It need Ceres >= 2.1.0
sudo apt install libomp-dev
cd workspace/src
git clone https://github.com/gogojjh/M-LOAM.git
cd ..
catkin build -DCMAKE_BUILD_TYPE=Release

● Trouble shooting for M-LOAM

  • If you get pcl_conversion error, just comment them in src/rosXXX.cpp
// #include <pcl/ros/conversions.h>
  • If you get cv error in kittiHelper.cpp, fix them as:
// cv::Mat left_image = cv::imread(left_image_path.str(), CV_LOAD_IMAGE_GRAYSCALE);
// cv::Mat right_image = cv::imread(left_image_path.str(), CV_LOAD_IMAGE_GRAYSCALE);
cv::Mat left_image = cv::imread(left_image_path.str(), cv::IMREAD_GRAYSCALE);
cv::Mat right_image = cv::imread(left_image_path.str(), cv::IMREAD_GRAYSCALE);
  • You will get segmentation fault right after running the code, fix M-LOAM/estimator/src/imageSegmenter/image_segmenter.hpp
// before
//dist = sqrt(d1 * d1 + d2 * d2 - 2 * d1 * d2 * cos(alpha));
//alpha = iter->first == 0 ? segment_alphax_ : segment_alphay_;

// after (order matters)
alpha = iter->first == 0 ? segment_alphax_ : segment_alphay_;
dist = sqrt(d1 * d1 + d2 * d2 - 2 * d1 * d2 * cos(alpha));

// before
//cloud_scan[i].erase(cloud_scan[i].begin() + cloud_scan_order[index]);
//if (j % 5 == 0)
//    laser_cloud_outlier.push_back(cloud_matrix.points[index]);

// after
if (cloud_scan_order[index] >= 0 && cloud_scan_order[index] < cloud_scan[i].size())
{
    cloud_scan[i].erase(cloud_scan[i].begin() + cloud_scan_order[index]);
    if (j % 5 == 0)
        laser_cloud_outlier.push_back(cloud_matrix.points[index]);
}



How to run

● check each of config files and launch files in the folders of this repo

Trouble shooting for Gazebo Velodyne plugin

  • When using CPU ray, instead of GPU ray, height - width should be interchanged, I used this script file

About

LeGO-LOAM, LIO-SAM, LVI-SAM, FAST-LIO2, Faster-LIO, VoxelMap, R3LIVE, Point-LIO, KISS-ICP, DLO, DLIO, Ada-LIO, PV-LIO, SLAMesh, ImMesh, FAST-LIO-MULTI, M-LOAM, LOCUS, SLICT, MA-LIO, CT-ICP, GenZ-ICP, iG-LIO, SR-LIO application and comparison on Gazebo and real-world datasets. Installation and config files are provided.

Topics

Resources

License

Stars

Watchers

Forks

Languages