/sr_lio

A LiDAR-inertial odometry (LIO) package that can adjust the execution frequency beyond the sweep frequency

Primary LanguageC++GNU General Public License v2.0GPL-2.0

SR-LIO

SR-LIO (LiDAR-Inertial Odometry with Sweep Reconstruction) is an accurate and robust bundle adjustment (BA) based LiDAR-inertial odometry (LIO) that can increase the execution frequency beyond the sweep frequency. It segments and reconstructs raw input sweeps from spinning LiDAR to obtain reconstructed sweeps with higher frequency. Such method can shorten the time period of IMU pre-integration, and thus reduce the error of IMU pre-integration. Based on the proposed sweep reconstruction method, we build our newly designed BA based LIO system and achieve the state-of-the-art accuracy.

Related Work

SR-LIO: LiDAR-Inertial Odometry with Sweep Reconstruction

Authors: Zikang Yuan, Fengtian Lang and Xin Yang

Demo Video (2022-10-17 Update)

The x15 Real-Time Performance on sequence nclt_2013-01-10 (left), and the Resulted Global Map and Trajectory on sequence nclt_2013-01-10 (right). It is important to emphasize that "x15" is the multiplier relative to the 10 Hz raw input LiDAR sweep, not relative to the processing frequency of our system. On our currently hardware platform (Intel Core i7-12700 and 32 GB RAM), SR-LIO cannot run in real-time after the raw input LiDAR sweeps are reconstructed from 10 Hz to 30 Hz.

Related video:: Real-Time Performance, Global Map and Trajectory

Pipeline:

New Features:

  1. The proposed Sweep Reconstruction module splits the original sweep packet into continuous point cloud data streams, and then re-packages point cloud data streams in a multiplexing way to obtain sweeps with higher frequency, which is illustrated by the figure as follow:
  1. Sweep Reconstruction can effectively reduce the time interval for each IMU pre-integration, reducing the IMU pre-integration error and enabling the usage of BA based LiDAR-inertial optimization.
  2. Following CT-ICP, SR-LIO represents the state of two moments in each sweep: 1) at the beginning time of a sweep, and 2) at the end time of the sweep.
  3. SR-LIO proposes Multi-Segment LIO Optimization for equally optimize all state variables during the period of a reconstructed sweep.
  4. All details about the Jacobian matrixes are available in the appendix of our article.

Installation

1. Requirements

GCC >= 5.4.0

Cmake >= 3.0.2

Eigen3 >= 3.2.8

PCL == 1.7 for Ubuntu 16.04, and == 1.8 for Ubuntu 18.04

Ceres >= 1.14

ROS

Have Tested On:
OS GCC Cmake Eigen3 PCL Ceres
Ubuntu 16.04 5.4.0 3.16.0 3.2.8 1.7 1.14
Ubuntu 18.04 7.5.0 3.11.2 3.3.4 1.8 1.14

2. Create ROS workspace

mkdir -p ~/SR-LIO/src
cd SR-LIO/src

3. Clone the directory and build

git clone https://github.com/ZikangYuan/sr_lio.git
cd ..
catkin_make

Run on Public Datasets

Noted:

A. Except fot the external parameters between IMU and LiDAR, and the value of gravitational acceleration, the parameter configurations used in different datasets are exactly the same to demonstrate the stability and robustness of the SR-LIO.

B. Please make sure the LiDAR point clouds have the "ring" channel information.

C. The warning message "Failed to find match for field 'time'." doesn't matter. It can be ignored.

D. Please create a folder named "output" before running. When SR-LIO is running, the estimated pose is recorded in real time in the pose.txt located in the output folder.

E. If you want to get some visualization of the split and recombine, please set the debug_output parameter in the launch file to 1 (true). After that, you can get some .pcd files in "output/cloud_frame" and "output/cut_sweep" folders.

