How can I use Kalibr with the ZED Mini camera in ROS?

This tutorial illustrates how to use the Kalibr software to correctly get the camera/IMU calibration parameters for the ZED mini.

Preliminar operations

Install Kalibr following the installation guide

Create a folder to keep all the data:

$ mkdir ~/zed-kalibr

Move to the new folder:

$ cd ~/zed-kalibr

 

Set the correct ZED wrapper parameters:

  • Set the parameter imu_timestamp_sync in the file params/zedm.yaml to false. This way the timestamps of the IMU topics are not bound to the last grabbed frame.

Start the ZED Mini node:

$ roslaunch zed_wrapper zedm.launch

Enable left and right camera visualization to be sure to keep the target in sight.
Open a new terminal and enter:

$ rosrun image_view image_view image:=/zed/zed_node/left/image_rect_color &
$ rosrun image_view image_view image:=/zed/zed_node/right/image_rect_color &

Prepare the calibration target

Kalibr supports three different types of calibration target, but using Aprilgrid is suggested. Indeed using Aprilgrid, partially visible calibration boards can be used and the pose of the target is fully resolved (no flips), making the data collection a simpler operation.
Print one of the Aprilgrid available on the Kalibr wiki and fill the file aprilgrid.yaml with the correct information:

  • Measure the size of a single tag and report the value in meters in the field tagSize
  • Measure the distance between two tags (black line) and write it down as "spacing". Report the ratio between the "spacing" measure and the tagSize measure in the tagSpacing field: tagSpacing = "spacing"/tagSize

This is, for example, a target configuration file using tags sized 88 mm and spaced 25 mm:

target_type: 'aprilgrid'   #gridtype
tagCols: 6 #number of apriltags
tagRows: 6 #number of apriltags
tagSize: 0.088 #size of apriltag, edge to edge [m]
tagSpacing: 0.3 #ratio of space between tags to tagSize
#example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]

Data recording

Record the rosbag to be used as Kalibr input:

$ rosbag record -O Kalibr_data.bag /zed/zed_node/imu/data_raw /zed/zed_node/left/image_rect_color /zed/zed_node/right/image_rect_color

For a correct calibration you should provide the following movements:

  • Pitch rotation
  • Yaw rotation
  • Roll rotation
  • Up/Down translation
  • Left/Right translation
  • Forward/Backward translation
  • A series of random movement

All the movements should keep the target in sight. See this YouTube video as an example:
https://youtu.be/puNXsnrYWTY?t=57

At the end of the recording you can check if the rosbag is correct:

$ rosbag info Kalibr_data.bag

You should get an output similar to the following: 

path:         Kalibr_data.bag
version: 2.0
duration: 1:31s (91s)
start: Nov 27 2018 09:10:06.64 (1543306206.64)
end: Nov 27 2018 09:11:38.50 (1543306298.50)
size: 18.8 GB
messages: 76713
compression: none [5473/5473 chunks]
types: sensor_msgs/Image [060021388200f6f0f447d0fcd9c64743]
sensor_msgs/Imu [6a62c6daae103f4ff57a132d6f95cec2]
topics: /zed/zed_node/imu/data_raw 71241 msgs : sensor_msgs/Imu
/zed/zed_node/left/image_rect_color 2736 msgs : sensor_msgs/Image
/zed/zed_node/right/image_rect_color 2736 msgs : sensor_msgs/Image

Be sure that the left and right image message count is equal or at least very similar. If they are not similar try to reduce the framerate of the camera changing the parameter frame_rate in the file params/common.yaml

Get the camera parameters

You can generate the camera parameters file using the following command:

$ kalibr_calibrate_cameras --bag Kalibr_data.bag --topics /zed/zed_node/left/image_rect_color /zed/zed_node/right/image_rect_color --models pinhole-radtan pinhole-radtan --target april_grid.yaml

When the elaboration is complete (it takes many minutes according to the number of image acquired) you will get the file camchain-Kalibr_data.yaml and a full PDF report of the result of the calibration.
For a resolution set to 720p you should get a content similar to this:

cam0:
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [0.0006582543326556634, -0.001338594740114625, 0.0003683086103016461,
0.0016571045464767074]
distortion_model: radtan
intrinsics: [662.3788576876452, 662.6400636208534, 654.9267980662693, 366.0819275708851]
resolution: [1280, 720]
rostopic: /zed/zed_node/left/image_rect_color
cam1:
T_cn_cnm1:
- [0.9999986510996722, -0.0016331077959597664, -0.00017537889157211785, -0.0629588597678212]
- [0.0016329526616102362, 0.9999982785656727, -0.0008810966399958309, 0.00022143806487843462]
- [0.00017681751546064542, 0.0008808090660565046, 0.9999995964553927, -0.00011965349864953196]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
distortion_coeffs: [0.0010461230854058824, -0.002115228401778391, 0.000836698561389109,
0.0018856320059108783]
distortion_model: radtan
intrinsics: [662.2364704261663, 662.383079430529, 654.39543324047, 366.98591988046206]
resolution: [1280, 720]
rostopic: /zed/zed_node/right/image_rect_color

