Open3Dの使い方：FPFH特徴でレジストレーション

pythonで点群処理できる[Open3D](http://www.open3d.org)の探検．

点群特徴FPFHで位置合わせして，ICPで微修正．
- [公式チュートリアル](http://www.open3d.org/docs/tutorial/Advanced/global_registration.html)を見やすくしてみた．
- 数値がハードコードだったので，点群のサイズの1/10を基本にして，他のいろいろなパラメータを決めるようにしてみた．
- メソッド名が長いので，`import as`で短く

Tips:
- ダウンサンプリングのサイズを1/10程度に粗くしないと，RANSACがうまく機能しない．（1/100ぐらい細かくしすぎるとダメ）
- FPFHのサイズは1/10 * 50にしている．大きめのほうが良さそう．（小さすぎると特徴計算に使う点の数が少なくなるから）
- 法線計算は1/10にしているが，あまり影響なし．


# コード

```python:open3d
import sys
sys.path.append("../..") # Open3D/build/lib/ へのパス
import copy
import numpy as np
import py3d
from py3d import registration_ransac_based_on_feature_matching as RANSAC
from py3d import registration_icp as ICP
from py3d import compute_fpfh_feature as FPFH


def show(model, scene, model_to_scene_trans=np.identity(4)):
    model_t = copy.deepcopy(model)
    scene_t = copy.deepcopy(scene)

    model_t.paint_uniform_color([1, 0, 0])
    scene_t.paint_uniform_color([0, 0, 1])

    model_t.transform(model_to_scene_trans)

    py3d.draw_geometries([model_t, scene_t])


model = py3d.read_point_cloud("bun000.ply")
scene = py3d.read_point_cloud("bun045.ply")
## PCLモデルを使うならこちら
#model = py3d.read_point_cloud("milk.pcd")
#scene = py3d.read_point_cloud("milk_cartoon_all_small_clorox.pcd")

# いろいろなサイズの元： model点群の1/10を基本サイズsizeにする
size = np.abs((model.get_max_bound() - model.get_min_bound())).max() / 10
kdt_n = py3d.KDTreeSearchParamHybrid(radius=size, max_nn=50)
kdt_f = py3d.KDTreeSearchParamHybrid(radius=size * 50, max_nn=50)

py3d.estimate_normals(model, kdt_n)
py3d.estimate_normals(scene, kdt_n)
show(model, scene)

# ダウンサンプリング
model_d = py3d.voxel_down_sample(model, size)
scene_d = py3d.voxel_down_sample(scene, size)
py3d.estimate_normals(model_d, kdt_n)
py3d.estimate_normals(scene_d, kdt_n)
show(model_d, scene_d)

# 特徴量計算
model_f = FPFH(model_d, kdt_f)
scene_f = FPFH(scene_d, kdt_f)

# 準備
checker = [py3d.CorrespondenceCheckerBasedOnEdgeLength(0.9),
           py3d.CorrespondenceCheckerBasedOnDistance(size * 2)]

est_ptp = py3d.TransformationEstimationPointToPoint()
est_ptpln = py3d.TransformationEstimationPointToPlane()

criteria = py3d.RANSACConvergenceCriteria(max_iteration=40000,
                                          max_validation=500)
# RANSACマッチング
result1 = RANSAC(model_d, scene_d,
                 model_f, scene_f,
                 max_correspondence_distance=size * 2,
                 estimation_method=est_ptp,
                 ransac_n=4,
                 checkers=checker,
                 criteria=criteria)
show(model_d, scene_d, result1.transformation)

# ICPで微修正
result2 = ICP(model, scene, size, result1.transformation, est_ptpln)
show(model, scene, result2.transformation)
```


# 結果

以下の結果はサイズが1/30のときのもの．（1/10にするともっと粗くなる）

## bunny

元の点群
<img width="451" alt="スクリーンショット 2018-03-26 16.02.28.png" src="https://qiita-image-store.s3.amazonaws.com/0/44835/253eb6ea-241a-0da1-2b91-06b5a3c5239c.png">
ダウンサンプリング後
<img width="504" alt="スクリーンショット 2018-03-26 16.02.33.png" src="https://qiita-image-store.s3.amazonaws.com/0/44835/a4156d5e-71a0-1c23-b3f2-eca2ae5b5865.png">
RANSACマッチング後
<img width="428" alt="スクリーンショット 2018-03-26 16.02.38.png" src="https://qiita-image-store.s3.amazonaws.com/0/44835/f66ffe66-e2bb-92c8-5265-4c599b8d87f8.png">
ICP後
<img width="428" alt="スクリーンショット 2018-03-26 16.02.43.png" src="https://qiita-image-store.s3.amazonaws.com/0/44835/7f517d29-a63b-31cd-998a-cd57cba30de5.png">


## PCLのデーブル・ミルクデータ

元の点群
<img width="509" alt="スクリーンショット 2018-03-26 16.01.20.png" src="https://qiita-image-store.s3.amazonaws.com/0/44835/2345a745-2afb-9649-be98-f85cf4a285a7.png">
ダウンサンプリング後
<img width="655" alt="スクリーンショット 2018-03-26 16.01.30.png" src="https://qiita-image-store.s3.amazonaws.com/0/44835/ebe1eb26-b202-e912-f1bb-ef2c312d6dd3.png">
RANSACマッチング後
<img width="775" alt="スクリーンショット 2018-03-26 16.01.50.png" src="https://qiita-image-store.s3.amazonaws.com/0/44835/46055e7f-e42e-ea93-2359-7aa5768161cf.png">
ICP後
<img width="836" alt="スクリーンショット 2018-03-26 16.02.02.png" src="https://qiita-image-store.s3.amazonaws.com/0/44835/72492fcb-375a-3151-c2f7-52cb842488b4.png">