Main Content

rosWriteCameraInfo

Write monocular or stereo camera parameters to ROS or ROS 2 message structure

Since R2022a

Description

msgOut = rosWriteCameraInfo(msg,cameraParams) writes data from the monocular camera parameters structure, cameraParams, to a sensor_msgs/CameraInfo message structure, msg, and returns the output message, msgOut.

Use rosWriteCameraInfo to write the camera parameters obtained after the calibration process. For more information on performing camera calibration using Computer Vision Toolbox™, see Camera Calibration (Computer Vision Toolbox).

example

[msgOut1,msgOut2] = rosWriteCameraInfo(msg,stereoParams) writes data from the stereo camera parameters structure, stereoParams, to two sensor_msgs/CameraInfo message structures, msgOut1 and msgOut2.

example

Examples

collapse all

Create a set of calibration images.

images = imageDatastore(fullfile(toolboxdir("vision"),"visiondata", ...
      "calibration","mono"));
imageFileNames = images.Files;

Detect calibration pattern.

[imagePoints,boardSize] = detectCheckerboardPoints(imageFileNames);

Generate world coordinates of the corners of the squares.

squareSize = 29; % millimeters
worldPoints = patternWorldPoints("checkerboard",boardSize,squareSize);

Calibrate the camera.

I = readimage(images,1);
imageSize = [size(I,1),size(I,2)];
params = estimateCameraParameters(imagePoints,worldPoints, ...
                                  ImageSize=imageSize);

Create a ROS sensor_msgs/CameraInfo message structure.

msg = rosmessage("sensor_msgs/CameraInfo","DataFormat","struct");

Write the camera parameters obtained after calibration to the ROS message. Use the toStruct function to convert the cameraParameters object to a structure.

msg = rosWriteCameraInfo(msg,toStruct(params))
msg = struct with fields:
        MessageType: 'sensor_msgs/CameraInfo'
             Header: [1×1 struct]
             Height: 712
              Width: 1072
    DistortionModel: 'plumb_bob'
                  D: [5×1 double]
                  K: [9×1 double]
                  R: [9×1 double]
                  P: [12×1 double]
           BinningX: 0
           BinningY: 0
                Roi: [1×1 struct]

Specify images containing a checkerboard for calibration.

imageDir = fullfile(toolboxdir("vision"),"visiondata","calibration","stereo");
leftImages = imageDatastore(fullfile(imageDir,"left"));
rightImages = imageDatastore(fullfile(imageDir,"right"));

Detect the checkerboards.

[imagePoints,boardSize] = detectCheckerboardPoints(leftImages.Files,rightImages.Files);

Specify world coordinates of checkerboard keypoints.

squareSizeInMillimeters = 108;
worldPoints = patternWorldPoints("checkerboard",boardSize,squareSizeInMillimeters);

Read in the images.

I1 = readimage(leftImages,1);
I2 = readimage(rightImages,1);
imageSize = [size(I1, 1),size(I1, 2)];

Calibrate the stereo camera system.

stereoParams = estimateCameraParameters(imagePoints,worldPoints,ImageSize=imageSize);

Rectify the images using full output view.

[J1_full,J2_full] = rectifyStereoImages(I1,I2,stereoParams,OutputView="full");

Create a ROS sensor_msgs/CameraInfo message structure.

msg = rosmessage("sensor_msgs/CameraInfo","DataFormat","struct");

Write the stereo parameters obtained after calibration to two ROS messages. Use the toStruct function to convert the stereoParameters object to a structure.

[msg1,msg2] = rosWriteCameraInfo(msg,toStruct(stereoParams))
msg1 = struct with fields:
        MessageType: 'sensor_msgs/CameraInfo'
             Header: [1×1 struct]
             Height: 960
              Width: 1280
    DistortionModel: 'plumb_bob'
                  D: [5×1 double]
                  K: [9×1 double]
                  R: [9×1 double]
                  P: [12×1 double]
           BinningX: 0
           BinningY: 0
                Roi: [1×1 struct]

msg2 = struct with fields:
        MessageType: 'sensor_msgs/CameraInfo'
             Header: [1×1 struct]
             Height: 960
              Width: 1280
    DistortionModel: 'plumb_bob'
                  D: [5×1 double]
                  K: [9×1 double]
                  R: [9×1 double]
                  P: [12×1 double]
           BinningX: 0
           BinningY: 0
                Roi: [1×1 struct]

Input Arguments

collapse all

ROS or ROS 2 sensor_msgs/CameraInfo message, specified as a message structure.

Data Types: struct

Monocular camera parameters structure, specified as a cameraParameters (Computer Vision Toolbox) structure. To obtain the structure from a cameraParameters object, use the toStruct (Computer Vision Toolbox) function.

Data Types: struct

Stereo camera parameters structure, specified as a stereoParameters (Computer Vision Toolbox) structure. To obtain the structure from a stereoParameters object, use the toStruct (Computer Vision Toolbox) function.

Data Types: struct

Output Arguments

collapse all

ROS or ROS 2 camera info message for a monocular camera, returned as a sensor_msgs/CameraInfo message structure.

ROS or ROS 2 camera info message for camera 1 in a stereo pair, returned as a sensor_msgs/CameraInfo message structure.

ROS or ROS 2 camera info message for camera 2 in a stereo pair, returned as a sensor_msgs/CameraInfo message structure.

Extended Capabilities

expand all

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2022a