Чтение данных о лидаре и камере из файла Rosbag

В этом примере показано, как считать и сохранять изображения и данные облака точек из файла rosbag. Этот пример также показывает, как подготовить данные для калибровки камеры лидара.

Загрузите файл rosbag с данного URL-адреса с помощью helperDownloadRosbag вспомогательная функция, заданная в конце этого примера.

outputFolder = fullfile(tempdir,'RosbagFile');
rosbagURL = ['https://ssd.mathworks.com/supportfiles/lidar/data/' ...
    'lccSample.zip'];
helperDownloadRosbag(outputFolder,rosbagURL);

Получите информацию из файла сумки.

path = fullfile(outputFolder,'lccSample.bag');
bag = rosbag(path);

Выберите изображения и сообщения облака точек из rosbag и выберите подмножество сообщений из файла с помощью соответствующих имен тем. Можно также фильтровать данные с помощью временных меток. Для получения дополнительной информации смотрите select (ROS Toolbox) функция.

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

Прочитайте все сообщения.

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

Чтобы подготовить данные для калибровки камеры лидара, данные на обоих датчиках должны быть синхронизированы по времени. Создание timeseries (ROS Toolbox) объекты для выбранных тем и извлечь временные метки.

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

Для точной калибровки изображения и облака точек должны быть получены с одинаковыми временными метками. Соответствовать соответствующим данным от обоих датчиков в соответствии с их временными метками. Для расчета неопределенности используйте запас в 0,1 секунды.

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

Создайте директории, чтобы сохранить допустимые изображения и облака точек.

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

Извлеките изображения и облака точек. Назовите и сохраните файлы в соответствующих папках. Сохраните соответствующее изображение и облака точек под тем же номером.

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

Запустите приложение Lidar Camera Calibrator и используйте интерфейс для загрузки данных в приложение. Также можно загрузить данные и запустить приложение из командной строки MATLAB ®.

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

function helperDownloadRosbag(outputFolder,rosbagURL)
% Download the data set from the given URL to the output folder.
rosbagFile = fullfile(outputFolder,'lccSample.bag');
rosbagZipFile = fullfile(outputFolder,'lccSample.zip');
    if ~exist(rosbagFile,'file')
        if ~exist(rosbagZipFile,'file')
            mkdir(outputFolder);
            disp('Downloading the rosbag file (8.5 MB)...');
            websave(rosbagZipFile,rosbagURL);
        end
        unzip(rosbagZipFile,outputFolder);
    end
end