Editor: aihanb
Authentic authors: Nico
Calibration of the LiDAR sensor with RGB camera finds its usage in many application fields from enhancing image classification to the environment perception and mapping. This package is used to calculate the translation and rotation matrix between camera and laser coordinate, especially for Hesai Pandar P40.
There are two different important points between Hesai LiDAR and Velodyne:
- Timestamp. Because Pandar_P40 doesn't apply the Linux UTC time, you should add two sentences in /HesaiLidar-ros/src/main.cc
void lidarCallback(boost::shared_ptr<PPointCloud> cld, double timestamp)
{
...
output.header.frame_id = "/pandar_40";
output.header.stamp = ros::Time::now();
...
}
In Panda40 User's Manual, it says "If the user wants to get absolute time, GPS module is required".
- Different coordinate. Pay more attention on cv::solvePnPRansac.
I use the QR code board as the marker, detect the center point pair of the QR code as 2D point in image and 3D points in laser coordinates. Then use the PnP method to get the relation between the two coordinates. Actualy you can use any other marker. It will also give you good result.
Image shows the image and laser fuse result:
We use ros to get the image and laser message.
Devices in here: [Camera] Point Grey BFLY-U3-23S6; [LiDAR] Hesai_Pandar40/40P
Here we use the QR code as the marker. The size of the marker is 80*80 cm. Other sizes will also fit.
Marker
You can download the test data in here(password: ppw0). This rosbag have two topic:
image topic:/camera/image_color
laser topic: /pandar_points
Run the bag file:
$rosbag play ss8.bag -l
And follow the "Usage"step, you can test the package.
If you use the test data, you can ignore this step.
$rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.058 image:=/camera/image_color camera:=/camera
You shoude change the size/square/image/camera with your own parameters. Write the camera calibration result in the cam_laser_calib/src/solvepnp/param/calib.yml file. After this step, you can obtain the camera instrinsic matrix.
Create a ros workspaces named cam_laser_calib.
$cd cam_laser_calib/src
$git clone https://github.com/aihanb/cam_laser_calib.git
$cd ..
$catkin_make
$source ./devel/setup.bash
$roslaunch cam_laser_calib calibration.launch
Here we shoud notice the parameter in launch file, change the "onlyDrawPointsColor" value to false. Replace the point cloud and image topic with your own topic name.
<launch>
<node pkg="cam_laser_calib" type="cam_laser_calib_node" name="cam_laser_calib" args="$(find cam_laser_calib)/../solvepnp/imageCloudPoints.txt $(find cam_laser_calib)/../solvepnp/param/calib.yml" output="screen">
<param name="strSub_pc2" type="string" value="/pandar_points"/>
<param name="strSub_img" type="string" value="/camera/image_color"/>
<param name="onlyDrawPointsColor" type="bool" value="false"/>
<param name="DistanceThreshold" type="double" value="0.05"/>
</node>
</launch>
We use rqt_reconfigure to dynamic config the rectangle cut area of point cloud.
$rosrun rqt_reconfigure rqt_reconfigure
rqt_reconfigure
Before cut the point cloud
After cut the point cloud
Estimated plane
After adjust the parameter, write it in the cam_laser_calib/src/camLaserCalib/cfg/cam_laser_calib.cfg file and shutdown the program. Recompile the package.
$cd cam_laser_calib/
$catkin_make
$roslaunch cam_laser_calib calibration.launch
After this step, we write the point pairs of QR code center in laser coordinate and image coordinate to cam_laser_calib/src/solvepnp/imageCloudPoints.txt file.
We use solvePnP method in openCV to get the calibration matrix.
$cd cam_laser_calib/src/solvepnp/
$cmake .
$make
$cd bin/
$./solvepnp
After this step, we can obtain the camera extrinsic matrix T. Please copy the T matrix in terminal to cam_laser_calib/src/solvepnp/param/calib.yml file. Replace the CameraExtrinsicMat.
At this step we could change the onlyDrawPointsColor parameter in "calibtation.launch" file to true. Got the color point cloud as the first figure.