Считайте данные о лидаре и камере из файла 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