The camera_calibration package of ROS provides us with a convenient tool to calibrate a stereo camera. With camera_calibration, we could calibrate a stereo camera in an online fashion. And camera_calibration could tell you, while in the process of sampling data for calibration, whether you have collected enough images that could possibly result in a good calibration. After the calibration, you could choose to save the data into your file system as a zip file. In this zip file, you will find two YAML files that contain the calibration results for the individual cameras that form the stereo system. Later in the 3D reconstruction process based on OpenCV, you will need to convert the above-mentioned YAML files into some format that OpenCV could handle easily. To be specific, you need to convert and generate files that feed essential information to the OpenCV APIs to make OpenCV work properly to give you a disparity map. The files need to be generated by converting the YAML files include:

  • The camera matrix, K.
  • The distortion coefficient vector, D.
  • The Projection matrix, P. And,
  • The Rotation matrix, R. (Called rectification_matrix in the YAML file.)

K, D, P and, R need to be generated for each camera of the stereo system. It is very straightforward to convert data from the YAML file to formats that could be used by OpenCV. In particular, for the current project I am working on, I would like to convert the YAML files into plain text files that could be directly loaded by numpy’s loadtxt() function in Python. Later, the 3D reconstruction will be performed in Python with the help of cv2 and numpy packages.

For K, P, and R, it is really simple. In python, you just read in the YAML files and then export the matrices by numpy’s savetxt() function. For D, things are a little bit more complex. From the documentation of cv2, we know that there should be 8 parameters in the distortion coefficient vector. Namely, there are designated as

\[\left [ k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6 \right ]\]

where \(k_1\) to \(k_6\) are for radial distortion, and the ps are for tangential distortion. In the documentation of cv2, you could find the detailed definitions and descriptions of these parameters.

However, it seems to me that camera_calibration package only gives you 5 parameters. These parameters in a YAML file look like the following figure.

<img>The distortion vector in the YAML file.

We have to figure out what is the relationship between these parameters in the YAML file and the parameters defined in an OpenCV distortion vector. The clue lies in the “distortion_model” entry in the YAML file. It says that camera_calibration package uses a distortion model with the name of “plumb_bob”. It turns out that, after a quick Googling, plumb_bob is just a simple version of the distortion model which is utilized by OpenCV. I found a web page documenting the Plumb Bob model. The result for our converting is so simple that we just copy the 5 values that in the YAML file into a numpy vector, one by one without any modification of the ordering of these values.

Another point that I would like to mention is that for my current cv2 (version 3.3.1-dev), the calibration API function produces a vector of a length 14 to hold the distortion vector. I am not sure why it has to be 14. However, I just allocate a vector of length of 14 with all its elements being zero. Then this 14-element long vector gets filled up by the 5 values from the YAML file, leaving other elements of the vector remaining zero and not touched.

I write a simple python script to do the job. This script requires at least two arguments to work. These arguments define the folder where you keep the input YAML files and define the output directory. There are a couple of optional arguments that control the naming of the input and output files. Using - -help to get the explanation of each argument. The script as follows. The use of sensor_msgs.msg.CameraInfo from ROS just has no relevance to the converting task. Please ignore any confusion brought by sensor_msgs.msg.CameraInfo.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118

from __future__ import print_function

# Description
# ===========
# 
# Convert the YAML files form the camera_calibration package in ROS into
# plain text files that coulde be used by OpenCV.
#
# Author
# ======
#
# Yaoyu Hu <yaoyuh@andrew.cmu.edu>
#
# Data
# ====
#
# Created 2018-06-22
# 

import os
import argparse

import numpy as np

from sensor_msgs.msg import CameraInfo

DISTORTION_COEFFICIENTS_LENGTH = 14

def load_camera_info(filename):
        """Load a yaml file as the camera info."""

        import yaml

        # Open the file.
        fp = open( filename, "r" )

        # Parse the yaml file.
        ci = yaml.safe_load(fp)

        # Copy the information into cameraInfo.
        cameraInfo = CameraInfo()

        cameraInfo.height = ci["image_height"]
        cameraInfo.width  = ci["image_width"]
        cameraInfo.distortion_model = ci["distortion_model"]
        cameraInfo.K = ci["camera_matrix"]["data"]
        cameraInfo.D = ci["distortion_coefficients"]["data"]
        cameraInfo.R = ci["rectification_matrix"]["data"]
        cameraInfo.P = ci["projection_matrix"]["data"]

        return cameraInfo

