Calibrate Cameras

Table of contents

  1. Overview
  2. Prerequisites
  3. Calibrate Stereo
  4. Calibrate Tracking Camera
    1. Tracking Camera Calibration Procedure

Overview

For Visual Inertial Odometry (VIO) and Visual Obstacle Avoidance (VOA) to work accurately, the tracking and stereo cameras must be calibrated properly. VIO is robust and the default calibration files will work out of the box, however we provide a calibration method here should you wish to recalibrate.

Stereo depth mapping for VOA, on the other hand, is highly sensitive both to the lens parameters as well as the physical mounting of the cameras on the frame. Therefore we do not provide a default calibration file and stereo calibration MUST BE PERFORMED before use.

If you wish to calibrate your cameras, this step should be done after setting up ROS and running voxl-configure-cameras.

Prerequisites

  1. Follow the Setup ROS guide and confirm the ROS environment is set up so your host PC and VOXL can talk ROS to one another.

  2. Install python pip and py

     me@mylaptop:~$ sudo apt install python-pip
     me@mylaptop:~$ pip install pyyaml
    
  3. Requisition an openCV camera calibration card. You can print one from here https://github.com/opencv/opencv/blob/master/doc/acircles_pattern.png

We suggest printing as large as you can. Internally we use a roughly 52x73cm print of the above image with 60mm spacing between circles. The remaining instructions will be based on this dimension and you will need to adjust this number to match your specific calibration card.

  1. Download the ModalAI VOXL Camera Calibration package to the directory of your choosing. Here we put

     me@mylaptop:~$ cd ~/git/
     me@mylaptop:~/git/$ git clone https://gitlab.com/voxl-public/voxl-camera-calibration.git
     me@mylaptop:~/git/$ cd voxl-camera-calibration
    
  2. Patch the ROS Camera Calibrator

    The Downward Facing Tracking camera has an ultra-wide fisheye lens and needs to be calibrated with a fisheye lens distortion model which ROS does not readily support. However, support can be added with the patch included with this package. The patch also improves performance of the stereo calibration. This is only necessary for fisheye calibration but suggested for both.

     me@mylaptop:~/git/voxl-camera-calibration$ ./apply_fisheye_patch.sh
    

    If desired, the ROS calibration tool can be reset back to stock with the following script

     me@mylaptop:~/git/voxl-camera-calibration$ ./fisheye_patch/fisheye_unpatch.sh
    

Calibrate Stereo

  1. Start the voxl_cam_ros node on VOXL to stream left and right stereo images. Note, if doing this over SSH, you may need to use the xterm terminal as the Ubuntu terminal breaks ROS Indigo on VOXL.

     # on VOXL
     yocto:~# roslaunch voxl_cam_ros stereo.launch
    
  2. (Optional) check that the images look sensible and are streaming with rqt_image_view on host PC. Images should be streamed with the ROS topics /stereo/left/image_raw and /stereo/right/image_raw.

     # on host PC
     me@mylaptop:~$ rqt_image_view
    
  3. Start the ROS camera calibrator tool. Note you will need to adjust the pattern, size, and square arguments to match the calibration card you are using. The “square” argument denotes the linear distance between circles in meters in the directions parallel to the image edge, not diagonally.

     # on host pc
     me@mylaptop:~$ rosrun camera_calibration cameracalibrator.py --camera_name=stereo --no-service-check --pattern 'acircles' --size 4x11 --square 0.060 right:=/stereo/right/image_raw left:=/stereo/left/image_raw right_camera:=/stereo/right left_camera:=/stereo/left
    
  4. Move the calibration pattern or the camera around until the X, Y, Size, and Skew bars become green. Now set the calibration pattern down and hit the “CALIBRATE” button in the GUI. The GUI will appear to freeze as it processes the saved image data. This may take 20-60 seconds depending on number of images collected and the speed of your computer.

  5. Once it is done, you will see the calibration coefficients printed to the terminal in which you launched the calibrator GUI. The last few lines of the printout should look something like this:

     camera matrix
     430.403124 0.000000 323.046188
     0.000000 432.020182 236.679750
     0.000000 0.000000 1.000000
    
     distortion
     -0.015701 -0.003187 -0.000397 -0.000020 0.000000
    
     rectification
     0.998362 0.036306 -0.044217
     -0.036325 0.999340 0.000363
     0.044201 0.001244 0.999022
    
     projection
     460.015876 0.000000 353.705967 -36.456202
     0.000000 460.015876 236.873411 0.000000
     0.000000 0.000000 1.000000 0.000000
    

