6DOF camera calibration using ROS for BlackFly and Kinect

The BlackFly ROS node as well as the calibration script was tested with ROS Hyrdo (but very likely will work with Groovy as well). The camera_pose_calibration uses ROS Fuerte.

BlackFly ROS node

We wrote a ROS node to publish images from the BlackFly camera, which can be found in the 3rdhand git repository. The camera nodes are started by a launch file (an example file is included).

<node name="fly_cam" pkg="flycam" type="flycam_topic" output="screen" >
  <param name="serial_number" value="13421101" />
  <param name="calibration_file" value="cal.xml" />
  <param name="exposure" value="1" />
  <param name="image_width" value="1280" />
  <param name="image_height" value="1024" />
  <param name="image_format" value="MONO" />

As an argument the serial number of the camera and a calibration file has to be provided, along with some additional options. If a calibration file is provided, the rectified images will be published via the image_rect topic and image_raw provides the image as is. Without a calibration file, or if the calibration file could not be found, only the image_raw topic provides actual data.

Camera Calibration

The camera calibration can be performed using the calibrate.py script provided along with the BlackFly ROS node. It uses the standard camera_calibration package of ROS and does a bit of post processing to get the calibration data into the right format.

calibrate.py takes three arguments. First the name of the camera ROS-node that publishes the uncalibrated image under the name of image_raw. Second and third the serial number of the camera as well as a lens description. The last two parameters are only used for naming the resulting calibration file and the name of the lens can be omitted.

After starting the script, the interactive calibration begins. The checkerboard should be moved in the image until the "Calibrate" button becomes active and the progress bars on the right side are as big as possible. A click on "Calibrate" should make the "Save" and "Commit" buttons available (Note that in our experience the program seems to freeze for quite some time on calibration, so be patient ;)). Click "Save" and then "Commit". If the press on "Commit" does not end the program (which happened regularly for us) you can end it by e.g. using xkill. After the calibration, some transformations take place and the calibration in xml and yaml format will be produced.

The yaml file is generally what ROS uses, the xml file has a format which can be loaded easily using OpenCV and is also the format our ROS node uses.

For ROS the calibration files are generally expected in /.ros/camera_info under the name SERIALNUMBER.yaml.

6DOF Pose Calibration

As long as all the cameras publish a rectified image under the image_rect topic, the pose calibration can be performed.

The package we use for calibration was written for ROS fuerte and since multiple ROS versions behave pretty well together we did not change that. So if needed, ROS fuerte should be installed. The different ROS versions can be switched by sourcing the different setup.bash files.

We use camera_pose_calibration from https://github.com/Paethon/camera_pose. If it is already installed, the original camera-pose package has to be uninstalled. After cloning the git repository it has to be built with rosmake. (Be aware that the camera_pose directory has to be in ROS_PACKAGE_PATH for rosmake to run).

The calibration process can be started with roslaunch camera_pose_calibration calibrate_2_camera.launch camera1_ns:=fly_cam camera2_ns:=/camera/rgb checker_rows:=4 checker_cols:=5 checker_size:=0.03 where camera1_ns and camera2_ns are the names of the cameras under which the image_rect topics are published and the checker_* arguments have to be set in accordance to the used checker board. More than two cameras can be calibrated using calibrate_3_camera.launch etc.

After this, the checker board should be shown in a way so that it is visible to all cameras, and it has to be held steady until all the bars are green. A message should pop up informing the user about the successful (or not) optimization. In this case the node publishes the 6DOF of all the cameras under /camera_calibration.

This should be repeated at different points until the program only returns optimization failures (or until the workspace has been sufficiently covered).

Saving the calibration data

A small ROS-node (calibration_saver.py) is provided to save the produced calibration data (published under the /camera_calibration topic) in an xml-file for later use (~ /.ros/calibration.xml). The node can either be started manually or included in the launch file with:

<node pkg="camera_pose_calibration" type="calibration_saver.py" name="calibration_saver_xml"
        args="calibration.xml" output="screen" />

The standard calibrate_2_camera.launch already starts the calibration saver node automatically.

ROS Hydro

In ROS hydro the Kinect camera does not publish image_rect and therefore camera_pose_calibration does not work. As a workaround we manually started a node which does the rectification using ROS_NAMESPACE=/camera/rgb rosrun image_proc image_proc. This takes /camera/rgb/image_raw and publishes a rectified /camera/rgb/image_rect. In Groovy this is not necessary since image_rect is being published directly.

Author: Sebastian Stabinger

Email: firstname@lastname.name

Created: 2018-01-01 Mon 20:07

Emacs 25.3.1 (Org mode 8.2.10)