if __name__ == "__main__":
    # Create an argument parser.
    parser = argparse.ArgumentParser( description = "Read the yaml files that produced by the ROS node camera_calibration and convert them into files of OpenCV format." )
    parser.add_argument( "from_dir",    nargs = "?", help="The directory where the yaml files are.")
    parser.add_argument( "out_dir",     nargs = "?", help="The destination directory to put the converted files." )
    parser.add_argument( "--in_file_1", nargs = "?", default = "left.yaml", help="Input file of the left camera." )
    parser.add_argument( "--in_file_2", nargs = "?", default = "right.yaml", help="Input file of the right camera." )
    parser.add_argument( "--cm_1",      nargs = "?", default = "CameraMatrixLeft.dat", help="Output file of the left camera matrix, numpy txt format." )
    parser.add_argument( "--cm_2",      nargs = "?", default = "CameraMatrixRight.dat", help="Output file of the right camera matrix, numpy txt format." )
    parser.add_argument( "--dst_1",     nargs = "?", default = "DistortionCoefficientLeft.dat", help="Output file of the left distortion coefficient, numpy txt format." )
    parser.add_argument( "--dst_2",     nargs = "?", default = "DistortionCoefficientRight.dat", help="Output file of the right distortion coefficient, numpy txt format." )
    parser.add_argument( "--P_1",       nargs = "?", default = "P1.dat", help="Output file of the left projection matrix, numpy txt format." )
    parser.add_argument( "--P_2",       nargs = "?", default = "P2.dat", help="Output file of the right projection matrix, numpy txt format." )
    parser.add_argument( "--R_1",       nargs = "?", default = "R1.dat", help="Output file of the left rotation/rectification matrix, numpy txt format." )
    parser.add_argument( "--R_2",       nargs = "?", default = "R2.dat", help="Output file of the right rotation/rectification matrix, numpy txt format." )

    # Transfer the command line arguments to local variabless.
    args = parser.parse_args()

    fromDir = args.from_dir
    outDir  = args.out_dir

    inFiles = [ fromDir + "/" + args.in_file_1, fromDir + "/" + args.in_file_2 ]
    outCM   = [ outDir + "/" + args.cm_1,  outDir + "/" + args.cm_2 ]
    outDST  = [ outDir + "/" + args.dst_1, outDir + "/" + args.dst_2 ]
    outP    = [ outDir + "/" + args.P_1,   outDir + "/" + args.P_2 ]
    outR    = [ outDir + "/" + args.R_1,   outDir + "/" + args.R_2 ]

    # Read the yaml files.
    cameraInfo = []

    cameraInfo.append( load_camera_info(inFiles[0]) )
    cameraInfo.append( load_camera_info(inFiles[1]) )

    # Test the destination directory.
    if ( False == os.path.isdir( outDir ) ):
        print("The destination directory (%s) does not exist. Create the directory." % outDir)
        os.mkdir( outDir )

    # Output data to file system.
    nCameraInfo = len( cameraInfo )

    for i in range(nCameraInfo):
        print("Process the %d/%dth yaml file..." % ( i+1, nCameraInfo ))

        ci = cameraInfo[i]

        # Camera matrix.
        cameraMatrix = np.array( ci.K ).reshape((3, 3))
        np.savetxt( outCM[i], cameraMatrix )

        # Distortion coefficients.
        distortionCoefficient = np.zeros((1, DISTORTION_COEFFICIENTS_LENGTH))
        nDSTYaml = len( ci.D )
        distortionCoefficient[0, :nDSTYaml] = ci.D
        np.savetxt( outDST[i], distortionCoefficient )

        # Projection matrix.
        P = np.array( ci.P ).reshape((3, 4))
        np.savetxt( outP[i], P )

        # Rotation/Rectification matrix.
        R = np.array( ci.R ).reshape((3, 3))
        np.savetxt( outR[i], R )

    print("Done.")

Image courtesy: Cover image is copied from OpenCV documentation.