ポイントクラウドからリアルタイム平面検出した話

# はじめに

平面検出は様々な用途で利用される。
XRのための床面、壁面の検知であったり、地図作成のためであったり、人工物の三次元再構成のためであったり。
最近は、OSSやライブラリ等により簡単に平面検出ができるようになってきている。
しかしながら、インプットが限られていたり、カスタマイズ性を持たせたりしたいなど、自前で実装しなければならない場合もあるのではないだろうか。
今回、3次元ポイントクラウドのみをインプットとし、リアルタイムで平面検出ができる以下の論文を実装してみたので紹介する。

- [Fast plane extraction in organized point clouds using agglomerative hierarchical clustering ](https://ieeexplore.ieee.org/abstract/document/6907776)

# まずは実装結果（撮影はREALSENSE D435）

![1.gif](https://qiita-image-store.s3.amazonaws.com/0/356592/82c9e639-1f06-c229-2554-b89ea56ab1bc.gif)


# 手法説明

論文では、平面検出を以下の3ステップで処理している

1. INITGRAPH  
    平面取得のためのデータ構造構築
1. AHCLUSTER  
    粗い平面検出
1. REFINE  
    粗い平面を精緻化

## 1. INITGRAPH (Graph Initialization)

### 処理概要

探索を簡潔にしたり処理時間を短縮するため、以下の流れでデータ構造を作成する
① 3次元ポイントクラウドを2Dでグルーピング（node化）する
② 欠損や深度が適切に取れなかったnodeを除去する
③ node間の連結関係を取得する

### 処理詳細

① 3次元ポイントクラウドを2Dでグルーピングする

- スマホやセンサーで取得したポイントクラウドを、深度情報を除いた2D空間上に配置する。視点から得た2D画像を扱うイメージでよい
- 一定のピクセル間隔でグルーピングする  
    ※この1つ1つの要素を「node」と呼ぶ。下の画像では10×10でグルーピングしている
    <img src="https://qiita-image-store.s3.amazonaws.com/0/356592/4c4917c7-9abc-8ee6-d3ba-d2881d8ad49e.png" width="480px">


② 欠損や深度が適切に取れなかったnodeを除去する

- 以下の条件に当てはまるnodeは除去する
    - 欠損データを含む
    - node内のポイントクラウドに対し平面近似したときのMSE（Mean Squared Error）が一定以上
        ※平面近似はPCAで行うと簡単
    - node内のある1点に注目した時、周囲4点との奥行きの差が一定以上

    除去された nodeを黒色で表示している
    <img src="https://qiita-image-store.s3.amazonaws.com/0/356592/fdc51f83-8052-f9d0-e96e-1231921b9036.png" width="480px">


③ node間の連結関係を取得する

- node間の初期連結関係を以下の条件を満たせば接続していると判断し取得する
    - 自身を含む上下左右隣接nodeが除去されたnodeではない
    - 上下隣接node間、および左右隣接node間の平面の法線のなす各が一定以内

    連結関係を白線で表示している
    <img src="https://qiita-image-store.s3.amazonaws.com/0/356592/9cc115aa-8f70-844d-b66c-0db27bcd3af8.png" width="480px">



## 2. AHCLUSTER（Agglomerative Hierarchical Clustering）

### 処理概要

① 高速処理のため、前処理でヒープを構築する
② nodeの連結関係に従い、nodeどうしのマージを行う

### 処理詳細

① 高速処理のため、前処理でヒープを構築する

- 平面近似MSEの昇順でnodeのヒープを構築する

② nodeの連結関係に従い、nodeどうしのマージを行う

- 作成したヒープが空になるまで以下の処理を行う
    - ヒープからnodeを取り出す
    - 取り出したnodeと連結しているnodeに対して1つずつマージを試み、マージしたときの平面近似MSEが一番小さいnodeをマージする
       ※連結nodeが1つもないときはマージ失敗としてそのまま以下の処理に進む
    - マージnodeの平面近似MSEが一定以上ならマージ失敗として以下の処理をする
        - マージ前のnodeが一定以上のポイントクラウドを含んでいる場合、マージ前のnodeを **平面として抽出する**
        - マージ前のnodeが持つ連結関係をすべて除去する
        - マージ前のnodeを除去する
    - マージnodeの平面近似MSEが一定以下ならマージ成功として以下の処理をする
        - マージした2つのnodeがそれぞれ持っていた連結関係をマージnodeに引き継ぐ
        - マージ前のnodeをヒープから除く
        - マージnodeをヒープに格納する

    処理後のマージnodeをnode単位で着色して表示している

    <img src="https://qiita-image-store.s3.amazonaws.com/0/356592/afa76a67-fd49-50e4-b1d0-e1010a7a6e3d.png" width="480px">



## 3. REFINE（Segmantation Refinement）

### 処理概要

① 粗く検出した平面のフチを取り除く
② フチ除去後の平面のフチからnodeではなくポイントベースで未検出領域に平面領域を広げていく
③ ポイントベースで更新したnode同士の連結関係から再び「2. AHCLUSTER」を実施し、nodeをマージする

### 処理詳細

① 粗く検出した平面のフチを取り除く

- 2.で抽出した平面すべてに対し、各々以下の処理を行う
    - 抽出平面に含まれている初期node（マージ前node）で平面境界に当たるnode(上下左右全てに隣接nodeを持っている状態ではないnode)を平面から除去する
    - 上記処理後の平面境界nodeを構成しているすべての**ポイント**をキューに追加する
    <img src="https://qiita-image-store.s3.amazonaws.com/0/356592/325e6763-6e24-a95d-91da-ca01a262121d.png" width="480px">


② フチ除去後の平面のフチからnodeではなくポイントベースで未検出領域に平面領域を広げていく

- キューが空になるまで以下の処理を行う
    - キューからポイントを取り出す
    - 取り出したポイントに上下左右隣接するポイントそれぞれに以下の処理を行う
        - 以下の条件をいずれか満たす場合、当ポイントの処理を終了し次のポイントに処理を移す。
            - 取り出したポイントと同一node(k)に属している
            - 同一のrefineポイントクラウド(k)に属している
            - node(k)平面との距離がnode(k)平面のMSE × 9 以上 (なぜ9倍しているかはわかりません。)
        - ポイントが他のrefineポイントクラウド(l)に属している場合
            - node(k)とnode(l)を連結関係とする
            - また、node(k)平面とポイントの距離がnode(l)平面とポイントの距離より小さければ以下の処理を行う
                - ポイントをrefineポイントクラウド(l)から削除し、refineポイントクラウド(k)に追加する
                - キューにポイントを追加する
        - ポイントが他のrefineポイントクラウド(l)に属していない場合
            - ポイントをrefineポイントクラウド(k)に追加する
            - キューにポイントを追加する

③ ポイントベースで更新したnodeどうしの連結関係から再び「2. AHCLUSTER」を実施し、nodeをマージする

- すべてのkに対してrefineポイントクラウド(k)をnode(k)に追加する
    <img src="https://qiita-image-store.s3.amazonaws.com/0/356592/51cb55fa-898f-d052-c629-b4ae85653df0.png" width="480px">


- 更新したnodeと連結状態をに対して再び「2. AHCLUSTER」を実施し、nodeをマージする

<font color="crimson"><b>これで1フレーム分の処理が完了！！</b></font>
    <img src="https://qiita-image-store.s3.amazonaws.com/0/356592/477bcc8b-7cef-a407-99b6-a7cd92ae0040.png" width="480px">


# まとめ

- 論文はkinectを利用しており、カメラ性能に依存したheuristicなパラメータを利用しているがREALSENSE D415でもほぼ同じパラメータ設定で動画くらいの精度で平面検出できた
- outlierにはそれなりに強いが、カメラの性能に依存する（特に遠方）
- CPUのみでリアルタイムに平面検出できる
- RGBを必要としない
- 論文がかなり親切に書いてあるため実装しやすい

# 最後に
ヒトやモノをデータ化＆解析してみたい、という方。
３D技術と深層学習を組み合わせて、何か面白いサービスを作ってみたい！、という方。

弊社では一緒に働いてくれる仲間を大募集しています。

ご興味がある方は下記リンクから是非ご応募ください！
https://about.sapeet.com/recruit/

# 参考
- [Fast plane extraction in organized point clouds using agglomerative hierarchical clustering ](https://ieeexplore.ieee.org/abstract/document/6907776)