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

# はじめに

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

法線ベクトル推定は，[PCL](https://pointclouds.org/)や[Open3D](https://www.open3d.org/)でも用意されている機能ですが，計算自体は基本的な処理の組み合わせだし，自前実装の方が使い勝手も良かろうということで作ってみました．コードは全て[Zeo](https://github.com/mi-lib/zeo)に入っています．

https://pointclouds.org/

https://www.open3d.org/

https://github.com/mi-lib/zeo

# 計算の流れ

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

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

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

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/706455/624a4160-e289-4d38-96b1-2afb83d2f61c.png)

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


# Zeoでの実装

## 近傍点探索

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

https://qiita.com/zhidao/items/57483f0a341ed9b85638

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})$の方程式は，次のように書けます．
```math
\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}$は次のように求まるでしょう．
```math
\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実対称行列の固有値・固有ベクトル計算と点群処理への応用](https://qiita.com/zhidao/items/128c4e3db45c277effb7)」でみた**3次元点群の主成分分析**そのものです．

https://qiita.com/zhidao/items/128c4e3db45c277effb7

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

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

https://qiita.com/zhidao/items/128c4e3db45c277effb7
```C
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に対応する関数は，次のように実装しています．
```C
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](https://graphics.stanford.edu/data/3Dscanrep/)に上記の関数を適用してみました．

![bunny_pcd.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/706455/b1881961-0671-4ded-9585-2ebc1da21508.png)
![bunny_pcd_normal.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/706455/dae2f89e-939f-4347-8c71-b78d79ffb568.png)

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

![bunny_pcd_normal_ear.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/706455/870c78e0-292e-429f-9951-f6fdba3d88c3.png)

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

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