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