Read Lidar and Camera Data from Rosbag File

This example shows how to read and save images and point cloud data from a rosbag file. This example also shows how to prepare the data for lidar camera calibration.

Download the rosbag file using the helperDownloadRosbag helper function, defined at the end of this example.

path = helperDownloadRosbag;

Retrieve information from the bag file.

bag = rosbag(path);

Select image and point cloud messages from the rosbag and select a subset of messages from the file by using the appropriate topic names. You can filter the data by using timestamps as well. For more information, see the select (ROS Toolbox) function.

imageBag = select(bag,'Topic','/camera/image/compressed');
pcBag = select(bag,'Topic','/points');

Read all the messages.

imageMsgs = readMessages(imageBag);
pcMsgs = readMessages(pcBag);

To prepare data for lidar camera calibration, the data across both the sensors must be time-synchronized. Create timeseries (ROS Toolbox) objects for the selected topics and extract the timestamps.

ts1 = timeseries(imageBag);
ts2 = timeseries(pcBag);
t1 = ts1.Time;
t2 = ts2.Time;

For accurate calibration, images and point clouds must be captured with the same timestamps. Match the corresponding data from both the sensors according to their timestamps. To account for uncertainty, use a margin of 0.1 seconds.

k = 1;
if size(t2,1) > size(t1,1)
    for i = 1:size(t1,1)
        [val,indx] = min(abs(t1(i) - t2));
        if val <= 0.1
            idx(k,:) = [i indx];
            k = k + 1;
        end
    end
else
    for i = 1:size(t2,1)
        [val,indx] = min(abs(t2(i) - t1));
        if val <= 0.1
            idx(k,:) = [indx i];
            k = k + 1;
        end
    end
end

Create directories to save the valid images and point clouds.

pcFilesPath = fullfile(tempdir,'PointClouds');
imageFilesPath = fullfile(tempdir,'Images');
if ~exist(imageFilesPath,'dir')
    mkdir(imageFilesPath);
end
if ~exist(pcFilesPath,'dir')
    mkdir(pcFilesPath);
end

Extract the images and point clouds. Name and save the files in their respective folders. Save corresponding image and point clouds under the same number.

for i = 1:length(idx)
    I = readImage(imageMsgs{idx(i,1)});
    pc = pointCloud(readXYZ(pcMsgs{idx(i,2)}));
    n_strPadded = sprintf('%04d',i) ;
    pcFileName = strcat(pcFilesPath,'/',n_strPadded,'.pcd');
    imageFileName = strcat(imageFilesPath,'/',n_strPadded,'.png');
    imwrite(I,imageFileName);
    pcwrite(pc,pcFileName);
end

Launch the Lidar Camera Calibrator app and use the interface to load the data into the app. You can also load the data and launch the app from the MATLAB® command line.

checkerSize = 81; %millimeters
padding = [0 0 0 0];
lidarCameraCalibrator(imageFilesPath,pcFilesPath,checkerSize,padding)

Supporting Function

function rosbagFile = helperDownloadRosbag()
% Download the data set from the given URL.
rosbagZipFile = matlab.internal.examples.downloadSupportFile( ...
    'lidar','data/lccSample.zip');
[outputFolder,~,~] = fileparts(rosbagZipFile);
rosbagFile = fullfile(outputFolder,'lccSample.bag');
if ~exist(rosbagFile,'file')
    unzip(rosbagZipFile,outputFolder);
end
end