Import Stereo Camera Parameters from ROS

Open Live Script

The ROS camera calibration package estimates stereo camera parameters using the OpenCV camera calibration tools [1]. After calibrating a stereo camera in ROS, you can export its camera parameters to an INI file using the camera calibration parser. To use the calibrated stereo camera with Computer Vision Toolbox™ functions, such as rectifyStereoImages, you must read the camera parameters from the INI file and convert them into a stereoParameters object using stereoParametersFromOpenCV.

Note: The stereoParametersFromOpenCV function supports importing stereo camera parameters for only those pinhole camera models that use the ROS plumb-bob distortion model.

Read Stereo Camera Parameters from ROS INI File

Read the stereo camera parameters stored in stereoParams.ini using the helper function helperReadINI.

stereoParamsINI = helperReadINI("stereoParams.ini");

Compute Baseline Parameters of Stereo Camera

The baseline parameters of a stereo camera describe the relative translation and rotation of the two cameras in the stereo camera pair. The relative rotation and translation of camera 2 with respect to camera 1 is required to create the stereoParameters object using stereoParametersFromOpenCV. You can compute these from the rectification and projection matrices read from the ROS INI file [2].

Extract the two camera parameters from the stereoParams structure.

cameraParams1 = stereoParamsINI.narrow_stereo_left;
cameraParams2 = stereoParamsINI.narrow_stereo_right;

Extract the translation of camera 2 relative to camera 1 from the last column of the projection matrix.

translationOfCamera2 = cameraParams2.projection(:,end);

The rotation of camera 2 relative to camera 1, $R_{21}$ , is derived from the rectification matrices of the stereo pair $R_{1}$ and $R_{2}$ . The rectification matrices are the rotation matrices that align the camera coordinate system to the ideal stereo image plane such that epipolar lines in both stereo images are parallel. Compute the rotation of camera 2 relative to camera 1 as $R_{21}$ = $R_{2}$ * $R_{1}^{T}$ .

rotationOfCamera2 = cameraParams2.rectification*cameraParams1.rectification';

Create `stereoParameters` Object using `stereoParametersFromOpenCV`

Extract the intrinsic matrices and distortion coefficients of the two cameras from the stereoParams structure.

intrinsicMatrix1 = cameraParams1.camera_matrix;
intrinsicMatrix2 = cameraParams2.camera_matrix;

distortionCoefficients1 = cameraParams1.distortion;
distortionCoefficients2 = cameraParams2.distortion;

Obtain the image size from the image field of the stereoParams structure.

imageSize = [stereoParamsINI.image.height stereoParamsINI.image.width];

Use stereoParametersFromOpenCV to create a stereoParameters object from the ROS stereo camera parameters.

stereoParametersObj = stereoParametersFromOpenCV(intrinsicMatrix1, ...
    distortionCoefficients1,intrinsicMatrix2,distortionCoefficients2, ...
    rotationOfCamera2,translationOfCamera2,imageSize);

Rectify Pair of Stereo Images

Use the imported stereo parameters with rectifyStereoImages to rectify an image pair captured using the calibrated stereo camera.

% Load the image pair.
imageDir = fullfile(toolboxdir("vision"),"visiondata","calibration","stereo");
leftImages = imageDatastore(fullfile(imageDir,"left"));
rightImages = imageDatastore(fullfile(imageDir,"right"));
I1 = readimage(leftImages,1);
I2 = readimage(rightImages,1);

% Rectify the image pair.
[J1,J2] = rectifyStereoImages(I1,I2,stereoParametersObj,OutputView="full");

% Display the results.
figure
J = stereoAnaglyph(J1,J2);
imshow(J)

Figure contains an axes object. The hidden axes object contains an object of type image.

Supporting Functions

helperReadINI

The helperReadINI function reads the camera parameters from its input INI file that has been exported from ROS.

function cameraParams = helperReadINI(filename)
% helperReadINI reads a ROS INI file, filename, and returns a structure with
% these fields: image, <camera_name1>, <camera_name2>. image is a
% structure describing the height and width of the image captured by the
% cameras of the stereo pair. The fields <camera_name1> and <camera_name2>
% are structures named after the camera names present in the INI file, and they contain
% these fields: camera_matrix, distortion, rectification_matrix,
% and projection_matrix. These fields are stored in the INI file with their
% values placed in a new line followed by their name.

    f = fopen(filename,"r");
    sectionName = '';
    
    while ~feof(f)
        % Read line from file.
        line = fgetl(f);

        % Trim leading and trailing whitespaces.
        line = strtrim(line);
        
        if isempty(line) || line(1)=='#'
            % Skip empty line and comments.
            continue
        elseif line(1) == '[' && line(end) == ']'
            % Identify section names and continue reading.
            sectionName = line(2:end-1);
            sectionName = strrep(sectionName,'/','_');
            continue
        end

        % Replace blankspaces with underscores to create valid MATLAB variable
        % name.
        name = line;
        name(name == ' ') = '_';
        
        % Read the value data in upcoming lines.
        value = [];
        while ~feof(f)
            line = fgetl(f);
            line = strtrim(line);

            if isempty(line)
                % A empty line indicates end of value data.
                break
            elseif line(1)=='#'
                % Skip comment lines.
                continue
            end
            line = str2num(line); %#ok
            value = [value; line]; %#ok
        end
    
        % Store post-processed value.
        if isempty(sectionName)
            cameraParams.(name) = value;
        else
            cameraParams.(sectionName).(name) = value;
        end
    end
    
    fclose(f);
end

References

[1] http://wiki.ros.org/camera_calibration

[2] http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/CameraInfo.html