rosbag ファイルからの LiDAR とカメラのデータの読み取り

この例では、rosbag ファイルからイメージと点群のデータを読み取って保存する方法を示します。また、この例では LiDAR カメラ キャリブレーション用にデータを準備する方法も示します。

この例の終わりに定義されている補助関数 helperDownloadRosbag を使用して、rosbag ファイルをダウンロードします。

path = helperDownloadRosbag;

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);

LiDAR カメラ キャリブレーション用にデータを準備するには、両方のセンサーの間でデータが時間同期されていなければなりません。選択したトピックの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 カメラ キャリブレーターアプリを起動し、インターフェイスを使用してアプリにデータを読み込みます。データの読み込みとアプリの起動は、MATLAB® コマンド ラインからも実行できます。

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

サポート関数

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