Open3D点云配准

点云配准：
由于设备和空间的限制，我们往往不能一次性完整的获得扫描的场景或者物体
的全部信息，因此，我们需要进行多视角的扫描点云数据，在数据处理中需要将
各片点云数据纳入到一个统一的坐标系中再进行后续的应用。我们将点云数据统
一到一个世界坐标系的过程称之为点云配准或者点云拼接。
点云配准过程，就是求一个两个点云之间的旋转平移矩阵，将源点云(source cloud)变换到目标点云(target cloud)相同的坐标系下。

常见的点云配准算法：
基于点特征的的配准 ICP、Color ICP、Trimed-ICP

import open3d as o3d
import numpy as np

# 获取示例数据
source_cloud = o3d.io.read_point_cloud("cloud_bin_0.pcd")
target_cloud = o3d.io.read_point_cloud("cloud_bin_1.pcd")
source_cloud.paint_uniform_color([1, 0.706, 0])
target_cloud.paint_uniform_color([0, 0.651, 0.929])
threshold = 0.02# RMSE残差阈值，小于该残差阈值，迭代终止

#初始位姿
trans_init = np.asarray([[0.862, 0.011, -0.507, 0.5],
                         [-0.139, 0.967, -0.215, 0.7],
                         [0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]])

# 显示未配准点云
o3d.visualization.draw_geometries([source_cloud, target_cloud],zoom=0.4459,front=[0.9288, -0.2951,0.2242],
lookat=[1.6784, 2.0612, 1.4451],up=[-0.3402, -0.9189, -0.1996])

# 点到点的ICP
result = o3d.pipelines.registration.registration_icp(
        source_cloud, target_cloud, threshold,trans_init,
        o3d.pipelines.registration.TransformationEstimationPointToPoint())
print(result)
print("Transformation is:")
print(result.transformation)

# 重置点到点的配准结果
source_cloud = o3d.io.read_point_cloud("cloud_bin_0.pcd")
target_cloud = o3d.io.read_point_cloud("cloud_bin_1.pcd")
source_cloud.paint_uniform_color([1, 0.706, 0])
target_cloud.paint_uniform_color([0, 0.651, 0.929])

# 点到面的ICP
result = o3d.pipelines.registration.registration_icp(
    source_cloud, target_cloud, threshold,trans_init,
    o3d.pipelines.registration.TransformationEstimationPointToPlane())
print(result)
print("Transformation is:")
print(result.transformation, "\n")