OPEN3D学习笔记（三）——KDTree & ICP Registration

KDTree

Build KDTree from point cloud

print("Testing kdtree in open3d ...")
print("Load a point cloud and paint it gray.")
pcd = o3d.io.read_point_cloud("../../TestData/Feature/cloud_bin_0.pcd")
pcd.paint_uniform_color([0.5, 0.5, 0.5])
pcd_tree = o3d.geometry.KDTreeFlann(pcd)

Find neighboring points

print("Paint the 1500th point red.")
pcd.colors[1500] = [1, 0, 0]

# Using search_knn_vector_3d
print("Find its 200 nearest neighbors, paint blue.")
[k, idx, _] = pcd_tree.search_knn_vector_3d(pcd.points[1500], 200)  # 查找索引1500的点的邻近的200个点
np.asarray(pcd.colors)[idx[1:], :] = [0, 0, 1]  # 将这些点变成蓝色

# Using search_radius_vector_3d（RNN）
print("Find its neighbors with distance less than 0.2, paint green.")
[k, idx, _] = pcd_tree.search_radius_vector_3d(pcd.points[1500], 0.2)  # 查找索引1500的点的半径0.2以内的点
np.asarray(pcd.colors)[idx[1:], :] = [0, 1, 0]

# 可视化
print("Visualize the point cloud.")
o3d.visualization.draw_geometries([pcd], zoom=0.5599,
                                  front=[-0.4958, 0.8229, 0.2773],
                                  lookat=[2.1126, 1.0163, -1.8543],
                                  up=[0.1007, -0.2626, 0.9596])

除了上面的两种外，还有一种search_hybrid_vector_3d混合的方法，查找半径内最多的k个点

ICP Registration

可视化配准过程

下面的函数将目标点云和源点云可视化，并通过路线转换对其进行转换。目标点云和源点云分别用青色和黄色绘制。两点云重叠的越紧密，对齐的结果就越好。

def draw_registration_result(source, target, transformation):
    source_temp = copy.deepcopy(source)
    target_temp = copy.deepcopy(target)
    source_temp.paint_uniform_color([1, 0.706, 0])
    target_temp.paint_uniform_color([0, 0.651, 0.929])
    source_temp.transform(transformation)
    o3d.visualization.draw_geometries([source_temp, target_temp],
                                      zoom=0.4459,
                                      front=[0.9288, -0.2951, -0.2242],
                                      lookat=[1.6784, 2.0612, 1.4451],
                                      up=[-0.3402, -0.9189, -0.1996])

input

source = o3d.io.read_point_cloud("../../TestData/ICP/cloud_bin_0.pcd")
target = o3d.io.read_point_cloud("../../TestData/ICP/cloud_bin_1.pcd")
threshold = 0.02  # 阈值
trans_init = np.asarray([[0.862, 0.011, -0.507, 0.5],  # 旋转矩阵4*4
                         [-0.139, 0.967, -0.215, 0.7],
                         [0.487, 0.255, 0.835, -1.4],
                         [0.0, 0.0, 0.0, 1.0]])
draw_registration_result(source, target, trans_init)  # 用自定义的函数实现点云的旋转平移

print("Initial alignment")
evaluation = o3d.registration.evaluate_registration(source, target, threshold, trans_init)  # evaluate_registration计算两个指标
print(evaluation)
'''
Initial alignment
registration::RegistrationResult with fitness=1.747228e-01, inlier_rmse=1.177106e-02, and correspondence_set size of 34741
Access transformation to get result.
'''

Point-to-point ICP

Class TransformationEstimationPointToPoint provides functions to compute the residuals and Jacobian matrices of the point-to-point ICP objective. （TransformationEstimationPointToPoint这个类计算残差和雅可比矩阵）
Function registration_icp takes it as a parameter and runs point-to-point ICP to obtain results.（registration_icp利用上面计算的两个参数，得出配准后的结果）

print("Apply point-to-point ICP")
reg_p2p = o3d.registration.registration_icp(
        source, target, threshold, trans_init,
        o3d.registration.TransformationEstimationPointToPoint())  # 源点云，目标点云，阈值，初始旋转矩阵，TransformationEstimationPointToPoint函数
print(reg_p2p)  # 打印两个指标fitness和inlier_rmse
print("Transformation is:")
print(reg_p2p.transformation)  # 打印旋转矩阵
draw_registration_result(source, target, reg_p2p.transformation)  # 对源点云进行旋转平移，与目标点云配准融合
'''
Apply point-to-point ICP
registration::RegistrationResult with fitness=3.724495e-01, inlier_rmse=7.760179e-03, and correspondence_set size of 74056
Access transformation to get result.
Transformation is:
[[ 0.83924644  0.01006041 -0.54390867  0.64639961]
 [-0.15102344  0.96521988 -0.21491604  0.75166079]
 [ 0.52191123  0.2616952   0.81146378 -1.50303533]
 [ 0.          0.          0.          1.        ]]
'''

reg_p2p = o3d.registration.registration_icp(source, target, threshold, trans_init,
        o3d.registration.TransformationEstimationPointToPoint(),
        o3d.registration.ICPConvergenceCriteria(max_iteration = 2000))  # 如果收敛了就结束，本来默认最多30代结束，这里指定2000代
print(reg_p2p)
print("Transformation is:")
print(reg_p2p.transformation)
draw_registration_result(source, target, reg_p2p.transformation)
'''
registration::RegistrationResult with fitness=6.211230e-01, inlier_rmse=6.583448e-03, and correspondence_set size of 123501  # 可以看到效果比上面的好
Access transformation to get result.
Transformation is:
[[ 0.84024592  0.00687676 -0.54241281  0.6463702 ]
 [-0.14819104  0.96517833 -0.21706206  0.81180074]
 [ 0.52111439  0.26195134  0.81189372 -1.48346821]
 [ 0.          0.          0.          1.        ]]
'''

Point-to-plane ICP

相比之前的，这里添加多一个目标点云p的法线n。这个方法比上面的point-to-point更快收敛
使用不同的参数TransformationEstimationPointToPlane调用registration_icp。在内部，该类实现函数以计算点对面ICP物镜的残差和Jacobian矩阵。

print("Apply point-to-plane ICP")
reg_p2l = o3d.registration.registration_icp(
        source, target, threshold, trans_init,
        o3d.registration.TransformationEstimationPointToPlane())  # 跟上面不同的是传入的函数不同
print(reg_p2l)  # 同样是打印两个指标
print("Transformation is:")
print(reg_p2l.transformation)
draw_registration_result(source, target, reg_p2l.transformation)
'''
Apply point-to-plane ICP
registration::RegistrationResult with fitness=6.209722e-01, inlier_rmse=6.581453e-03, and correspondence_set size of 123471  # 在默认的30代已经可以对齐了
Access transformation to get result.
Transformation is:
[[ 0.84023324  0.00618369 -0.54244126  0.64720943]
 [-0.14752342  0.96523919 -0.21724508  0.81018928]
 [ 0.52132423  0.26174429  0.81182576 -1.48366001]
 [ 0.          0.          0.          1.        ]]
'''