この記事はMATLAB/Simulink Advent Calendar 2024の12月11日分です。
この記事で書いていること
MATLABのR2024bから、点群データから網羅的に平面検出できるアルゴリズムが追加されました。Lidar Toolboxのpcsegplanesです。
この関数にはいくつかハイパーパラメータがあるのですが、その意味が初見だとわかりづらく、どのように設定していいものやらよくわからないと思いました。少なくとも私はそうでした。でも便利な関数なのできっちり使いこなしたい。
そこでこの記事では、元論文にあたりアルゴリズムの全容を理解した後、MATLABでのパラメータの意味について解説しようと思います。
pcsegplanesのプロパティ
アルゴリズムにあたる前に、どのようなプロパティ(ハイパーパラメータ)があるか、その説明を見ておきます。
- MaxAngularDistance — Maximum absolute angular distance(直訳：最大絶対角度距離)
Maximum absolute angular distance between the normal vector of a detected plane and point normals, specified as a positive scalar in the range (0, 60]. Units are in degrees. Specify a smaller value to get fewer planes with better detection quality.
以下AI訳 検出された平面の法線ベクトルと点の法線との間の最大絶対角度を、(0, 60] の範囲内の正のスカラーとして指定します。単位は度です。より少ない値を指定すると、より少ない平面が検出され、検出品質が向上します。
- MaxCoplanarDistance — Maximum coplanar distance（最大の同一平面上の距離）
Maximum coplanar distance, specified as a positive scalar in the range [75, 90). Units are in degrees. Maximum coplanar distance is the angular distance between the arbitrary vector joining the farthest inlier point from the center of the detected plane and the normal vector of the detected plane.
最大共面距離は、[75, 90) の範囲内の正のスカラーとして指定します。単位は度です。最大共面距離とは、検出された平面の中心から最も遠いインライア点を結ぶ任意のベクトルと、検出された平面の法線ベクトルとの間の角度距離を指します。
- ReferenceVector — Reference orientation constraint（）
Reference orientation constraint to detect a plane in the input point cloud, specified as a 1-by-3 vector representing xyz-coordinates.
入力点群内で平面を検出するための基準方向制約は、xyz座標を表す1行3列のベクトルとして指定します。
- MaxReferenceDeviation — Angular tolerance between orientations of detected plane and reference vector
Angular tolerance between the orientations of the detected plane and the reference vector, specified as a positive scalar. Units are in degrees.
検出された平面の方向と基準ベクトルの方向との間の角度許容範囲は、正のスカラーとして指定します。単位は度です。
- OutlierRatio — Maximum outlier ratio
Maximum outlier ratio, specified as a scalar in the range [0, 1]. This value controls the maximum allowed percentage of outliers relative to the total number of points in a set that is being fit with a planar surface.
最大外れ値率は、[0, 1] の範囲内のスカラーとして指定します。この値は、平面にフィットさせるセット内の総ポイント数に対する外れ値の最大許容割合を制御します。
- MinNumPoints — Minimum number of points in detected planar surface
Minimum number of points in detected planar surface, specified as a positive scalar. By default, this value is total number of points in the input point cloud multiplied by 0.001.
検出された平面上の最小点数は、正のスカラーとして指定します。デフォルトでは、この値は入力点群の総ポイント数に0.001を掛けたものです。
- MinDimensionSize — Minimum size of detected planar surface
Minimum size of a detected planar surface, specified as a positive scalar. The largest edge of the segmented planar surface must be greater than this value. Otherwise the function rejects the planar surface. By default, this value is total number of points in the input point cloud multiplied by 0.01
検出された平面の最小サイズは、正のスカラーとして指定します。セグメント化された平面の最大辺がこの値より大きくなければなりません。そうでない場合、関数はその平面を拒否します。デフォルトでは、この値は入力点群の総ポイント数に0.01を掛けたものです。
この中で一番意味不明だと思ったのがMaxCoplanarDistanceでした。共面距離となっていますが、同一平面上とみなせる距離ということだと解釈したのですが、距離なのに単位は角度となっていてなんですかこれは、と。OutlierRatioも意味不明でした。だいたいの点群では平面に属する点群の方が少ないはずなので、Outlierレシオって50％より多くなりそうなのですが、デフォルト値は25%となっていて、直感と合わないものでした。
これはきちんとアルゴリズムを理解する必要があります。ということで元論文にあたりましょう。
元論文
こちらです。
Araújo, Abner M. C., and Manuel M. Oliveira. “A Robust Statistics Approach for Plane Detection in Unorganized Point Clouds.” Pattern Recognition 100 (April 2020): 107115. https://doi.org/10.1016/j.patcog.2019.107115.
幸いWeb上で無料で読むことができます。
アルゴリズム部分だけ解説します。
この論文では、split, grow, mergeの3段階で、点群全体から平面を検出していきます。それぞれについて説明します。
split
8分木を使って、点群を分割していきます。どこまで分割するかというとノード内にεだけの点群が残るまで、となります。論文中では点群全体の点数の0.1%を実験的に定めています。50,000点のデータなら50点ということですね。
平面判定アルゴリズム
各ノードにおいて、ノードに含まれている点群が平面かどうかを判定します。平面と判定されたノードはgrow、そしてmergeプロセスに移行します。近傍の8ノードすべてで非平面判定となると親ノードが次の判定対象という形でさかのぼっていき、平面判定されるまでさかのぼります。ではどのように平面と判定するかについてみていきます。
よくある平面判定アルゴリズムの一つは点群を固有値分解して、第一固有値と第三固有値の比率を見る方法です。平面の場合、第一、第二固有値が第三固有値に対して卓越するため、この比を見ることで平面判定できる、ということになります。（第一と第二の比率は見なくてもいいんだっけ？？）しかし、この論文では固有値による判定は外れ値に対して弱いため、用いません。代わりに下記のステップで平面を判定します。
ノード内点群の平均座標と代表法線ベクトルの計算
平均値と書きましたがmedianで求めます。外れ値を考慮しています。単位法線ベクトルも各点の法線ベクトルのmedianで求めます。こちらも外れ値を考慮した結果です。それぞれ$ C $,$ N $とします。この段階では、平面かどうかは気にしていませんが、$C$を通る法線$N$の平面を仮定することはできますので、この平面にノード内の点が実際乗っているかどうかを見ていくことになります。
Plane-sample distance test
まず始めのテストでは、検出した（$ C $と$ N $から定義できる）平面と各点の距離を調べます。$ C $とその法線$ N $を使って、各点$ P_i $から平面への距離$ d_i $は次のように計算できます。
$$ d_i = |(P_i-C) \cdot N| $$
内積を使った距離計算です。シンプルです。この$d_i$の集合$D$を使って外れ値を除くための有効なサンプルの区間$I_D$を作ることができます。
$$ I_D = [median(D)-\alpha MAD(D); median(D)+\alpha MAD(D)] $$
ここで$MAD$はmedian of absolute distanceで
$$MAD = k * median(|d_i-median(D)|) $$
となります。メディアンの嵐。kは正規分布だと$k=1.4826$になるようです。
メディアンになっていますが、$MAD$の意味としては標準偏差と同じです。外れ値の定義のために通常よく使われるのが3シグマで正規分布の場合99.7%のサンプルが3シグマに含まれます。ここでもそれを踏襲して$\alpha=3$としています。
この区間の上限をmaximum distance to plane (MDP)とこの論文では定義します。これ以上離れた点は、平面には属さないことになります。
MDPが小さいということは$d_i$のばらつきが小さいということになりますから、それは平面といっても良さそうです。一方でこのMDP、検査しているノードの大きさによる影響を受けそうです。極端な話1m幅のノードに対して、1㎝くらいのMDPであれば平面とみなせそうですが、同じ1㎝のMDPでも1㎝幅のノードだと…ですよね。ですから、ノード（検出した平面）の大きさによってMDPを正規化してあげることでフェアな条件で平面判定できることになります。それを表しているかというとちょっと違う気もするのですが下図です（論文から引用）。
$C$を通って$N$に直交する平面内のベクトル$U,V$を考えると平面パッチの幅、長さが得られますが、そのうち大きい方のベクトル（の2分の1）と$MDP\cdot N$の和でベクトル$F$が定義できます。この$F$と$N$のなす角$\theta$を平面判定の基準とします。$\theta$が大きいほど平面、ということになります。自分的にはこの定義わかりづらいんですが、$atan(ノードの長さ/ノードの厚み)$と考えると意味は同じですがしっくりきます。
実験的に$\theta > 75\deg$をしきい値としたということです。
Plane-sample normal deviation test
さらに条件を付加してより適切な平面検出を行います。2つ目の条件は、1つ目よりグッとわかりやすくなり、法線のばらつきに関するものになります。
各点において法線ベクトル$N_i$が得られますが、これを前節で得られた法線の代表値$N$と比較するとその角度差$\phi_i$は次のように表すことができます。
$$ \phi_i = acos(|N_i\cdot N|) $$
この$\phi_i$の集合から$\Phi$が得られます。前節での$d_i$と完全に同じように
$$I_\Phi = [median(\Phi)-\alpha MAD(\Phi); median(\Phi)+\alpha MAD(\Phi)]$$
という区間を考えてこの上限をmaximum normal deviation (MND)とします。またしても$\alpha = 3$です。実験から$MND < 60 \deg$が適切とのことです。
Outlier percentage test
すべてのパッチにおいて、前節の区間$I_D$と$I_\Phi$に含まれるない点が全体の25%以上であればそのパッチはnoisyなパッチと認定され、平面ではない、という判定となります。
以上が平面ノード（パッチ）判定の条件となります。私は勘違いしていましたが、あくまで平面を含むノードを判定しているのであって、そこに含まれる1点1点を判定しているわけではないことに注意してください。
grow
ここまではあくまでノードごとの平面判定なので、ノード内のどの点が平面に属しているか、またはいないかの判定が必要です。このためにgrowプロセスを行います。
検出された平面パッチは次の3つの属性を持ちます。
- $C_i, N_i$
- $MDP_i$ (maximum distance to the plane)
- $MND_i$ (maximum normal deviation)
growプロセスでは、点群をグラフ$G$で表しこれを利用します。グラフの各頂点は点群の各点、エッジは、隣接する点を結んでおり、k最近傍法で検出します（$k=50$）。
G内のパッチ$P_i$に属する点を初期シードとして幅優先探索を行います。探索した頂点（点）は
- 他のパッチに属していない
- inlier conditionを満たす
場合に$P_i$に取り込まれます。
inlier condition
次の2つの条件から成ります。対象としている点を$j$で表すと、平面との距離に関して
$$ d_j = |(P_j-C_i)\cdot N_i| < MDP_i $$
を満たす。
$$ \phi_j = acos(|N_j\cdot N_i|) < MND_i $$
一つのサンプルは複数の平面パッチに属する場合があり得ますので、探索する平面パッチに優先度を付けます。優先度はその平面パッチの$MND$を使い、昇順に並べて順番とするようです。この順番付けをすることで、よりノイズの少ない平面からgrowできるとのこと。
このgrowプロセスによって、平面判定されたノードに属している平面上の点が拾われるのはもちろん、平面判定から漏れてしまったノードに属しているけど本当はとなりのノードの平面に属している点も拾われます。
merge
隣接するノードで平面判定となった場合、それらは同一の平面である可能性があります。その場合、mergeプロセスによりマージする必要があります。mergeするには次の3つの条件を満たす必要があります。
- 隣接している
- それぞれのパッチの法線ベクトルが近しい（$ acos(N_A\cdot N_B) < max(MND_A,MND_B)$で判定）
- 少なくともパッチAに属する一つの点において、パッチBのinlier conditionを満たす。逆も同じ
3つめの条件は平行だけど距離のある2平面をマージしないための措置、ということです。
一通りmergeしたあと、元のサンプル数から50%以上サンプル数が増えたパッチについては、$D$と$\Phi$を更新し、$MND$や$MDP$も更新されます。更新したらもう一度grow,mergeプロセスを回します。これをひたすら繰り返し、glow,mergeする対象がなくなったら終了です。
関数のプロパティの解説
以上のアルゴリズムを理解すれば、MATLAB関数のプロパティについても明快になります。というかそのままです。
| プロパティ名 | アルゴリズムで言うと・・・ |
|---|---|
| MaxAngularDistance | $MND$のしきい値(デフォルト：60度） |
| MaxCoplanarDistance | $\theta$のしきい値（デフォルト：75度） |
| ReferenceVector | 検出した平面に設ける向きの制約 |
| MaxReferenceDeviation | ReferenceVectorからの許容誤差 |
| OutlierRatio | Outlier Percentage（デフォルト：0.25） |
| MinNumPoints | 説明そのまま |
| MinDimensionSize | 説明そのまま |
テスト
実際にプロパティの値を変えて、理解が正しいか検証してみます。ここではあくまで定性的な検証とします。公式ドキュメントの例をそのまま使ってみます。
%% Segment Planar Surfaces from Point Cloud
% Load an unorganized point cloud.
filepath = fullfile(toolboxdir("lidar"),"lidardata","lcc","bboxGT.mat");
load(filepath,"pc");
%%
% Estimate the normal vectors by using the <docid:vision_ref#buwsa5o-1 pcnormals>
% function.
numOfNeighbors = 50;
normals = pcnormals(pc,numOfNeighbors);
pc.Normal = normals;
%%
% Segment point cloud into planar surfaces.
[labels,numPlanes,planeNormals] = pcsegplanes(pc);
%%
% Display the segmented point cloud.
figure
pcshow(pc.Location,labels)
colormap(jet(double(numPlanes)))
title("Segmented Point Cloud")
ここでは、プロパティを一切指定していないので、すべてデフォルト値になっています。
ではパラメータを変えてみましょう
MaxAngularDistanceを変えてみる
MaxAngularDistance を小さくすると厳しく、大きくすると緩い条件になるはずで、それぞれ検出される平面は減る、増えるはずです。
やってみましょう。
[labels,numPlanes,planeNormals] = pcsegplanes(pc,'MaxAngularDistance',45);
pcshow(pc.Location,labels)
colormap(jet(double(numPlanes)));
[labels,numPlanes,planeNormals] = pcsegplanes(pc,'MaxAngularDistance',80);
pcshow(pc.Location,labels)
colormap(jet(double(numPlanes)))
次を使用中のエラー: pcsegplanes>validateAndParseInputs (行 63)
'MaxAngularDistance' 引数の値が無効です。 MaxAngularDistance は、値が <= 60 のスカラーでなければなりません。
エラー: pcsegplanes (行 13)
minDimensionSize] = validateAndParseInputs(ptCloudIn, varargin{:});
なぬ！60度以下じゃないといけないんですね。
いったん45°の結果を見てみると、正直60度との違いが判りません。割合きれいに平面なオブジェクトが多いので、45度でもまだまだ検出できてしまうのでしょうか。
さらに絞っていきます。
[labels,numPlanes,planeNormals] = pcsegplanes(pc,'MaxAngularDistance',20);
pcshow(pc.Location,labels)
colormap(jet(double(numPlanes)));
カラーマップをきちんと見ていませんでしたが、暗い青が平面として検出できなかった点を表すようです。左上の建物で検出漏れが多発していたり、左下の地面がごそっと検出から漏れてしまっています。想定通りの結果ですね！
MaxCoplanarDistance
当初意味不明だったこのパラメータも怖くありません。こちらは値を大きくすると厳しい判定になっていくはずです。まずは緩くして過検出にしてみましょう。
[labels,numPlanes,planeNormals] = pcsegplanes(pc,'MaxCoplanarDistance',45);
pcshow(pc.Location,labels)
colormap(jet(double(numPlanes)))
次を使用中のエラー: pcsegplanes>validateAndParseInputs (行 64)
'MaxCoplanarDistance' 引数の値が無効です。 MaxCoplanarDistance は、値が >= 75 のスカラーでなければなりません。
エラー: pcsegplanes (行 13)
minDimensionSize] = validateAndParseInputs(ptCloudIn, varargin{:});
な、なんとこちらも過検出の方向には振れないようになってるんですね。
では、厳しくする方向を見てみます。
[labels,numPlanes,planeNormals] = pcsegplanes(pc,'MaxCoplanarDistance',80);
pcshow(pc.Location,labels)
colormap(jet(double(numPlanes)))
ここでも左下の地面が検出漏れしてしまいました。この地面の凹凸が大きいのでしょう。
さらに厳しくしてみます。85度これはかなり厳しいはず！
(コード略)
だいぶ漏れてますね。想定通り！
Outlier ratio
こちらも見ておきましょう。
outlier ratioを小さくすると、少しの外れ値があっても平面判定から弾かれることになるので条件は厳しくなり検出漏れが多くなるはずです。
[labels,numPlanes,planeNormals] = pcsegplanes(pc,'OutlierRatio',0.1);
pcshow(pc.Location,labels)
colormap(jet(double(numPlanes)))
・・・ちがいがわからない。
思い切って0.01にしてみた結果も変わらずです。
しっかり理解したと思ったのによくわからなくなった…。
こちら社内の開発に聞いてみたところ、、、InlierRatioになっていた、というオチでした。正直に言います。個人ブログなのですが申し訳ありません。じき修正されるようですが、今の段階ではそのように理解ください。
では、値を大きくすれば条件が厳しくなるといことになるので0.9でやってみましょう。
[labels,numPlanes,planeNormals] = pcsegplanes(pc,'OutlierRatio',0.9);
pcshow(pc.Location,labels)
colormap(jet(double(numPlanes)))
お～、たしかに検出されている平面が減っていますね。
終わりに
今回はまじめに一つの関数の使い方を理解して解説しました。その中で不具合も見つけてしまいました…。
論文を読んできちんとアルゴリズムを理解するというのはやはりいつの時代も大切なことだと思います、ツールを使いこなせるように。