F. As the groundtruth acquisition of some datasets (UTBM and ULHK) are extremely complicated, in order to facilitate evaluation, we store the pose ground truth of the three datasets used by us as TUM format. Please down load from Google drive.

1. Run on NCLT

The time for finishing a sweep by the LiDAR of NCLT is not 100ms, but 130~140ms (around 7 Hz). Therefore, we need to package the data stream of the NCLT dataset as 7 Hz sweep packages. The nclt_to_rosbag.py in the "tools" folder can be used to package 7 Hz sweeps and linearly interpolated 100 Hz IMU data into a rosbag file:

python3 nclt_to_rosbag.py PATH_OF_NVLT_SEQUENCE_FOLDER PATH_OF_OUTPUT_BAG

Then, please go to the workspace of SR-LIO and type:

cd SR-LIO
sourcr devel/setup.bash
roslaunch sr_lio lio_nclt.launch

Then open the terminal in the path of the bag file, and type:

rosbag play SEQUENCE_NAME.bag --clock -d 1.0 -r 0.2 

2. Run on UTBM

Before evaluating on UTBM dataset, a dependency needs to be installed. If your OS are Ubuntu 16.04, please type:

sudo apt-get install ros-kinetic-velodyne 

If your OS are Ubuntu 18.04, please type:

sudo apt-get install ros-melodic-velodyne 

Then open the terminal in the path of SR-LIO, and type:

sourcr devel/setup.bash
roslaunch sr_lio lio_utbm.launch

Then open the terminal in the path of the bag file, and type:

rosbag play SEQUENCE_NAME.bag --clock -d 1.0 -r 0.2 

3. Run on ULHK

For sequence HK-Data-2019-01-17 and HK-Data-2019-03-17, the imu data does not include the gravity acceleration component, and the topic of LiDAR point cloud data is /velodyne_points_0. For other sequences of ULHK used by us, the imu data includes the gravity acceleration component, and the topic of LiDAR point cloud data is /velodyne_points. Therefore, we provide two launch files for the ULHK dataset.

If you test SR-LIO on HK-Data-2019-01-17 or HK-Data-2019-03-17, please type:

sourcr devel/setup.bash
roslaunch sr_lio lio_ulhk1.launch

If you test SR-LIO on HK-Data-2019-03-16-1, HK-Data-2019-04-26-1 or HK-Data-2019-04-26-2, please type:

sourcr devel/setup.bash
roslaunch sr_lio lio_ulhk2.launch

Then open the terminal in the path of the bag file, and type:

rosbag play SEQUENCE_NAME.bag --clock -d 1.0 -r 0.2 

4. Adjustment of "-r" when play rosbag

-r is used to control the rosbag playback speed. For example, when we set -r 0.2, the playback speed of this operation is 1/5 of the original data acquisition rate. Theoretically, when the input LiDAR sweeps are reconstituted from 10 Hz to 30 Hz, we need to complete the processing of a sweep within (1000/30)ms. However, our system could not achieve such excellent computational efficiency on existing hardware platforms. By slowing down the playback of rosbag packets, we can give our system more time to process each sweep.

The most significant parameters affecting the efficiency of our system are the registration times of ICP and the iteration times of each registration. Therefore, for each sequence, we test the time consumption with different number of ICP point cloud registration and different number of iteration solutions for each registration. For each test, we also record the pose accuracy (i.e., ATE) to explore how many registration and iterations are need to reach the best pose accuracy. The results are arranged in the following table. Please refer to the Table VII of our article to select the "-r" parameter.

Citation

If you use our work in your research project, please consider citing:

@article{yuan2022sr,
  title={SR-LIO: LiDAR-Inertial Odometry with Sweep Reconstruction},
  author={Yuan, Zikang and Lang, Fengtian and Yang, Xin},
  journal={arXiv preprint arXiv:2210.10424},
  year={2022}
}

Acknowledgments

Thanks for CT-ICP, Fast-LIO, VINs-Mono and Open-VINs.