3次元点群の法線ベクトル推定をフルスクラッチで作ってみた

はじめに

点群に構造を見出すのは，空間認識の主要な機能です．これに必要な基本計算の一つが，それぞれの点の近傍の局所的な曲面形状を推定することで，法線ベクトルはその曲面の1階微分量に相当します．

法線ベクトル推定は，PCLやOpen3Dでも用意されている機能ですが，計算自体は基本的な処理の組み合わせだし，自前実装の方が使い勝手も良かろうということで作ってみました．コードは全てZeoに入っています．

計算の流れ

観測によって，ある点群$\mathcal{P}=\left\{\boldsymbol{p}_{i}\right\}$（$i=1,\cdots,N$）が得られたとします．このような点群は，一般的にはある曲面上に分布しているものと仮定できます（曲面の内部の点を観測できるのは特殊な条件下においてであるため）．
それぞれの点$\boldsymbol{p}_{i}$におけるこの曲面の単位法線ベクトル$\boldsymbol{\nu}_{i}$を推定することが目的です．1点だけから面を推定することはできませんので，その近傍の点を集めて最小二乗法により面をフィッティングする，という方針をとります．

曲面全体を推定するわけではないので，推定された接平面のどちら側が立体の内側/外側なのかは分かりません．この点は注意が必要です．視点の情報も使えば局所的にこれを判定することは可能ですが，本記事ではそのような方法は扱わず，点群のみが与えられるものとします．

計算全体の流れは次のような疑似コードで書けます．

近傍点探索FindVicinityと，点群への平面フィッティングFitPlaneがほぼ全てです．近傍点の数が閾値$N_{\mathrm{Vmin}}$（適当に設定します）に満たない場合には，推定を行わないものとしました．

Zeoでの実装

近傍点探索

別記事「比較，kd-treeとoctree」にまとめているように，近傍点探索にはkd-treeやoctreeが使えます．そこでは，「点群の分布に偏りがある場合はoctreeの方が有利な可能性がある」と書きました．今は点群が元形状の表面上に分布していることを仮定しているので，偏りはあると考えられます．後ほど比較してみることにします．

Zeoの関数としては，zVec3DTreeVicinity()またはzVec3DOctreeNN()を用いることになります．点群がデータ構造zVec3DDataのインスタンスとして与えられていた場合，前者ではzVec3DDataToTree()を使ってzVec3DTree型インスタンスに，後者ではzVec3DDataToOctree()を使ってzVec3DOctree型インスタンスに，それぞれ変換する必要があります．

近傍点群への平面フィッティング

$\mathcal{V}_{i}=\left\{\boldsymbol{p}_{ij}\right\}$（$j=1,\cdots,N_{\mathrm{V}}$）に，点$\boldsymbol{p}_{i}$を通り単位法線ベクトルが$\boldsymbol{\nu}_{i}$である平面をフィッティングします．このような平面$\pi(\boldsymbol{p}_{i},\boldsymbol{\nu}_{i})$の方程式は，次のように書けます．

\pi(\boldsymbol{p}_{i},\boldsymbol{\nu}_{i})=\left\{\boldsymbol{p}\left|\boldsymbol{\nu}_{i}^{\mathrm{T}}(\boldsymbol{p}-\boldsymbol{p}_{i})=0\right.\right\}

したがって最小二乗法を使えば，$\boldsymbol{\nu}_{i}$は次のように求まるでしょう．

\boldsymbol{\nu}_{i}=\mathop{\mathrm{arg~min}}_{\boldsymbol{\nu}}\left\{\left.
\sum_{\boldsymbol{p}_{ij}\in\mathcal{V}_{i}}|\boldsymbol{\nu}^{\mathrm{T}}(\boldsymbol{p}_{ij}-\boldsymbol{p}_{i})|^{2}
\right|\|\boldsymbol{\nu}\|=1
\right\}

これは，別記事「3×3実対称行列の固有値・固有ベクトル計算と点群処理への応用」でみた3次元点群の主成分分析そのものです．

主軸方向は3個あり，しかもそれらは互いに直交するのでした．主成分（点群の分散共分散行列の固有値の$N$倍に相当）が大きいほど，それに対応する主軸に沿った点群の分散が大きいということなので，1番大きい主成分に対応する主軸は点分布が最も広がっている方向，2番目に大きい主成分に対応する主軸は，それに直交するうちで点分布が最も広がっている方向を意味します．点が平面上に分布していると仮定するならば，これらこそがその平面を張る2本の軸となるわけです．したがって，残る最も小さい主成分に対応する主軸方向が，その平面の法線方向ということになります．

法線をとる点の位置は$\boldsymbol{p}_{i}$と決まっていますので，別記事で紹介したzVec3DDataPCA()を使っても良いのですが，これを内部で使って法線ベクトルを求めるzVec3DDataMeanNormal()という関数も用意されています．

zVec3D *zVec3DDataMeanNormal(zVec3DData *data, const zVec3D *center, zVec3D *normal)
{
  double pc[3];
  zVec3D pa[3];
  int i;

  if( !zVec3DDataPCA( data, center, pc, pa ) ) return NULL;
  i = pc[0] < pc[1] ? ( pc[0] < pc[2] ? 0 : 2 ) : ( pc[1] < pc[2] ? 1 : 2 );
  return zVec3DCopy( &pa[i], normal );
}

点群をdata，点$\boldsymbol{p}_{i}$の座標をcenterとしてそれぞれ与えると，normalに単位法線ベクトルが入ります．

処理全体

上記疑似コードのEstimateNormalVectorに対応する関数は，次のように実装しています．

