CamVox
A Low-cost and Accurate Lidar-assisted Visual SLAM System
We propose CamVox by adapting Livox lidars into visual SLAM (ORB-SLAM2) by exploring the lidars’ unique features. Based on the non-repeating nature of Livox lidars, we propose an automatic lidar-camera calibration method that will work in uncontrolled scenes. The long depth detection range also benefit a more efficient mapping. Comparison of CamVox with visual SLAM (VINS-mono) and lidar SLAM (livox_horizon_loam) are evaluated on the same dataset to demonstrate the performance.
Developer: Yuewen Zhu, Chunran Zheng, Chongjian Yuan
Our related video: our related videos are now available on [YouTube Video] [bilibili Video].
Paper: our related paper has been posted on arXiv.
1. Prerequisites
1.1 Ubuntu and ROS
Ubuntu 64-bit 16.04 or 18.04. ROS Kinetic or Melodic. Follow ROS Installation.
1.2 Pangolin
We use Pangolin for visualization and user interface. Follow Pangolin Installation.
1.3 OpenCV
We use OpenCV to manipulate images and features. Follow Opencv Installation. Required at leat 2.4.3. Tested with OpenCV 2.4.11 and OpenCV 3.2.
1.4 Eigen3
Follow Eigen Installation. Required at least 3.1.0.
1.5 Ceres Solver
Follow Ceres Installation.
1.6 MVS camera driver
Install the HIKROBOT camera driver as follows.
tar zxvf MVS-2.0.0_x86_64_20191126.tar.gz
cd ./MVS-2.0.0_x86_64_20191126
chmod +x setup.sh
sudo ./setup.sh
2. Build CamVox
Clone the repository and catkin_make:
cd ~/catkin_ws/src
git clone https://github.com/ISEE-Technology/CamVox
cd CamVox/isee-camvox && chmod +x build.sh && chmod +x build_ros.sh
./build_ros.sh
./build.sh
source ~/catkin_ws/devel/setup.bash
3. Run with Hardware
3.1 Hardware
Platform | Item | Pics | Shopping Link |
---|---|---|---|
Livox Horizon | Lidar | ||
MV-CE060-10UC | Camera | ||
Inertial Sense uINS | RTK | ||
Manifold2C | Onboard-Computer | ||
Scout-mini | Robot Chassis |
3.2 Hard Synchronization
Hard synchronization is performed with all of these sensors by a trigger signal of 10 Hz. The camera output at each trigger signal(10 Hz). The lidar keeps a clock (synced with GPS-RTK) and continuously outputs the scanned point with an accurate timestamp. In the meantime, the IMU outputs at a frequency of 200 Hz synced with the trigger. The Hardware Synchronization diagram is as follows.
3.3 Running
Connect to your PC to Livox Horizon lidar by following Livox-ros-driver installation.
chmod +x run.sh
./run.sh
4. Run with Rosbag Example
4.1 SUSTech Dataset (Loop Closure)
We open sourced our dataset in SUSTech campus with loop closure. Download here.
Other framework data formats for comparison. VINS-mono | livox_loam_horizon.
4.2 Rosbag Example with static scenes (Automatic Calibration trigger)
We provide a rosbag file with static scenes to test the automatic calibration thread. Download here.
When the car detects more than 20 frames of still images (about 2 seconds), the automatic calibration thread starts to work. The thread will be interrupted to enter the SLAM mode if the car starts to move before the end of calibration.
The effects of automatic calibration is shown as follows.
4.3 Running
cd CamVox/isee-camvox
rosrun online camvox Vocabulary/ORBvoc.bin camvox/offline/Livox.yaml 1000
rosbag play YOUR_DOWNLOADED.bag
5. Acknowledgements
The authors are grateful for the pioneering work from ORB_SLAM2, ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras. The authors also sincerely thank colleagues at Livox Technology for help in data aquisition and discussion.
This work is from ISEE Research Group at SUSTech.
6. License
The source code is released under GPLv2.0 license.
If you use CamVox in an academic work, please cite:
@misc{zhu2020camvox,
title={CamVox: A Low-cost and Accurate Lidar-assisted Visual SLAM System},
author={Yuewen Zhu and Chunran Zheng and Chongjian Yuan and Xu Huang and Xiaoping Hong},
year={2020},
eprint={2011.11357},
archivePrefix={arXiv},
primaryClass={cs.RO}
}