The velo2cam_calibration software implements an Automatic Calibration algorithm for Lidar-Stereo camera setups [1]. This software is provided as a ROS package.
Package developed at Intelligent Systems Laboratory, Universidad Carlos III de Madrid.
cloud2 (sensor_msgs/PointCloud2)
Stereo camera point cloud containing points belonging to edges in the left image
cam_plane_coeffs (pcl_msgs::ModelCoefficients)
Coefficients of the calibration target plane model
/stereo_pattern/centers_cloud (velo2cam_calibration::ClusterCentroids)
Target circles centers obtained from stereo camera data
cloud1 (sensor_msgs/PointCloud2)
LIDAR pointcloud
/laser_pattern/centers_cloud (velo2cam_calibration::ClusterCentroids)
Target circles centers obtained from LIDAR data
~cloud1 (velo2cam_calibration::ClusterCentroids)
Target circles centers obtained from LIDAR data
~cloud2 (velo2cam_calibration::ClusterCentroids)
Target circles centers obtained from stereo camera data
~cloud3 (sensor_msgs/PointCloud2)
Original LIDAR pointcloud
TF containing the transformation between both sensors (see TF info in ROS Wiki)
The node broadcasts the TF transformation between velodyne and stereo frames. The fixed transformation between stereo_camera and stereo is published by a static broadcaster in stereo_pattern.launch. The picture below shows the three coordinate frames:
Note: Additionally, a .launch file called calibrated_tf.launch containing the proper TF broadcasters is created in the /launch folder of the velo2cam_calibration package so you can use the calibration just executing:
roslaunch velo2cam_calibration calibrated_tf.launch
Some sample .launch files are provided in this package. The simplest way to launch the algorithm is by running the three main ROS nodes as follows:
roslaunch velo2cam_calibration laser_pattern.launch
roslaunch velo2cam_calibration stereo_pattern.launch
roslaunch velo2cam_calibration velo2cam_calibration.launch
Note: In order to test the algorithm with a proper ground truth, a simulator environment in Gazebo is provided here
The following scheme shows the real size of the calibration target used by this algorithm. Measurements are given in centimeters (cm).
Note: Other size may be used for convenience. If so, please configure nodes parameters accordingly.
[1] Guindel, C., Beltrán, J., Martín, D. and García, F. (2017). Automatic Extrinsic Calibration for Lidar-Stereo Vehicle Sensor Setups. arXiv:1705.04085 [cs.CV] *
* Accepted to IEEE International Conference on Intelligent Transportation Systems 2017 (ITSC).