static zVec3D *_zVec3DDataNormalVecFromVicinity(zVec3DData *vicinity, const zVec3D *point, const zVec3D *center, zVec3D *normal)
{
  zVec3D d;

  if( zVec3DDataSize(vicinity) < ZEO_VEC3DDATA_NORMALVEC_NUM_MIN )
    ZRUNWARN( ZEO_WARN_VEC3DDATA_NORMAL_TOOFEWPOINTS );
  if( zVec3DDataSize(vicinity) > ZEO_VEC3DDATA_NORMALVEC_NUM_MAX )
    ZRUNWARN( ZEO_WARN_VEC3DDATA_NORMAL_TOOMANYPOINTS );
  zVec3DDataMeanNormal( vicinity, point, normal );
  zVec3DSub( point, center, &d );
  if( zVec3DInnerProd( normal, &d ) < 0 )
    zVec3DRevDRC( normal ); /* flip the normal vector to be outward (not a strict way) */
  return normal;
}

zVec3DData *zVec3DDataNormalVec_Tree(zVec3DData *pointdata, double radius, zVec3DData *normaldata)
{
  zVec3DTree tree;
  zVec3DData vicinity, *retval = NULL;
  zVec3D *v, normal, center;

  if( !zVec3DDataToTree( pointdata, &tree ) ) goto TERMINATE;
  zVec3DDataBarycenter( pointdata, &center );
  zVec3DDataInitList( normaldata );
  zVec3DDataRewind( pointdata );
  while( ( v = zVec3DDataFetch( pointdata ) ) ){
    zVec3DDataInitAddrList( &vicinity );
    if( !zVec3DTreeVicinity( &tree, v, radius, &vicinity ) ) goto TERMINATE;
    _zVec3DDataNormalVecFromVicinity( &vicinity, v, &center, &normal );
    if( !zVec3DDataAdd( normaldata, &normal ) ) goto TERMINATE;
    zVec3DDataDestroy( &vicinity );
  }
  retval = normaldata;
 TERMINATE:
  zVec3DTreeDestroy( &tree );
  return retval;
}

zVec3DData *zVec3DDataNormalVec_Octree(zVec3DData *pointdata, double radius, zVec3DData *normaldata)
{
  zVec3DOctree octree;
  zAABox3D aabb;
  zVec3DData vicinity, *retval = NULL;
  zVec3D *v, normal, center;

  zVec3DDataAABB( pointdata, &aabb, NULL );
  if( !zVec3DDataToOctree( pointdata, &octree, zAABox3DXMin(&aabb), zAABox3DYMin(&aabb), zAABox3DZMin(&aabb), zAABox3DXMax(&aabb), zAABox3DYMax(&aabb), zAABox3DZMax(&aabb), radius ) )
    goto TERMINATE;
  zVec3DDataBarycenter( pointdata, &center );
  zVec3DDataInitList( normaldata );
  zVec3DDataRewind( pointdata );
  while( ( v = zVec3DDataFetch( pointdata ) ) ){
    zVec3DDataInitAddrList( &vicinity );
    if( !zVec3DOctreeVicinity( &octree, v, radius, &vicinity ) ) goto TERMINATE;
    _zVec3DDataNormalVecFromVicinity( &vicinity, v, &center, &normal );
    if( !zVec3DDataAdd( normaldata, &normal ) ) goto TERMINATE;
    zVec3DDataDestroy( &vicinity );
  }
  retval = normaldata;
 TERMINATE:
  zVec3DOctreeDestroy( &octree );
  return retval;
}

zVec3DData *(* zVec3DDataNormalVec)(zVec3DData*, double, zVec3DData*) = zVec3DDataNormalVec_Octree;

近傍点探索にkd-treeを使うものをzVec3DDataNormalVec_Tree()，octreeを使うものをzVec3DDataNormalVec_Octree()としてそれぞれ実装しました．これらはほとんど同じ骨格を持っていることが，お分かりになるかと思います．
また，どちらを使うか特に意識しない人向けにzVec3DDataNormalVecという関数ポインタも用意しています．上のコードの通り，デフォルトではzVec3DDataNormalVec_Octreeを指すようにしています．
引数pointdataは点群，radiusは各点の近傍を定義する半径であり，推定された法線ベクトル群はnormaldataに保存されます．

一箇所，説明が必要な処理があります．関数_zVec3DDataNormalVecFromVicinity()の中で，zVec3DDataMeanNormal()にて点pointにおける法線ベクトルnormalを推定した後に，点群重心からpointに向かって伸ばしたベクトルとnormalの挟角が90度を超えていた場合には，normalを反転させています．これは簡易的に，点群の重心からpointに向かう方向を曲面の外側と判定するものです．
重心から各点に向かって伸びる半直線がその点以外に曲面と交点を持たない場合には，この判定は正しいことが保証されますが，一般的にはそのようなことは言えません．この影響も後ほど見ることにします．

テスト

例によって，Stanford bunnyに上記の関数を適用してみました．

概ねうまく推定できているように見えます．しかし，耳のまわりをよく見てみると…

ちょっと分かりにくいですが，耳の裏側の点群では，推定された法線が内側を向いてしまっています．前脚の上側の点群でも，同様のことが起こっていました．視点の情報を使わないならば，このような誤判定は免れません．

clock()関数を使って計算時間を簡易計測したところ，kd-tree版zVec3DDataNormalVec_Tree()は414526クロック，octree版zVec3DDataNormalVec_Octree()は240461クロックでした．精度は高くありませんが，少なくともこの例では予想通り，octree版の方がkd-tree版よりも5/3倍程度速かったと言えます．