Author: Nico
This package is used to calculate the translation and rotation matrix between the camera and laser coordinate.
The QR code is used as a marker. The center point of the QR code is detected as a 2D point in the image and a 3D point in the laser coordinate. The PnP method is used to get the relation between the two coordinates. Actually you can use any other markers. They will give you good results as well.
The following image shows the image and laser fusion result:
ROS is used to obtain the image and laser message.
Here we use the QR code as the marker. The size of the marker is 80*80 cm.
Marker
The test data can be found here. This rosbag has two topics: image topic: /camera/image_color laser topic: /velodyne32/velodyne_points Run the bag file:
$rosbag play 2016-12-22-14-03-09.bag -l
Follow the "Usage" step to test the package.
Ignore this step if you are using the test data.
$rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.061 image:=/camera/image_color camera:=/camera
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.
Create a ros workspace named cam_laser_calib.
$cd cam_laser_calib/src
$git clone git@github.com:NicoChou/camera-laser-calibration.git
$cd ..
$catkin_make
$roslaunch cam_laser_calib calibration.launch
Change the "onlyDrawPointsColor" parameter value in launch file to false. Replace the point cloud and image topics 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="/velodyne32/velodyne_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>
The rqt_reconfigure is used to dynamic config the rectangle cutting area of point clouds.
$rosrun rqt_reconfigure rqt_reconfigure
rqt_reconfigure
Before cut the point cloud
After cut the point cloud
Estimated plane
Adjust the parameter and wirite them into the cam_laser_calib/src/camLaserCalib/cfg/cam_laser_calib.cfg file. Shutdown the program and recompile the package.
$cd cam_laser_calib/
$catkin_make
$roslaunch cam_laser_calib calibration.launch
After this step, write the point pairs of QR code center under both laser coordinate and image coordinate to cam_laser_calib/src/solvepnp/imageCloudPoints.txt file.
Use the solvePnP method in openCV to get the calibration matrix.
$cd cam_laser_calib/src/solvepnp/build
$make
$../bin/solvepnp
Copy the T matrix shown in the terminal to cam_laser_calib/src/solvepnp/param/calib.yml file to replace the CameraExtrinsicMat.
At this step, the onlyDrawPointsColor parameter in "calibration.launch" file can be changed to true, and then you will be able to get the color point cloud as the first figure.