Convert Figma logo to code with AI

TixiaoShan logoLIO-SAM

LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

3,328
1,233
3,328
130
View on GitHub

Top Related Projects

HKUST-Aerial-Robotics's avatar

A-LOAM

2,089

Advanced implementation of LOAM

cartographer-project's avatar

cartographer

7,089

Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor configurations.

RobustFieldAutonomyLab's avatar

LeGO-LOAM

2,354

LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain

HKUST-Aerial-Robotics's avatar

VINS-Mono

4,926

A Robust and Versatile Monocular Visual-Inertial State Estimator

raulmur's avatar

ORB_SLAM2

9,286

Real-Time SLAM for Monocular, Stereo and RGB-D Cameras, with Loop Detection and Relocalization Capabilities

Quick Overview

LIO-SAM is an open-source SLAM (Simultaneous Localization and Mapping) solution that combines LiDAR inertial odometry with pose graph optimization. It's designed for real-time 3D mapping in complex environments, offering improved accuracy and efficiency over traditional methods.

Pros

  • High accuracy and robustness in various environments
  • Real-time performance suitable for autonomous navigation
  • Integrates LiDAR, IMU, and GPS data for improved localization
  • Open-source with active community support

Cons

  • Requires specific hardware setup (LiDAR, IMU, GPS)
  • May have higher computational requirements compared to simpler SLAM methods
  • Learning curve for setup and parameter tuning
  • Limited documentation for advanced customization

Code Examples

  1. Subscribing to sensor topics:
ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, imuHandler, ros::TransportHints().tcpNoDelay());
ros::Subscriber subOdom = nh.subscribe<nav_msgs::Odometry>(odomTopic, 2000, odometryHandler, ros::TransportHints().tcpNoDelay());
ros::Subscriber subCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 5, cloudHandler, ros::TransportHints().tcpNoDelay());
  1. Performing scan-to-map optimization:
void performScanContextMatching(const pcl::PointCloud<PointType>::Ptr& cloudKeyFrames3D,
                                const pcl::PointCloud<PointType>::Ptr& cloudKeyFrames6D,
                                const pcl::PointCloud<PointType>::Ptr& cloudInputDS,
                                Eigen::Matrix4f& transformTobeMapped)
{
    // Scan context matching implementation
    // ...
}
  1. Saving map data:
void saveMapService(const std::string & saveMapDirectory)
{
    // Save map as pcd files
    savePCD(saveMapDirectory);

    // Save pose graph
    savePoseGraph();

    // Save path
    savePath(saveMapDirectory);
}

Getting Started

  1. Clone the repository:

    git clone https://github.com/TixiaoShan/LIO-SAM.git

  2. Build the project:

    cd LIO-SAM
catkin_make

  3. Run the launch file:

    roslaunch lio_sam run.launch

  4. Play your ROS bag file:

    rosbag play your_bag_file.bag

Ensure you have ROS, PCL, and GTSAM installed before building the project. Adjust sensor topics and parameters in the configuration files to match your setup.

Competitor Comparisons

HKUST-Aerial-Robotics logo

A-LOAM

2,089

Advanced implementation of LOAM

Pros of A-LOAM

  • Simpler implementation, making it easier to understand and modify
  • Faster processing time due to its lightweight nature
  • Better suited for systems with limited computational resources

Cons of A-LOAM

  • Less accurate in complex environments compared to LIO-SAM
  • Lacks advanced features like loop closure and multi-sensor fusion
  • May struggle with dynamic objects and rapid movements

Code Comparison

A-LOAM (scanRegistration.cpp):

void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
    if (!systemInited)
    {
        systemInitCount++;
        if (systemInitCount >= systemDelay)
        {
            systemInited = true;
        }
        return;
    }
    // ... (processing code)
}

LIO-SAM (imageProjection.cpp):

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
    if (!deskewInfo())
        return;
    projectPointCloud();
    cloudExtraction();
    publishClouds();
}

LIO-SAM offers a more structured and modular approach, with separate functions for different processing steps, while A-LOAM combines most of the processing within a single function. This reflects LIO-SAM's more advanced and feature-rich nature compared to A-LOAM's simpler implementation.

cartographer-project logo

cartographer

7,089

Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor configurations.

Pros of Cartographer

  • More mature and widely adopted in the robotics community
  • Supports a broader range of sensor configurations and platforms
  • Better documentation and community support

Cons of Cartographer

  • Higher computational requirements
  • More complex setup and configuration process
  • Less optimized for LiDAR-inertial systems compared to LIO-SAM

