特徴を使用した 2 つの点群間の変換の推定

この例では、2 つの点群の間の剛体変換を推定する方法を示します。この例では特徴の抽出と照合を使用して、推定に必要な点の数を大幅に少なくします。点群から関数extractFPFHFeaturesを使用して高速点特徴ヒストグラム (FPFH) の特徴を抽出した後、抽出された点群から関数pcmatchfeaturesを使用して一致を検索します。最後に、関数estgeotform3dと一致する特徴を使用して剛体変換を推定します。

前処理

入力点群に剛体変換を適用して 2 つの点群を作成します。

点群データをワークスペースに読み取ります。

rng("default")
ptCld = pcread("highwayScene.pcd");
ptCld.Count

ans = 65536

約 65,000 の点が含まれているため、計算速度を向上させるために点群をダウンサンプリングします。

ptCloud = pcdownsample(ptCld,gridAverage=0.2);
ptCloud.Count

ans = 24596

30 度の回転と "x" 軸および "y" 軸の 5 単位分の並進をもつ剛体変換行列を作成します。

rotAngle = 30;
trans = [5 5 0];
tform = rigidtform3d([0 0 rotAngle],trans);

入力点群を変換します。

ptCloudTformed = pctransform(ptCloud,tform);

2 つの点群を可視化します。

pcshowpair(ptCloud,ptCloudTformed)
axis on
xlim([-50 75])
ylim([-40 80])
legend("Original","Transformed",TextColor=[1 1 0])

特徴の抽出とレジストレーション

関数 extractFPFHFeatures を使用して両方の点群から特徴を抽出します。

fixedFeature = extractFPFHFeatures(ptCloud);
movingFeature = extractFPFHFeatures(ptCloudTformed);

一致する特徴を検出し、一致するペアの数を表示します。

[matchingPairs,scores] = pcmatchfeatures(fixedFeature,movingFeature, ...
    ptCloud,ptCloudTformed,Method="Exhaustive");
length(matchingPairs)

ans = 1814

点群から一致する点を選択します。

fixedPts = select(ptCloud,matchingPairs(:,1));
matchingPts = select(ptCloudTformed,matchingPairs(:,2));

一致する点を使用して変換行列を推定します。

estimatedTform = estgeotform3d(fixedPts.Location, ...
    matchingPts.Location,"rigid");
disp(estimatedTform.A)

    0.8660   -0.5000   -0.0003    4.9995
    0.5000    0.8660    0.0000    5.0022
    0.0002   -0.0002    1.0000    0.0020
         0         0         0    1.0000

定義された変換行列を表示します。

disp(tform.A)

    0.8660   -0.5000         0    5.0000
    0.5000    0.8660         0    5.0000
         0         0    1.0000         0
         0         0         0    1.0000

推定された変換を使用して ptCloudTformed を再変換し、初期点群に戻します。

ptCloudTformed = pctransform(ptCloudTformed,invert(estimatedTform));

2 つの点群を可視化します。

pcshowpair(ptCloud,ptCloudTformed)
axis on
xlim([-50 50])
ylim([-40 60])
title("Aligned Point Clouds")