MA-LIO (RA-L'23)
This repository (MA-LIO) contains the code for Asynchronous Multiple LiDAR-Inertial Odometry using Point-wise Inter-LiDAR Uncertainty Propagation, which is accepted by RA-L 2023. MA-LIO proposes an asynchronous multiple LiDAR-inertial odometry. To deal with the abovementioned challenges in multi-LiDAR, we model point-wise uncertainty by considering the range and state covariance at each time. Furthermore, we calculate localization weight using the surrounding environment to determine the weight term during optimization. In the video, ours corresponds to the same as MA-LIO.
Find out more about our project through our Video.
As there is a typo-error in our paper on (IEEE), we recommend to refer the paper on Arxiv or here.
Contributions
Our work makes the following contributions:
- We address the Field of View (FOV) discrepancy by accurately transferring points among LiDARs, an issue that scan matching approaches failed to resolve. Using continuous time B-spline interpolation, we reduce temporal discrepancy, resulting in consistent inter-LiDAR scan alignment despite significant FOV differences.
- The proposed point-level uncertainty captures the increased ambiguity induced by range and point acquisition time. This point-wise assessment assigns larger uncertainty for points farther in range or later in time, thereby managing uncertainty more generically.
- We proposed a localization weight that balances the ratio between prior and measurement residual during optimization. This enables the automatic adaptation of each residual's proportion in degenerate environments, such as tunnels and narrow corridors.
- We have validated our method on public and our own datasets. It is compatible with any combination of LiDARs with different scanning patterns from various manufacturers.
## Update
### 2023.11.07
We found some typo-error in our papers. We revised our paper, and users can find it on [Arxiv](https://arxiv.org/abs/2305.16792) or [here](https://minwoo0611.github.io/publications/ral2023_mwjung.pdf).
### 2023.06.25
We are delighted to release our code, MA-LIO. This is the initial release, and as such, there may be some errors or areas for improvement. **We hope it will be widely used and encourage feedback and suggestions through the issue tracker at any time**.
## 1. Prerequisites
The code is tested on:
* Linux 20.04 LTS
* ROS Noetic
### 1.1. Livox_ros_driver
To install livox_ros_driver, please follow the [Livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver) guide. Before building MA-LIO and file_player for City Datasets, ensure this driver is sourced.
## 2. Build Package
To download and compile the package, use the following commands:
```bash
cd ~/catkin_ws/src
git clone https://github.com/minwoo0611/MA-LIO.git
cd ..
catkin build
```
## 3. Launch MA-LIO
The initial codes are designed for the City Dataset with three LiDARs. To launch the MA-LIO package for a different number of LiDARs and datasets, users will need to modify five aspects of the code:
### 3.1. Create a user.yaml file
The .yaml file contains variables such as `common/lid_topic`, `common/lid_type`, `common/N_SCANS`, `common/point_filter_num`, `common/lid_num`, `mapping/extrinsic_T`, and `mapping/extrinsic_R`. User should adjust each variable to match own LiDAR specifications.
### 3.2. Sync_policies
Sync_policies from message_filters are needed to subscribe to multiple LiDAR topics. After defining the variables, initialize them with `LiDARMsgType`.
For example, the initial sync_policies look like this:
```cpp
typedef message_filters::sync_policies::ApproximateTime LidarSyncPolicy;
```
The City Dataset has three LiDARs: Ouster, Livox, and Livox, hence there are three message types: `LidarMsgType`, `LivoxMsgType`, and `LivoxMsgType`. These types should be arranged to match the .yaml file. If user is using two LiDARs, for example, Velodyne and Livox, the message filters can be modified to:
```cpp
typedef message_filters::sync_policies::ApproximateTime LidarSyncPolicy;
```
### 3.3. Synchronizer
Following the changes in sync_policies, the synchronizer must also be adjusted accordingly. Here's how the initial code looks:
```cpp
message_filters::Synchronizer *sync =
new message_filters::Synchronizer(
LidarSyncPolicy(10), *sub_spin[0], *sub_livox[0], *sub_livox[1]);
sync->registerCallback(boost::bind(&lidar_cbk_, _1, _2, _3));
```
If user is using two LiDARs, for instance, Velodyne and Livox, the code should be modified to:
```cpp
message_filters::Synchronizer *sync =
new message_filters::Synchronizer(
LidarSyncPolicy(10), *sub_spin[0], *sub_livox[0]);
sync->registerCallback(boost::bind(&lidar_cbk_, _1, _2));
```
### 3.4. lidar_cbk_ function
Users will need to adjust both the input and internals of the function to suit their requirements. Here is what the initial code looks like:
```cpp
void lidar_cbk_(const sensor_msgs::PointCloud2::ConstPtr &scanMsg_,
const livox_ros_driver::CustomMsg::ConstPtr &livoxMsg_, const livox_ros_driver::CustomMsg::ConstPtr &livoxMsg2_)
{
standard_pcl_cbk(scanMsg_, 0);
livox_pcl_cbk(livoxMsg_, 1);
livox_pcl_cbk(livoxMsg2_, 2);
}
```
For instance, user can change the input like this:
```cpp
void lidar_cbk_(const sensor_msgs::PointCloud2::ConstPtr &scanMsg_,
const livox_ros_driver::CustomMsg::ConstPtr &livoxMsg_)
```
Also, modify the internals of the function as follows:
```cpp
standard_pcl_cbk(scanMsg_, 0);
livox_pcl_cbk(livoxMsg_, 1);
```
Each inputs is correspondeing to `ros_msg` and the order of LiDAR.
### 3.5. use-ikfom
Modify the `MTK_bUILD_MANIFOLD` based on `lid_num`.
The initial manifold of the code looks like this due to the use of three lidars:
```cpp
MTK_BUILD_MANIFOLD(state_ikfom,
((vect3, pos))
((SO3, rot))
((SO3, offset_R_0))
((SO3, offset_R_1))
((SO3, offset_R_2))
((vect3, offset_T_0))
((vect3, offset_T_1))
((vect3, offset_T_2))
((vect3, vel))
((vect3, bg))
((vect3, ba))
((S2, grav))
);
```
However, if you're using two LiDARs, you can adjust it like this:
```cpp
MTK_BUILD_MANIFOLD(state_ikfom,
((vect3, pos))
((SO3, rot))
((SO3, offset_R_0))
((SO3, offset_R_1))
((vect3, offset_T_0))
((vect3, offset_T_1))
((vect3, vel))
((vect3, bg))
((vect3, ba))
((S2, grav))
);
```
After making the corresponding modifications, you can build and launch as follows:
```bash
roslaunch ma_lio mapping_user.launch
```
If the modification was successful, you can check the following RViz output:
All these steps are documented in the code. You can review the yaml file and commented code for using two LiDARs in the UrbanNav file. Following this guide will enable you to modify the code without difficulty as a SLAM engineer. Additionally, for comparison with a single LiDAR, there is a comment about running the code with a single LiDAR. You can find it easily searching the word "Especially". Also, [this issue](https://github.com/minwoo0611/MA-LIO/issues/3) helps you to run MA-LIO with single LiDAR.
## 4. Datasets
We utilize 3 datasets for the evaluation. The field of view of LiDARs in each dataset is exhibited as below.
