Чтение данных Lidar и камеры из файла 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).

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

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

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

Для подготовки данных к калибровке лидарной камеры данные обоих датчиков должны быть синхронизированы по времени. Создать timeseriesОбъекты (панель инструментов ROS) для выбранных разделов и извлекают метки времени.

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