Code Comparison

LIO-SAM (C++):

pcl::PointCloud<PointType>::Ptr extractSurroundingKeyFrames()
{
    pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
    int numPoses = cloudKeyPoses3D->size();
    for (int i = numPoses-1; i >= 0; --i)
    {
        // ... (extraction logic)
    }
    return cloudOut;
}

Cartographer (C++):

std::unique_ptr<LocalTrajectoryBuilder3D>
CreateLocalTrajectoryBuilder3D(
    const proto::LocalTrajectoryBuilderOptions3D& options) {
  return absl::make_unique<LocalTrajectoryBuilder3D>(
      options.min_range(), options.max_range(),
      options.num_accumulated_range_data(),
      options.voxel_filter_size(),
      options.high_resolution_adaptive_voxel_filter(),
      options.low_resolution_adaptive_voxel_filter(),
      options.use_online_correlative_scan_matching(),
      options.real_time_correlative_scan_matcher_options(),
      options.ceres_scan_matcher_options(),
      options.motion_filter(),
      options.imu_gravity_time_constant(),
      options.rotational_histogram_size());
}
RobustFieldAutonomyLab logo

LeGO-LOAM

2,354

LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain

Pros of LeGO-LOAM

  • Simpler implementation, making it easier to understand and modify
  • Lower computational requirements, suitable for less powerful hardware
  • More established and widely used in the robotics community

Cons of LeGO-LOAM

  • Less accurate in challenging environments with rapid motion
  • Lacks IMU integration, potentially reducing robustness in certain scenarios
  • May struggle with loop closure in large-scale environments

Code Comparison

LeGO-LOAM:

void featureAssociation::extractFeatures(const pcl::PointCloud<PointType>::Ptr& cloud_in,
                                         std::vector<int>& cloudSmoothness,
                                         std::vector<int>& cloudNeighborPicked,
                                         std::vector<int>& cloudLabel)
{
    // Feature extraction logic
}

LIO-SAM:

void FeatureExtraction::extractFeatures(const pcl::PointCloud<PointType>::Ptr& cloud_in,
                                        const pcl::PointCloud<PointType>::Ptr& cloud_out,
                                        std::vector<int>& cloudLabel)
{
    // Enhanced feature extraction with IMU integration
}

The code comparison shows that LIO-SAM's feature extraction includes IMU integration, potentially improving accuracy and robustness. LeGO-LOAM's implementation is simpler but may be less effective in challenging scenarios.

HKUST-Aerial-Robotics logo

VINS-Mono

4,926

A Robust and Versatile Monocular Visual-Inertial State Estimator

Pros of VINS-Mono

  • Monocular visual-inertial SLAM system, suitable for environments without LiDAR
  • Robust to challenging scenarios like indoor/outdoor transitions and dynamic objects
  • Lightweight and computationally efficient, suitable for mobile devices

Cons of VINS-Mono

  • Less accurate in large-scale environments compared to LiDAR-based systems
  • May struggle in low-texture or low-light conditions
  • Requires careful camera-IMU calibration for optimal performance

Code Comparison

VINS-Mono (C++):

