教程：Python Open3d 完成 ICP 点云配准

Python Open3d 完成 ICP 点云配准

---

关于Open3d

Open3D 是一个在Python和C++平台上的三维数据处理与可视化库。它由 Qian-Yi Zhou，Jaesik Park， 以及 Vladlen Koltun 共同完成。其中 Zhou 博士在中国清华大学取得硕士学位，并分别在 USC 和Stanford 取得了博士以及博士后学位，目前在在旧金山的 Forma 公司担任首席研发官。Open3D目前支持 Windows, OSX, 和 Linux 平台。

三维点云配准配准

完成配准的演示：

读取数据

这里是本次配准用到的数据，可以下载之后跟着我使用：点云数据
为了完成点云配准，我们需要做的第一步当然是读入我们需要进行配准的两个点云，我们新建一个python文件，将它命名为 display_pcd.py, 并输入以下代码：

import open3d as o3d

#读取电脑中的 ply 点云文件
source = o3d.read_point_cloud("plys/6.ply")  #source 为需要配准的点云
target = o3d.read_point_cloud("plys/0.ply")  #target 为目标点云

#为两个点云上上不同的颜色
source.paint_uniform_color([1, 0.706, 0])    #source 为黄色
target.paint_uniform_color([0, 0.651, 0.929])#target 为蓝色

#创建一个 o3d.visualizer class
vis = o3d.visualization.Visualizer()
vis.create_window()

#将两个点云放入visualizer
vis.add_geometry(source)
vis.add_geometry(target)

#让visualizer渲染点云
vis.update_geometry()
vis.poll_events()
vis.update_renderer()

vis.run()

在以上的代码中，o3d.visualization.visualizer 是一个非常常用的简单的查看点云的类，它接受的输入参数是一个list，这个list里面需要包括你想查看的点云。

运行以上代码，我们得到配准前的两个点云：

每次visualizer 进行渲染都会根据点云的大小和位置自动初始化一个视角，上面一张图是它自己初始化的，我们通过滚动鼠标放大之后即可以看到下面的图：

所以这里需要大家自己用鼠标将视角调到一个自己觉得舒适的地方，然后在屏幕中按下 crtl + c， 这样open3d就会保存下你刚刚的视角。当你重新进入visualizer，现实点云的时候，只要按下 ctrl + v 就可以回到之前你保存的视角。

数据处理

在进行正式配准之前我们还需要对点云做以下处理：特例去除 (outlier removal)。 目前大部分深度摄像头所拍摄的点云图都带有噪音，以及不存在的点，大多因为生产误差以及摄像头本身就有的噪声，在配准的时候我们不希望包括这些因为误差被记录的点，所以为了提高配准的效率以及准确率，我们要先将这些特例点去除。

我们打开一个新的python文件，将它命名为 outlier_removal.py，并输入以下代码：

import open3d as o3d

#读取电脑中的 ply 点云文件
source = o3d.read_point_cloud("plys/6.ply")  #source 为需要配准的点云
target = o3d.read_point_cloud("plys/0.ply")  #target 为目标点云

#为两个点云上上不同的颜色
source.paint_uniform_color([1, 0.706, 0])    #source 为黄色
target.paint_uniform_color([0, 0.651, 0.929])#target 为蓝色

#为两个点云分别进行outlier removal
processed_source, outlier_index = o3d.geometry.radius_outlier_removal(source,
                                              nb_points=16,
                                              radius=0.5)

processed_target, outlier_index = o3d.geometry.radius_outlier_removal(target,
                                              nb_points=16,
                                              radius=0.5)

#o3d.geometry.radius_outlier_removal 这个函数是使用球体判断一个特例的函数，它需要
#两个参数：nb_points 和 radius。 它会给点云中的每个点画一个半径为 radius 的球体，如
#果在这个球体中其他的点的数量小于 nb_points, 这个算法会将这个点判断为特例，并删除。


#显示两个点云
#o3d.visuzlization_draw_geometries 是一个更加简单的显示点云的函数，它不需要创建
#一个visualizer类，直接调用这个函数，在参数里放一个包含你想显示的点云的list就行了。
o3d.visualization.draw_geometries([processed_source,processed_target])

运行以上代码，我们获得以下点云，是不是整洁很多呢？

开始配准

ICP (iterative closest point), 是对点云配准目前最常用的方法。其原理就是不断的对一个点云进行转换，并计算其中每个点与另外一个点云集的距离，将它转换成一个 fitness score。然后不断地变换知道将这个总距离降到最低。一般来说icp都是经过全局配准之后运用的，也就是说两个点云要先被粗略地配准，然后icp会完成配准的微调。

现在我们已经知道如何显示和处理我们的点云了，下一步就是集成我们所学的，并对它们进行配准。我们新创建一个python文件，管他叫做 icp_registration.py，然后输入以下代码。

import open3d as o3d
import numpy as np

#读取电脑中的 ply 点云文件
source = o3d.read_point_cloud("plys/6.ply")  #source 为需要配准的点云
target = o3d.read_point_cloud("plys/0.ply")  #target 为目标点云

#为两个点云上上不同的颜色
source.paint_uniform_color([1, 0.706, 0])    #source 为黄色
target.paint_uniform_color([0, 0.651, 0.929])#target 为蓝色

#为两个点云分别进行outlier removal
processed_source, outlier_index = o3d.geometry.radius_outlier_removal(source,
                                              nb_points=16,
                                              radius=0.5)

processed_target, outlier_index = o3d.geometry.radius_outlier_removal(target,
                                              nb_points=16,
                                              radius=0.5)
threshold = 1.0  #移动范围的阀值
trans_init = np.asarray([[1,0,0,0],   # 4x4 identity matrix，这是一个转换矩阵，
                         [0,1,0,0],   # 象征着没有任何位移，没有任何旋转，我们输入
                         [0,0,1,0],   # 这个矩阵为初始变换
                         [0,0,0,1]])

#运行icp
reg_p2p = o3d.registration.registration_icp(
        processed_source, processed_target, threshold, trans_init,
        o3d.registration.TransformationEstimationPointToPoint())

#将我们的矩阵依照输出的变换矩阵进行变换
print(reg_p2p)
processed_source.transform(reg_p2p.transformation)

#创建一个 o3d.visualizer class
vis = o3d.visualization.Visualizer()
vis.create_window()

#将两个点云放入visualizer
vis.add_geometry(processed_source)
vis.add_geometry(processed_target)

#让visualizer渲染点云
vis.update_geometry()
vis.poll_events()
vis.update_renderer()

vis.run()

运行以上代码，我们得到

以及

console: registration::RegistrationResult with fitness = 1.000000, inlier_rmse = 0.093900, and correspondence_set size of 12849

在返回的结果中，
fitness 算法对这次配准的打分，
inlier_rmse 表示的是 root of covariance, 也就是所有匹配点之间的距离的总和除以所有点的数量的平方根，可以用以下数学公式表达：
$$rmse=\sqrt{\frac{{\sum }_{i=1}^n{a}_i-b_i}{n}}.$$
其中，a为目标矩阵，b为被配准矩阵，n为b的点数量。
correspondence_size 代表配准后 $点{云}_a$ 与 $点{云}_b$ 中吻合的点的数量。
通过视觉上看我们配准的结果，以及参数上看配准结果，这次的配准还算挺成功的。

---

关于我

好了，我们这次点云配准就完成了，希望朋友们学到了点什么，如果大家对这篇文章有问题或者有和我想说的话欢迎在下方评论，私信我，或者email [email protected]。 谢谢阅读我的文章！