3 次元点群のレジストレーションと繋ぎ合わせ

この例では、反復最近接点 (ICP) アルゴリズムにより複数の点群を組み合わせて 3 次元シーンを再構成する方法を説明します。

概要

この例では、Kinect を使ってキャプチャした点群の集合を繋ぎ合わせて、より大きいシーンの 3 次元ビューを構成します。例では ICP が 2 つの連続する点群に適用されます。このタイプの再構成は、オブジェクトの 3 次元モデルを開発したり、Simultaneous localization and mapping (SLAM) に使用する 3 次元世界地図を作成するのに使用できます。

2 つの点群のレジストレーション

dataFile = fullfile(toolboxdir('vision'), 'visiondata', 'livingRoom.mat');
load(dataFile);

% Extract two consecutive point clouds and use the first point cloud as
% reference.
ptCloudRef = livingRoomData{1};
ptCloudCurrent = livingRoomData{2};

レジストレーションの質は、データのノイズおよび ICP アルゴリズムの初期設定によって異なります。前処理手順を適用してノイズをフィルター処理するか、データに適した初期プロパティ値を設定することができます。ここでは、ボックス グリッド フィルターを使用したダウンサンプリングによりデータを前処理し、グリッド フィルターのサイズを 10 cm に設定します。グリッド フィルターは点群の空間を立方体に分割します。各立方体内の点は、その X、Y、Z 座標の平均を取ることで単一の出力点に結合されます。

gridSize = 0.1;
fixed = pcdownsample(ptCloudRef, 'gridAverage', gridSize);
moving = pcdownsample(ptCloudCurrent, 'gridAverage', gridSize);

% Note that the downsampling step does not only speed up the registration,
% but can also improve the accuracy.

2 つの点群の位置を合わせるには、ICP アルゴリズムを使って、ダウンサンプリングされたデータに対し 3 次元剛体変換の推定を行います。ここでは最初の点群を参照として使用し、推定した変換を元の 2 番目の点群に適用します。オーバーラップする点を処理するには、シーンの点群を、位置合わせした点群とマージする必要があります。

まず、2 番目の点群を最初の点群と位置合わせするための剛体変換を求めます。これを使用して、2 番目の点群を、最初の点群によって定義された参照座標系に変換します。

tform = pcregistericp(moving, fixed, 'Metric','pointToPlane','Extrapolate', true);
ptCloudAligned = pctransform(ptCloudCurrent,tform);

これで、レジストレーション済みのデータを使ってワールド シーンを作成できるようになりました。オーバーラップする領域は、1.5 cm のボックス グリッド フィルターでフィルター処理されます。結果として得られるシーンの点群で必要とされるストレージ容量を抑えるにはマージ サイズを大きくし、シーンの解像度を高めるにはマージ サイズを小さくします。

mergeSize = 0.015;
ptCloudScene = pcmerge(ptCloudRef, ptCloudAligned, mergeSize);

% Visualize the input images.
figure
subplot(2,2,1)
imshow(ptCloudRef.Color)
title('First input image','Color','w')
drawnow

subplot(2,2,3)
imshow(ptCloudCurrent.Color)
title('Second input image','Color','w')
drawnow

% Visualize the world scene.
subplot(2,2,[2,4])
pcshow(ptCloudScene, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down')
title('Initial world scene')
xlabel('X (m)')
ylabel('Y (m)')
zlabel('Z (m)')

drawnow

点群のシーケンスの繋ぎ合わせ

より大きな 3 次元シーンを作成するには、上記と同じ手続きを繰り返して点群のシーケンスを処理します。最初の点群を使用して、参照座標系を規定します。各点群を参照座標系に変換します。この変換はペアワイズ変換の乗算です。

% Store the transformation object that accumulates the transformation.
accumTform = tform; 

figure
hAxes = pcshow(ptCloudScene, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down');
title('Updated world scene')
% Set the axes property for faster rendering
hAxes.CameraViewAngleMode = 'auto';
hScatter = hAxes.Children;

for i = 3:length(livingRoomData)
    ptCloudCurrent = livingRoomData{i};
       
    % Use previous moving point cloud as reference.
    fixed = moving;
    moving = pcdownsample(ptCloudCurrent, 'gridAverage', gridSize);
    
    % Apply ICP registration.
    tform = pcregistericp(moving, fixed, 'Metric','pointToPlane','Extrapolate', true);

    % Transform the current point cloud to the reference coordinate system
    % defined by the first point cloud.
    accumTform = affine3d(tform.T * accumTform.T);
    ptCloudAligned = pctransform(ptCloudCurrent, accumTform);
    
    % Update the world scene.
    ptCloudScene = pcmerge(ptCloudScene, ptCloudAligned, mergeSize);

    % Visualize the world scene.
    hScatter.XData = ptCloudScene.Location(:,1);
    hScatter.YData = ptCloudScene.Location(:,2);
    hScatter.ZData = ptCloudScene.Location(:,3);
    hScatter.CData = ptCloudScene.Color;
    drawnow('limitrate')
end

% During the recording, the Kinect was pointing downward. To visualize the
% result more easily, let's transform the data so that the ground plane is
% parallel to the X-Z plane.
angle = -pi/10;
A = [1,0,0,0;...
     0, cos(angle), sin(angle), 0; ...
     0, -sin(angle), cos(angle), 0; ...
     0 0 0 1];
ptCloudScene = pctransform(ptCloudScene, affine3d(A));
pcshow(ptCloudScene, 'VerticalAxis','Y', 'VerticalAxisDir', 'Down', ...
        'Parent', hAxes)
title('Updated world scene')
xlabel('X (m)')
ylabel('Y (m)')
zlabel('Z (m)')

×

例を開く

この例の変更されたバージョンがあります。編集された方の例を開きますか?

いいえ、変更されたバージョンを上書きします。 はい