void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
{
    cv::Mat img;
    TicToc t_r;
    cur_time = _cur_time;

    if (EQUALIZE)
    {
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
        TicToc t_c;
        clahe->apply(_img, img);
        ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
    }
    else
        img = _img;

LIO-SAM (C++):

void featureExtraction(const sensor_msgs::PointCloud2::ConstPtr& laserCloudMsg)
{
    // 1. Convert ros message to pcl point cloud
    pcl::PointCloud<PointType>::Ptr laserCloudIn(new pcl::PointCloud<PointType>());
    pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);

    // 2. Downsample new cloud (save memory)
    pcl::PointCloud<PointType>::Ptr laserCloudInDS(new pcl::PointCloud<PointType>());
    static pcl::VoxelGrid<PointType> downSizeFilter;
    downSizeFilter.setLeafSize(odometrySurfLeafSize, odometrySurfLeafSize, odometrySurfLeafSize);
    downSizeFilter.setInputCloud(laserCloudIn);
    downSizeFilter.filter(*laserCloudInDS);
raulmur logo

ORB_SLAM2

9,286

Real-Time SLAM for Monocular, Stereo and RGB-D Cameras, with Loop Detection and Relocalization Capabilities

Pros of ORB_SLAM2

  • Purely vision-based SLAM system, requiring only a monocular, stereo, or RGB-D camera
  • Robust loop closing and relocalization capabilities
  • Lightweight and efficient, suitable for real-time applications on CPU

Cons of ORB_SLAM2

  • Limited to visual information, potentially less accurate in feature-poor environments
  • May struggle with dynamic scenes or rapid camera movements
  • Lacks integration with other sensor types (e.g., LiDAR, IMU)

Code Comparison

ORB_SLAM2 (C++):

// Feature extraction and matching
void Frame::ExtractORB(int flag, const cv::Mat &im)
{
    (*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors);
}

LIO-SAM (C++):

// LiDAR feature extraction
void featureExtraction(const pcl::PointCloud<PointType>::Ptr& cloud_in)
{
    pcl::PointCloud<PointType>::Ptr cornerPointsSharp(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr cornerPointsLessSharp(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr surfPointsFlat(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr surfPointsLessFlat(new pcl::PointCloud<PointType>());
}

The code snippets highlight the different approaches to feature extraction. ORB_SLAM2 focuses on visual features using ORB descriptors, while LIO-SAM extracts LiDAR features categorized into corner and surface points.

Convert Figma logo designs to code with AI

Visual Copilot

Introducing Visual Copilot: A new AI model to turn Figma designs to high quality code using your components.

Try Visual Copilot

README

LIO-SAM

A real-time lidar-inertial odometry package. We strongly recommend the users read this document thoroughly and test the package with the provided dataset first. A video of the demonstration of the method can be found on YouTube.

drawing

drawing drawing drawing drawing

Menu

System architecture

drawing

We design a system that maintains two graphs and runs up to 10x faster than real-time.

  • The factor graph in "mapOptimization.cpp" optimizes lidar odometry factor and GPS factor. This factor graph is maintained consistently throughout the whole test.
  • The factor graph in "imuPreintegration.cpp" optimizes IMU and lidar odometry factor and estimates IMU bias. This factor graph is reset periodically and guarantees real-time odometry estimation at IMU frequency.

Dependency

This is the original ROS1 implementation of LIO-SAM. For a ROS2 implementation see branch ros2.

  • ROS (tested with Kinetic and Melodic. Refer to #206 for Noetic) 
    sudo apt-get install -y ros-kinetic-navigation
sudo apt-get install -y ros-kinetic-robot-localization
sudo apt-get install -y ros-kinetic-robot-state-publisher
  • gtsam (Georgia Tech Smoothing and Mapping library) 
    sudo add-apt-repository ppa:borglab/gtsam-release-4.0
sudo apt install libgtsam-dev libgtsam-unstable-dev

Install

Use the following commands to download and compile the package.

cd ~/catkin_ws/src
git clone https://github.com/TixiaoShan/LIO-SAM.git
cd ..
catkin_make

Using Docker

Build image (based on ROS1 Kinetic):

docker build -t liosam-kinetic-xenial .

Once you have the image, start a container as follows:

docker run --init -it -d \
  -v /etc/localtime:/etc/localtime:ro \
  -v /etc/timezone:/etc/timezone:ro \
  -v /tmp/.X11-unix:/tmp/.X11-unix \
  -e DISPLAY=$DISPLAY \
  liosam-kinetic-xenial \
  bash

Prepare lidar data

The user needs to prepare the point cloud data in the correct format for cloud deskewing, which is mainly done in "imageProjection.cpp". The two requirements are:

  • Provide point time stamp. LIO-SAM uses IMU data to perform point cloud deskew. Thus, the relative point time in a scan needs to be known. The up-to-date Velodyne ROS driver should output this information directly. Here, we assume the point time channel is called "time." The definition of the point type is located at the top of the "imageProjection.cpp." "deskewPoint()" function utilizes this relative time to obtain the transformation of this point relative to the beginning of the scan. When the lidar rotates at 10Hz, the timestamp of a point should vary between 0 and 0.1 seconds. If you are using other lidar sensors, you may need to change the name of this time channel and make sure that it is the relative time in a scan.
  • Provide point ring number. LIO-SAM uses this information to organize the point correctly in a matrix. The ring number indicates which channel of the sensor that this point belongs to. The definition of the point type is located at the top of "imageProjection.cpp." The up-to-date Velodyne ROS driver should output this information directly. Again, if you are using other lidar sensors, you may need to rename this information. Note that only mechanical lidars are supported by the package currently.

Prepare IMU data

  • IMU requirement. Like the original LOAM implementation, LIO-SAM only works with a 9-axis IMU, which gives roll, pitch, and yaw estimation. The roll and pitch estimation is mainly used to initialize the system at the correct attitude. The yaw estimation initializes the system at the right heading when using GPS data. Theoretically, an initialization procedure like VINS-Mono will enable LIO-SAM to work with a 6-axis IMU. (New: liorf has added support for 6-axis IMU.) The performance of the system largely depends on the quality of the IMU measurements. The higher the IMU data rate, the better the system accuracy. We use Microstrain 3DM-GX5-25, which outputs data at 500Hz. We recommend using an IMU that gives at least a 200Hz output rate. Note that the internal IMU of Ouster lidar is an 6-axis IMU.

  • IMU alignment. LIO-SAM transforms IMU raw data from the IMU frame to the Lidar frame, which follows the ROS REP-105 convention (x - forward, y - left, z - upward). To make the system function properly, the correct extrinsic transformation needs to be provided in "params.yaml" file. The reason why there are two extrinsics is that my IMU (Microstrain 3DM-GX5-25) acceleration and attitude have different cooridinates. Depend on your IMU manufacturer, the two extrinsics for your IMU may or may not be the same. Using our setup as an example:

    • we need to set the readings of x-z acceleration and gyro negative to transform the IMU data in the lidar frame, which is indicated by "extrinsicRot" in "params.yaml."
    • The transformation of attitude readings might be slightly different. IMU's attitude measurement q_wb usually means the rotation of points in the IMU coordinate system to the world coordinate system (e.g. ENU). However, the algorithm requires q_wl, the rotation from lidar to world. So we need a rotation from lidar to IMU q_bl, where q_wl = q_wb * q_bl. For convenience, the user only needs to provide q_lb as "extrinsicRPY" in "params.yaml" (same as the "extrinsicRot" if acceleration and attitude have the same coordinate).

  • IMU debug. It's strongly recommended that the user uncomment the debug lines in "imuHandler()" of "imageProjection.cpp" and test the output of the transformed IMU data. The user can rotate the sensor suite to check whether the readings correspond to the sensor's movement. A YouTube video that shows the corrected IMU data can be found here (link to YouTube).

drawing

drawing

Sample datasets

  • Download some sample datasets to test the functionality of the package. The datasets below are configured to run using the default settings:

  • The datasets below need the parameters to be configured. In these datasets, the point cloud topic is "points_raw." The IMU topic is "imu_correct," which gives the IMU data in ROS REP105 standard. Because no IMU transformation is needed for this dataset, the following configurations need to be changed to run this dataset successfully:

    • The "imuTopic" parameter in "config/params.yaml" needs to be set to "imu_correct".
    • The "extrinsicRot" and "extrinsicRPY" in "config/params.yaml" needs to be set as identity matrices.

  • Ouster (OS1-128) dataset. No extrinsics need to be changed for this dataset if you are using the default settings. Please follow the Ouster notes below to configure the package to run with Ouster data. A video of the dataset can be found on YouTube:

  • Livox Horizon dataset. Please refer to the following notes section for paramater changes.

  • KITTI dataset. The extrinsics can be found in the Notes KITTI section below. To generate more bags using other KITTI raw data, you can use the python script provided in "config/doc/kitti2bag".

Run the package

  1. Run the launch file:
roslaunch lio_sam run.launch
  1. Play existing bag files:
rosbag play your-bag.bag -r 3

Other notes

  • Loop closure: The loop function here gives an example of proof of concept. It is directly adapted from LeGO-LOAM loop closure. For more advanced loop closure implementation, please refer to ScanContext. Set the "loopClosureEnableFlag" in "params.yaml" to "true" to test the loop closure function. In Rviz, uncheck "Map (cloud)" and check "Map (global)". This is because the visualized map - "Map (cloud)" - is simply a stack of point clouds in Rviz. Their postion will not be updated after pose correction. The loop closure function here is simply adapted from LeGO-LOAM, which is an ICP-based method. Because ICP runs pretty slow, it is suggested that the playback speed is set to be "-r 1". You can try the Garden dataset for testing.

drawing drawing

  • Using GPS: The park dataset is provided for testing LIO-SAM with GPS data. This dataset is gathered by Yewei Huang. To enable the GPS function, change "gpsTopic" in "params.yaml" to "odometry/gps". In Rviz, uncheck "Map (cloud)" and check "Map (global)". Also check "Odom GPS", which visualizes the GPS odometry. "gpsCovThreshold" can be adjusted to filter bad GPS readings. "poseCovThreshold" can be used to adjust the frequency of adding GPS factor to the graph. For example, you will notice the trajectory is constantly corrected by GPS whey you set "poseCovThreshold" to 1.0. Because of the heavy iSAM optimization, it's recommended that the playback speed is "-r 1".

drawing

  • KITTI: Since LIO-SAM needs a high-frequency IMU for function properly, we need to use KITTI raw data for testing. One problem remains unsolved is that the intrinsics of the IMU are unknown, which has a big impact on the accuracy of LIO-SAM. Download the provided sample data and make the following changes in "params.yaml":
    • extrinsicTrans: [-8.086759e-01, 3.195559e-01, -7.997231e-01]
    • extrinsicRot: [9.999976e-01, 7.553071e-04, -2.035826e-03, -7.854027e-04, 9.998898e-01, -1.482298e-02, 2.024406e-03, 1.482454e-02, 9.998881e-01]
    • extrinsicRPY: [9.999976e-01, 7.553071e-04, -2.035826e-03, -7.854027e-04, 9.998898e-01, -1.482298e-02, 2.024406e-03, 1.482454e-02, 9.998881e-01]
    • N_SCAN: 64
    • downsampleRate: 2 or 4
    • loopClosureEnableFlag: true or false

drawing drawing

  • Ouster lidar: To make LIO-SAM work with Ouster lidar, some preparations need to be done on hardware and software level.
    • Hardware:
      • Use an external IMU. LIO-SAM does not work with the internal 6-axis IMU of Ouster lidar. You need to attach a 9-axis IMU to the lidar and perform data-gathering.
      • Configure the driver. Change "timestamp_mode" in your Ouster launch file to "TIME_FROM_PTP_1588" so you can have ROS format timestamp for the point clouds.
    • Config:
      • Change "sensor" in "params.yaml" to "ouster".
      • Change "N_SCAN" and "Horizon_SCAN" in "params.yaml" according to your lidar, i.e., N_SCAN=128, Horizon_SCAN=1024.
    • Gen 1 and Gen 2 Ouster: It seems that the point coordinate definition might be different in different generations. Please refer to Issue #94 for debugging.

drawing drawing

  • Livox Horizon lidar: Please note that solid-state lidar hasn't been extensively tested with LIO-SAM yet. An external IMU is also used here rather than the internal one. The support for such lidars is based on minimal change of the codebase from mechanical lidars. A customized livox_ros_driver needs to be used to publish point cloud format that can be processed by LIO-SAM. Other SLAM solutions may offer better implementations. More studies and suggestions are welcome. Please change the following parameters to make LIO-SAM work with Livox Horizon lidar:
    • sensor: livox
    • N_SCAN: 6
    • Horizon_SCAN: 4000
    • edgeFeatureMinValidNum: 1
    • Use livox_ros_driver for data recording

drawing

Service

  • /lio_sam/save_map
    • save map as a PCD file. 
        rosservice call [service] [resolution] [destination]
      • Example:
        $ rosservice call /lio_sam/save_map 0.2 "/Downloads/LOAM/"

Issues

  • Zigzag or jerking behavior: if your lidar and IMU data formats are consistent with the requirement of LIO-SAM, this problem is likely caused by un-synced timestamp of lidar and IMU data.

  • Jumpping up and down: if you start testing your bag file and the base_link starts to jump up and down immediately, it is likely your IMU extrinsics are wrong. For example, the gravity acceleration has negative value.

  • mapOptimization crash: it is usually caused by GTSAM. Please install the GTSAM specified in the README.md. More similar issues can be found here.

  • gps odometry unavailable: it is generally caused due to unavailable transform between message frame_ids and robot frame_id (for example: transform should be available from "imu_frame_id" and "gps_frame_id" to "base_link" frame. Please read the Robot Localization documentation found here.

Paper

Thank you for citing LIO-SAM (IROS-2020) if you use any of this code.

@inproceedings{liosam2020shan,
  title={LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping},
  author={Shan, Tixiao and Englot, Brendan and Meyers, Drew and Wang, Wei and Ratti, Carlo and Rus Daniela},
  booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
  pages={5135-5142},
  year={2020},
  organization={IEEE}
}

Part of the code is adapted from LeGO-LOAM.

@inproceedings{legoloam2018shan,
  title={LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain},
  author={Shan, Tixiao and Englot, Brendan},
  booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
  pages={4758-4765},
  year={2018},
  organization={IEEE}
}

TODO

Related Package

Acknowledgement

  • LIO-SAM is based on LOAM (J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time).