You can compare the result values with the calibration parameters provided by the ZED SDK.
Check the parameters for the left camera:

$ rostopic echo /zed/zed_node/left/camera_info

 

cam0:

  • The first value (fx) in the intrinsic vector should be comparable to the first value of the K vector of the ROS message
  • The second value (fy) in the intrinsic vector should be comparable to the fifth value of the K vector of the ROS message
  • The third value (cx) in the intrinsic vector should be comparable to the third value of the K vector of the ROS message
  • The fourth value (cy) in the intrinsic vector should be comparable to the sixth value of the K vector of the ROS message
  • The distortion_coeffs values should be small, even if not equal to zero
  • The last value of the first row of the T_cn_cnm1 matrix should be very near to -0.06
  • The last value of the second row of the T_cn_cnm1 matrix should be near to zero
  • The last value of the third row of the T_cn_cnm1 matrix should be near to zero
  • The values on the diagonal of the T_cn_cnm1 matrix should be very near to 1.0
  • The remaining values of the T_cn_cnm1 matrix should be near to zero

Now you can kill the zed_wrapper node and the two image visualization nodes.

Set the IMU parameters

Create a file named imu-params.yaml

$ gedit imu-params.yaml

and fill it with the following content:

#Accelerometers
accelerometer_noise_density: 1.4e-03   #Noise density (continuous-time)
accelerometer_random_walk:   8.0e-05   #Bias random walk

#Gyroscopes
gyroscope_noise_density:     8.6e-05   #Noise density (continuous-time)
gyroscope_random_walk:       2.2e-06   #Bias random walk

rostopic:                    /zed/zed_node/imu/data_raw      #the IMU ROS topic
update_rate:                 800.0     #Hz (for discretization of the values above)

Note: the update_rate value must match the value of the parameter imu_pub_rate in the file params/zedm.yaml.

Camera/IMU calibration

You can get the final camera/IMU calibration executing the following command:

$ kalibr_calibrate_imu_camera --bag Kalibr_data.bag --cam camchain-Kalibr_data.yaml --imu imu-params.yaml --target april_grid.yaml

When the elaboration is complete (it takes more than one hour, according to the number of image acquired) you will get the file camchain-imucam-Kalibr_data.yaml containing the final calibration result and a PDF report.
For 720p resolution, a frame rate of 30 FPS and an IMU rate set to 800 Hz, you should get a content similar to this: 

cam0:
T_cam_imu:
- [-0.004248117180815025, -0.9998625130142822, -0.016028367639358054, 8.41951564249623e-05]
- [0.001063559295260974, 0.0160239856188632, -0.9998710420481793, -1.708597954171419e-06]
- [0.9999904111250486, -0.004264616471714999, 0.0009953413010867496, 3.811715513237331e-06]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [0.0006582543326556634, -0.001338594740114625, 0.0003683086103016461,
0.0016571045464767074]
distortion_model: radtan
intrinsics: [662.3788576876452, 662.6400636208534, 654.9267980662693, 366.0819275708851]
resolution: [1280, 720]
rostopic: /zed/zed_node/left/image_rect_color
timeshift_cam_imu: 0.10015660064007245
cam1:
T_cam_imu:
- [-0.004425225567390667, -0.9998865852715411, -0.014395623386818546, -0.06287466260313691]
- [0.0001755322988860608, 0.014394987421997407, -0.9998963713933223, 0.00021986359808054987]
- [0.9999901932354587, -0.004427293884297856, 0.00011181132455595488, -0.00011582840244468725]
- [0.0, 0.0, 0.0, 1.0]
T_cn_cnm1:
- [0.9999986510996738, -0.0016331077959597666, -0.00017537889157211785, -0.0629588597678212]
- [0.0016329526616102364, 0.9999982785656742, -0.0008810966399958309, 0.00022143806487843462]
- [0.00017681751546064537, 0.0008808090660565046, 0.9999995964553943, -0.00011965349864953196]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
distortion_coeffs: [0.0010461230854058824, -0.002115228401778391, 0.000836698561389109,
0.0018856320059108783]
distortion_model: radtan
intrinsics: [662.2364704261663, 662.383079430529, 654.39543324047, 366.98591988046206]
resolution: [1280, 720]
rostopic: /zed/zed_node/right/image_rect_color
timeshift_cam_imu: 0.09305991640438746

Problem solving

Python igraph

If you are getting the error ImportError: No module named igraph you can solve installing python-igraph with apt:

sudo apt update
sudo apt install python-igraph

Wrongly formatted IMU data

If you are getting a Python error of this type:

Waccel = np.eye(3) / (self.accelRandomWalk * self.accelRandomWalk)
TypeError: can't multiply sequence by non-int of type 'str'

Then you have filled the file imu-params.yaml wrongly. If you use scientific notation all the field must contation the decimal part, even if it is zero.
For example, 8.0e-05 is good, while 8e-05 will generate the error