A probabilistic voxelmap-based LiDAR-Inertial Odometry.
PV-LIO is a probabilistic voxelmap-based LiDAR-Inertial Odometry. It fuses LiDAR feature points with IMU data using IKFoM to allow robust navigation in fast-motion or narrow environments where degeneration occurs. PV-LIO also supports online LiDAR-IMU extrinsic estimation.
We utilize VoxelMap as the Local Map manager of PV-LIO, it calculates the covariance of each <LiDAR point,planar feature>
correspondence according to the LiDAR ranging model and uses it as confidence ratio to guide the update of KF. This enables robust pose estimation in degenerated scenarios such as narrow staircases. We derive the covariance propagation incorporating the LiDAR-IMU extrinsic parameters, enabling state estimation with IMU and online LiDAR-IMU calibration. We also implement a parallel-optimized map update module, which allows for a more efficient map update than the original implementation of VoxelMap.
Left: Robosense RS16, staircase_crazy_rotation dataset
Right: Livox AVIA, long_tunnel dataset
Ubuntu >= 16.04
For Ubuntu 18.04 or higher, the default PCL and Eigen is enough for PV-LIO to work normally.
ROS >= Melodic. ROS Installation
PCL >= 1.8, Follow PCL Installation.
Eigen >= 3.3.4, Follow Eigen Installation.
Follow livox_ros_driver Installation.
Remarks:
source $Livox_ros_driver_dir$/devel/setup.bash
to the end of file ~/.bashrc
, where $Livox_ros_driver_dir$
is the directory of the livox ros driver workspace (should be the ws_livox
directory if you completely followed the livox official document).Clone the repository and catkin_make:
cd ~/$A_ROS_DIR$/src
git clone https://github.com/hviktortsoi/PV_LIO.git
cd PV_LIO
cd ../..
catkin_make
source devel/setup.bash
export PCL_ROOT={CUSTOM_PCL_PATH}
Noted:
A. Please make sure the IMU and LiDAR are Synchronized, that's important.
B. The warning message "Failed to find match for field 'time'." means the timestamps of each LiDAR points are missed in the rosbag file. That is important for the forward propagation and backwark propagation.
Connect to your PC to Livox Avia LiDAR by following Livox-ros-driver installation, then
cd ~/$PV_LIO_ROS_DIR$
source devel/setup.bash
roslaunch pv_lio mapping_avia.launch
roslaunch livox_ros_driver livox_lidar_msg.launch
livox_lidar_msg.launch
since only its livox_ros_driver/CustomMsg
data structure produces the timestamp of each LiDAR point which is very important for the motion undistortion. livox_lidar.launch
can not produce it right now.mapping_avia.launch theratically supports, mid-70, mid-40 or other livox serial LiDAR, but need to setup some parameters befor run:
Edit config/avia.yaml
to set the below parameters:
lid_topic
imu_topic
extrinsic_T
extrinsic_R
(only support rotation matrix)time_sync_en
to ture to turn on. But turn on ONLY IF external time synchronization is really not possible, since the software time sync cannot make sure accuracy.Step A: Setup before run
Edit config/velodyne.yaml
to set the below parameters:
lid_topic
imu_topic
(both internal and external, 6-aixes or 9-axies are fine)scan_line
extrinsic_T
extrinsic_R
(only support rotation matrix)Step B: Run below
cd ~/$PV_LIO_ROS_DIR$
source devel/setup.bash
roslaunch pv_lio mapping_velodyne.launch
Step C: Run LiDAR's ros driver or play rosbag.
Step A: Setup before run
Edit launch/mapping_robosense.launch
, find and modify the following line:
<remap from="/rslidar_points" to="/your_lidar_topic"/>
Fill /your_lidar_topic
with your actual LiDAR topic name.
Step B:
Edit config/robosense.yaml
to set the below parameters:
imu_topic
(both internal and external, 6-aixes or 9-axies are fine)scan_line
extrinsic_T
extrinsic_R
(only support rotation matrix)Step C: Run below
cd ~/$PV_LIO_ROS_DIR$
source devel/setup.bash
roslaunch pv_lio mapping_robosense.launch
Step C: Run LiDAR's ros driver or play rosbag.
Files: Can be downloaded from Baidu Pan (password:4kpf) or Google Drive
Run:
roslaunch pv_lio mapping_robosense.launch
cd YOUR_BAG_DOWNLOADED_PATH
rosbag play *.bag
Important: The 3 bags are from the same dataset sequence, so they should be play sequentially, rather than be played alone.
Thanks a lot for the authors of VoxelMap, IKFoM and FAST-LIO;
Thanks to Xiaokai for his help with deriving the covariance propagation.