# Open3Dの使い方：簡単にICP

pythonで点群処理できるOpen3Dの探検．Open3Dの使い方：読み込みと表示，点と法線の取得の続き．

stanford bunnyの2つをICPで位置合わせしてみる．

# コード

open3d
```import sys
sys.path.append("../..") # Open3D/build/lib/ へのパス
import numpy as np
import py3d

py3d.draw_geometries([pcd1, pcd2], "bunny 000 and 045", 640, 480)

# paint_uniform_colorと同じだけど自分で色つけてみる
pcd1.colors = py3d.Vector3dVector(np.repeat(np.asarray([[1, 0, 0]]), len(np.asarray(pcd1.points)), axis=0))
pcd2.colors = py3d.Vector3dVector(np.repeat(np.asarray([[0, 0, 1]]), len(np.asarray(pcd2.points)), axis=0))

kdt = py3d.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)
py3d.estimate_normals(pcd1, search_param=kdt)
py3d.estimate_normals(pcd2, search_param=kdt)

py3d.draw_geometries([pcd1, pcd2], "bunny 000 and 045", 640, 480)

th = 0.02
T = np.asarray([[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])

# 初期値の情報
info = py3d.evaluate_registration(pcd1, pcd2,
max_correspondence_distance=th,
transformation=T)
print("correspondences:", np.asarray(info.correspondence_set))
print("fitness: ", info.fitness)
print("RMSE: ", info.inlier_rmse)
print("transformation: ", info.transformation)

info = py3d.registration_icp(pcd1, pcd2,
max_correspondence_distance=th,
init=T,
estimation_method=py3d.TransformationEstimationPointToPoint()
# estimation_method=py3d.TransformationEstimationPointToPlane() # 法線が必要                            )
print("correspondences:", np.asarray(info.correspondence_set))
print("fitness: ", info.fitness)
print("RMSE: ", info.inlier_rmse)
print("transformation: ", info.transformation)

pcd1.transform(info.transformation)

py3d.draw_geometries([pcd1, pcd2], "bunny 000 and 045", 640, 480)
```

# 結果

```correspondences: [[   15  1320]
[   16  1320]
[   17  1320]
...
[40252 38314]
[40253 38314]
[40254 38314]]
fitness:  0.6154113672496025
RMSE:  0.010350160221480371
transformation:  [[1. 0. 0. 0.]
[0. 1. 0. 0.]
[0. 0. 1. 0.]
[0. 0. 0. 1.]]
```
ICP結果
```correspondences: [[    0  1318]
[    1  1319]
[    2  1106]
...
[40253 40074]
[40254 40077]
[40255 40088]]
fitness:  0.9910323926868044
RMSE:  0.0019479950359222648
transformation:  [[ 8.36685395e-01  1.45575710e-02 -5.47490299e-01  3.58575243e-02]
[-1.87362765e-02  9.99822363e-01 -2.04822591e-03 -4.23147885e-04]
[ 5.47363227e-01  1.19716503e-02  8.36809523e-01  3.82771389e-02]
[ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]
```