Now hit the “SAVE” button in the GUI. This will save the calibration coefficients to the /tmp/ directory on the host PC. You can now close the GUI. Note that in some ROS releases you may have to kill the GUI with “Ctrl-C” from the terminal in which you launched it.

  1. To push this calibration data to the VOXL, run the following script. This will push both of the ROS camera_info files (left.yaml and right.yaml) to /home/root/.ros/camera_info/. It will also generate a single XML file for use by SNAV and the ModalAI Vision Lib. Note, this requires ADB and a USB cable!

     me@mylaptop:~/git/voxl-camera-calibration$  ./push_stereo_cal_to_voxl.sh
    
  2. (Optional) If you like, you can check that all 3 files appeared and that their contents match the new coefficients printed out during the calibration process on the host PC.

     # on VOXL
     yocto:~# ls /home/root/.ros/camera_info/
     yocto:~# tracking.yaml  hires.yaml  left.yaml  right.yaml
     yocto:~# cat /home/root/.ros/camera_info/right.yaml
     yocto:~# cat /etc/snav/calibration.stereo.xml
    

Calibrate Tracking Camera

This is far less critical than stereo calibration and VIO will likely work just fine out of the box with default camera calibration. However, if high precision is desired it can be calibrated as follows.

Tracking Camera Calibration Procedure

  1. Start the voxl_cam_ros node on VOXL to stream the tracking camera. Note, if doing this over SSH, you may need to use the xterm terminal as the Ubuntu terminal breaks ROS Indigo on VOXL.

     # on VOXL
     yocto:~# roslaunch voxl_cam_ros tracking.launch
    
  2. (Optional) check that the images look sensible and are streaming with rqt_image_view on host PC. Images should be streamed with the ROS topic /tracking/image_raw.

     # on host PC
     me@mylaptop:~$ rqt_image_view
    
  3. Start the ROS camera calibrator tool. Note you will need to adjust the pattern, size, and square arguments to match the calibration card you are using. The “square” argument denotes the linear distance between circles in meters in the directions parallel to the image edge, not diagonally.

     # on host pc
     me@mylaptop:~$ rosrun camera_calibration cameracalibrator.py -c tracking -p 'acircles' --size 4x11 --square 0.060 --fisheye image:=/tracking/image_raw
    
  4. Move the calibration pattern or the camera around until the X, Y, Size, and Skew bars become green. Now set the calibration pattern down and hit the “CALIBRATE” button in the GUI. The GUI will may appear to freeze as it processes the saved image data. It may take several seconds to complete.

  5. Once it is done, you will see the calibration coefficients printed to the terminal in which you launched the calibrator GUI. The last few lines of the printout should look something like this:

     [tracking]
    
     camera matrix
     276.265869 0.000000 323.111810
     0.000000 276.756284 242.173704
     0.000000 0.000000 1.000000
    
     distortion
     -0.004169 0.019566 -0.016125 0.004654
    
     rectification
     1.000000 0.000000 0.000000
     0.000000 1.000000 0.000000
     0.000000 0.000000 1.000000
    
     projection
     298.050232 0.000000 327.226979 0.000000
     0.000000 294.181091 229.660997 0.000000
     0.000000 0.000000 1.000000 0.000000
    

Now hit the “SAVE” button in the GUI. This will save the calibration coefficients to the /tmp/ directory on the host PC. You can now close the GUI. Note that in some ROS releases you may have to kill the GUI with “Ctrl-C” from the terminal in which you launched it.

  1. To push this calibration data to the VOXL, run the following script. This will push both of the ROS camera_info files (left.yaml and right.yaml) to /home/root/.ros/camera_info/. It will also generate a single XML file for use by SNAV and the ModalAI Vision Lib. Note, this requires ADB and a USB cable!

     me@mylaptop:~/git/voxl-camera-calibration$ ./push_tracking_cal_to_voxl.sh