This repo is essentially a fork of GRIL-CALIB. I found their work useful, but fairly difficult to analyze/debug/reuse because of how the code is structured. This repo is the result of shuffling it around a bit and creating an outer Python layer to make nice tables and plots to understand what is going on.
The contributions are
- An outer Python layer to be able to more easily run the code, save results, and plot tables. The original code had a play-the-bag and subscribe workflow, which makes it time-consuming and error-prone to replicate their results. We also run FAST-LIO on the resultant IMU-LiDAR calibration to see how the result changes compard to pure odometry.
- The cpp code is refactored a bit, so that it is able to be used as a library.
Future work
- Fuller replication of GRIL-Calib results on the other datasets used in their paper.
- Making the cpp code ROS-agnostic. The repository is currently tied to ROS 1. Ideally the cpp/ folder would not use ROS types at all and relegate the ROS1/ROS2 processing to the src/ executable folder.
To retain an editable development environment, the repositories are cloned and used in Docker through volumes. See the docker-compose file.
You will need the forks of
- Fast-LIO. For serial bag processing and saving outputs to a given directory. Link. @be8fc82
- Our lab's state estimation library, navlie. Link.
- as well as of our homegrown estimation evaluation toolbox, Link. @71506d0
If you want to do the Gazebo sanity checks with data you collect yourself, you will also need [gazebo_sim.] (https://github.com/vkorotkine/gazebo_sim) , the small gazebo sim repository we have to this effect.
A sanity-check dataset is provided in Gazebo. It is uploaded to a Google Drive Folder. If the link is broken,
simply use the attached URDF files/refer to the small gazebo_sim repository to collect your own.
Make sure to edit the docker-compose.yml to reflect where the repositories live on your machine.
I did take the liberty to hardcode some paths in the dataset.py file,
so the Docker environment is heavily recommended.
The datasets folder has to follow the structure
datasets
├── gazebo
│ ├── gazebo_full_calib.bag
├── M2DGR
│ ├── gate_01.bag
│ ├── gate_01.txt
│ ├── hall_04.bag
│ ├── hall_04.txt
cd docker && docker-compose up -d
Make sure to source livox drivers,
source /opt/ros/noetic/setup.bash
source /home/livox_ws/devel/setup.bash
We disable multithreading for serial bag processing. Issues can be caused by multithreading when you feed the bag into
the node in serial as-fast-as-possible mode.
Note: In ikd-tree, I modified the Multi_Thread_Rebuild_Point_Num parameter to be really high
to force single threaded behaviour.
RelWithDebInfo,
catkin config --profile reldebinfo --cmake-args \
-DCMAKE_BUILD_TYPE=RelWithDebInfo
Debug,
catkin config --profile debug \
--cmake-args -DCMAKE_BUILD_TYPE=Debug
And catkin build and source the workspace,
catkin build
source devel/setup.bashUse the sanity-check gazebo bag,
roslaunch gril_calib gril_calibration.launch serial_vs_subscribe:=serial rviz:=false config_path:=/home/catkin_ws/src/gril-calib/config/gazebo/full_calib.yaml # xyz="0.1 0.1 0.16" rpy="0 0 0.7854"roslaunch gril_calib gril_calibration.launch serial_vs_subscribe:=serial rviz:=false config_path:=/home/catkin_ws/src/gril-calib/config/m2dgr/velodyne_m2dgr_gate01.yamlshould output
[Calibration Result] Rotation matrix from LiDAR frame to IMU frame = -0.096029 0.174797 1.328488 deg
[Calibration Result] Translation vector from LiDAR frame to IMU frame = 0.209875 0.087234 0.180309 m
and
roslaunch gril_calib gril_calibration.launch serial_vs_subscribe:=serial rviz:=false config_path:=/home/catkin_ws/src/gril-calib/config/m2dgr/velodyne_m2dgr_hall04.yamlshould output
[Calibration Result] Rotation matrix from LiDAR frame to IMU frame = -0.209688 0.033951 -0.983070 deg
[Calibration Result] Translation vector from LiDAR frame to IMU frame = 0.262128 0.043000 0.175578 m
The proper spread should be quantified by the actual python analysis, but as a sanity check these should be the values you get without anything else in the mix.
This repo provides a Python library/script to get metrics, tables, and plots. First install local libraries,
pyenv activate py312venv
cd /home/estimation_evaluation_toolbox && pip install -e .
cd /home/catkin_ws/src/gril-calib/python/pygril && pip install -e .pyenv activate py312venv
cd /home/catkin_ws/src/gril-calib/
python python/tests/test_gril_calib.py --datasets gazebo_full_calib gate_01 hall_04which should yield, in the terminal, the following tables (reformatted here for the purposes of the README looking clean).
| Topic | Dataset | Method | APE Rot (Odom) [deg] | APE Pos (Odom) [m] | APE Rot (LIO) [deg] | APE Pos (LIO) [m] | APE Rot (LIO True) [deg] | APE Pos (LIO True) [m] |
|---|---|---|---|---|---|---|---|---|
/velodyne_points |
gazebo_full_calib |
loam | 3.14 | 0.0344 | 3.78 | 0.393 | 1.68 | 0.364 |
/velodyne_points |
gate_01 |
loam | 5.12 | 0.104 | 3.05 | 0.308 | 3.23 | 0.252 |
/velodyne_points |
hall_04 |
loam | 180 | 1.02 | 177 | 1.22 | 177 | 1.26 |
| Topic | Dataset | Method | True Euler (rpy) [deg] | Estimated Euler (rpy) [deg] | True rb→lb [m] | Estimated rb→lb [m] | Δξ (Lie) [deg] |
|---|---|---|---|---|---|---|---|
/velodyne_points |
gazebo_full_calib |
loam | [-0, 0, 45] | [-0.049, -0.024, 41.0] | [0.1, 0.1, 0.16] | [0.029, 0.119, 0.208] | [-0.052, 0.016, -4.03] |
/velodyne_points |
gate_01 |
loam | [-0, 0, 0] | [-0.100, 0.173, 1.33] | [0.273, -0.00053, 0.18] | [0.21, 0.0872, 0.18] | [-0.098, 0.174, 1.33] |
/velodyne_points |
hall_04 |
loam | [-0, 0, 0] | [-0.209, 0.0375, -0.983] | [0.273, -0.00053, 0.18] | [0.262, 0.043, 0.176] | [-0.209, 0.0358, -0.983] |
Similar to LI-Init, edit config/xxx.yaml to set the below parameters:
lid_topic: Topic name of LiDAR point cloud.imu_topic: Topic name of IMU measurements.imu_sensor_height: Height from ground to IMU sensor (meter)data_accum_length: A threshold to assess if the data is enough for calibration.x_accumulate: Parameter that determines how much the x-axis rotates (Assuming the x-axis is front)y_accumulate: Parameter that determines how much the y-axis rotates (Assuming the y-axis is left)z_accumulate: Parameter that determines how much the z-axis rotates (Assuming the z-axis is up)gyro_factor,acc_factor,ground_factor: Weight for each residualset_boundary: When performing nonlinear optimization, set the bound based on the initial value. (only translation vector)bound_th: Set the threshold for the bound. (meter) ⭐️ See the ceres-solver documentation for more information.
This repo is essentially a fork of GRIL-CALIB, where the code is a bit cleaned up and with analysis tools to understand the performance. The original acknowledgements are below as well.
Thanks to hku-mars/LiDAR_IMU_Init for sharing their awesome work!
We also thanks to url-kaist/patchwork-plusplus-ros for sharing LiDAR ground segmentation algorithm.
If you find this repository useful, please cite the original paper,
@ARTICLE{10506583,
author={Kim, TaeYoung and Pak, Gyuhyeon and Kim, Euntai},
journal={IEEE Robotics and Automation Letters},
title={GRIL-Calib: Targetless Ground Robot IMU-LiDAR Extrinsic Calibration Method Using Ground Plane Motion Constraints},
year={2024},
volume={9},
number={6},
pages={5409-5416},
keywords={Calibration;Laser radar;Robot sensing systems;Robots;Optimization;Odometry;Vectors;Calibration and identification;sensor fusion},
doi={10.1109/LRA.2024.3392081}}