LTAOM

LTA-OM: Long-Term Association LiDAR-Inertial Odometry and Mapping [JFR2024]

1. Introduction

LTA-OM is an efficient, robust, and accurate SLAM system. It integrates LIO, loop detection, loop optimization, LTA modules. It also supports multi-session mode.

1.1 Paper

Our paper is available on TODO.com/DOI.TODO .

1.2 Demo Video

Our demo video is available on https://youtu.be/DVwppEKlKps .

2. Tested Environment

2.0 Ubuntu

Our operation system is Ubuntu 18.

2.1 ROS

Following this ROS Installation to install ROS and its additional pacakge. Our ROS version is Melodic.

2.2 gtsam

Our gtsam version is gtsam-4.0.3. Note that: you need to add some functions to original gtsam code before compiling, please see the note at the end of this Readme.

2.3 ceres

Our ceres version is ceres-solver-1.14.0.

2.4 PCL

Our pcl version is pcl-1.9.

2.5 gcc/g++

Our gcc/g++ version is gcc/g++ -7.

2.6 Eigen3

2.7 TBB

Our tbb: Threading Building Blocks 2019 Update 9

Download it from github and build it in a folder. Link it using hard directory in STD Cmakelist.txt.

3. Build

Clone this repository and catkin_make:

cd ~/ws_LTAOM/src/
git clone https://github.com/hku-mars/LTAOM.git
cd ../
catkin_make
source ~/catkin_ws/devel/setup.bash

4. Run our examples

In : LiDAR & IMU messages (multisession mode: prior map, prior key poses, prior STD database) Out: cloud_result.pcd & optimized_poses.txt

Please make sure the output directory in the launch file is set. Otherwise, you may see that the program is not running.

4.1 Mulran (Ouster)

Download dataset from https://sites.google.com/view/mulran-pr/download. Run

cd ~/ws_LTAOM/src/LTAOM
./run_nodelet_ouster.sh
./run_loop_optimization.sh

// When LTAOM execution is done, run the following to save corrected map
rosparam set /save_map true

4.2 NCLT (Velodyne)

Download dataset from http://robots.engin.umich.edu/nclt/index.html#download. Run

cd ~/ws_LTAOM/src/LTAOM
./run_nodelet_velodyne.sh
./run_loop_optimization.sh

// When LTAOM execution is done, run the following to save corrected map
rosparam set /save_map true

4.3 Multilevel (Livox avia)

Download dataset from my one drive: TODO.com. Run

cd ~/ws_LTAOM/src/LTAOM
./run_nodelet_avia.sh
./run_loop_optimization.sh

// When LTAOM execution is done, run the following to save corrected map
rosparam set /save_map true

4.4 Multi-session mode

4.4.1 first session

First open loop_optimization/run_all_mulran_multisession.launch, and set multisession_mode = 2 in launch file to allow you save LTAOM result.

Run

cd ~/ws_LTAOM/src/LTAOM
./run_nodelet_ouster_multisession.sh
./run_loop_optimization.sh

When you have executed LTAOM with one bag, you can save the LTAOM result for future session:

rosparam set /save_prior_info true

4.4.1 second session

Then, set multisession_mode = 1 in launch file to allow you load previous LTAOM result. After that, you can run

cd ~/ws_LTAOM/src/LTAOM
./run_nodelet_mulran_multisession.sh
./run_loop_optimization.sh

// When LTAOM execution is done, run the following to save corrected map
rosparam set /save_map true

with a new bag as the second session.

4.5 Generate dense poses for map evaluation

Uncomment the line "//#define save_for_mapconsistency_eva" in laserMapping.cpp.

Execute LTAOM like step 4.1-4.3.

When loop optimization is done, you will get scanposes_corrected.txt. In addition, undistorted scans will be recorded as undistoted_scans.bag .

Note, when LTAOM execution is done, type the following command in the terminal to make sure that scanposes_corrected.txt is complete.

rosparam set /pub_pgopath true

