pythonで点群処理できるOpen3Dの探検.Open3Dの使い方:読み込みと表示,点と法線の取得の続き.
stanford bunnyの2つをICPで位置合わせしてみる.
コード
open3d
import sys
sys.path.append("../..") # Open3D/build/lib/ へのパス
import numpy as np
import py3d
pcd1 = py3d.read_point_cloud("bun000.ply")
pcd2 = py3d.read_point_cloud("bun045.ply")
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]]