Finally, follow FAST_LIO/map_eva/ReadMe.txt to do evaluation.

5. Example results

Nclt 20120202 map agianst statellite image

Comparison on Mulran Riverside02

Comparison on Mulran DCC02

Multilevel building (structurally-similar scene) map

Multisession map stitching reuslt

6. Report our problems and bugs

We know our packages might not completely stable at this stage, and we are working on improving the performance and reliability of our codes. So, if you have met any bug or problem, please feel free to draw an issue and I will respond ASAP.

7. Acknowledgments

In the development of LTAOM, we stand on the shoulders of the following repositories: FASTLIO2, ikdtree, STD, gtsam.

License

The source code is released under GPLv2 license.

If you use any code of this repo in your academic research, please cite at least one of our papers:

[1] Zou, Zuhao, et al. "LTA-OM: Long-Term Association LiDAR-Inertial
    Odometry and Mapping"
[2] Yuan, Chongjian, et al. "Std: Stable triangle descriptor for 3d place recognition"
[3] Xu, Wei, et al. "Fast-lio2: Fast direct lidar-inertial odometry."
[4] Xu, Wei, and Fu Zhang. "Fast-lio: A fast, robust lidar-inertial odometry
    package by tightly-coupled iterated kalman filter."
[5] Cai, Yixi, Wei Xu, and Fu Zhang. "ikd-Tree: An Incremental KD Tree for
    Robotic Applications."
[6] Lin, Jiarong, and Fu Zhang. "Loam-livox: A fast, robust, high-precision
    LiDAR odometry and mapping package for LiDARs of small FoV."

For commercial use, please contact me < zuhaozouATyahoo.com > and Dr. Fu Zhang < fuzhangAThku.hk >.

TODO

Loop optimization node cannot be correctly triggered as nodelet.
Problem when saving large output pcd in loop optimization node.

Note

// to the place between lines 97 ~ 99 of ISAM2.h

Values thetabkq;

VariableIndex variableIndexbkq;

mutable VectorValues deltabkq;

mutable VectorValues deltaNewtonbkq;

mutable VectorValues RgProdbkq;

mutable KeySet deltaReplacedMaskbkq;

NonlinearFactorGraph nonlinearFactorsbkq;

mutable GaussianFactorGraph linearFactorsbkq;

ISAM2Params paramsbkq;

mutable boost::optional<double> doglegDeltabkq;

KeySet fixedVariablesbkq;

int update_countbkq;

// to the place between lines 97 ~ 99 of ISAM2.h

void backup(); // to line 199 of ISAM2.h

void recover(); // to line 200 of ISAM2.h

// from line 395 of ISAM2.cpp

void ISAM2::backup(){

variableIndexbkq = variableIndex_;

thetabkq = theta_;

deltabkq = delta_;

deltaNewtonbkq = deltaNewton_;

RgProdbkq = RgProd_;

nonlinearFactorsbkq = nonlinearFactors_;

fixedVariablesbkq = fixedVariables_;

update_countbkq = updatecount;

deltaReplacedMaskbkq = deltaReplacedMask_;

linearFactorsbkq = linearFactors_;

doglegDeltabkq = doglegDelta_;

paramsbkq = params_;

// nodesbkq = nodes_;

// rootsbkq = roots_;

}

void ISAM2::recover(){

variableIndex_ = variableIndexbkq;

theta_ = thetabkq;

delta_ = deltabkq;

deltaNewton_ = deltaNewtonbkq;

RgProd_ = RgProdbkq;

nonlinearFactors_ = nonlinearFactorsbkq;

fixedVariables_ = fixedVariablesbkq;

updatecount = update_countbkq;

deltaReplacedMask_ = deltaReplacedMaskbkq;

linearFactors_ = linearFactorsbkq;

doglegDelta_ = doglegDeltabkq;

params_ = paramsbkq;

// nodes_ = nodesbkq;

// roots_ = rootsbkq;

}

// to line 427 of ISAM2.cpp

hku-mars / LTAOM

readme