Python Open3D点云配准点对点,点对面ICP(Iterative Closest Point)

Python Open3D点云配准 ICP(Iterative Closest Point)

这篇博客将介绍 迭代最近点配准算法(Iterative Closest Point, ICP) 。多年来,它一直是研究和工业中几何注册的支柱。输入是两个点云和一个初始变换,该变换大致将源点云与目标点云对齐。输出是一个精确的变换,它将两个点云紧密对齐。

  • 将展示俩种ICP:点对点ICP(PointToPoint)和点对面ICP(PointToPlane)。
  • 函数 draw_registration_result 在icp过程中可视化对齐效果。目标点云和源点云分别用青色和黄色绘制。两个点云彼此重叠得越多越紧密,对齐结果越好。
  • 函数 evaluate_registration 计算两个主要指标:
    • fitness:其测量重叠区域(内部对应的数量/目标中的点的数量),越高越好。
    • inlier_rmse,其测量所有inlier对应的rmse相似性,越低越好。

1. 效果图

原始点云黄色 VS 原始点云青色:
Python Open3D点云配准点对点,点对面ICP(Iterative Closest Point)_第1张图片

初始对齐 Point to Point ICP 匹配74056个点,效果图如下:

Apply point-to-point ICP
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.9
6521988 -0.21491604 0.75166079]
[ 0.52191123 0.2616952 0.81146378 -1.50303533]
[ 0. 0. 0. 1. ]]

Python Open3D点云配准点对点,点对面ICP(Iterative Closest Point)_第2张图片

Point to Plane ICP 匹配123471个点效果图如下:
可以看到点对面ICP更快达到收敛。fitness更高更好,inlier_rmse也小一些。

Apply point-to-plane ICP
RegistrationResult with fitness=6.209722e-01, inlier_rmse=6.581453e-03, and correspondence_set size of 123471
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. ]]

Python Open3D点云配准点对点,点对面ICP(Iterative Closest Point)_第3张图片

Initial alignment
RegistrationResult with fitness=1.747228e-01, inlier_rmse=1.177106e-02, and correspondence_set size of 34741
Access transformation to get result. 

Apply point-to-point ICP
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.9
 6521988 -0.21491604  0.75166079]
 [ 0.52191123  0.2616952   0.81146378 -1.50303533]
 [ 0.          0.          0.          1.        ]] 

Apply point-to-plane ICP
RegistrationResult with fitness=6.209722e-01, inlier_rmse=6.581453e-03, and correspondence_set size of 123471
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.        ]] 

2. 安装及原理

pip install open3d -i http://mirrors.aliyun.com/pypi/simple/

点云配准本质上是将点云从一个坐标系变换到另一个坐标系。通常会需要用到两个点云数据,第一类点云数据称为原始点云,用S(source)来表示,黄色。第二类点云数据称为目标点云,用T(Target)来表示,青色。

点云配准的主要任务是计算出旋转矩阵R和平移矩阵T。

2.1 点对点ICP

通常,ICP算法迭代两个步骤:

  • 查找对应集K={(p,q)} 从目标点云P和用当前变换矩阵T变换的源点云Q中提取并更新转换T
  • 通过最小化在对应集K上定义的目标函数E(T)
    ICP的不同变体使用不同的目标函数E(T):

E ( T ) = ∑ ( p , q ) ϵ κ ∣ ∣ p − T q ∣ ∣ 2 \bold {E(T)=\sum_{(p,q)\epsilon\kappa} \mid\mid p-T_q \mid \mid ^2 } E(T)=(p,q)ϵκpTq2

TransformationEstimationPointToPoint类提供了计算点对点ICP目标的残差和转换矩阵的函数。函数registration_icp将其作为参数并运行点对点icp以获得结果。

fitness分数从0.174723增加到0.372450。inlier_rmse从0.011771减少到0.007760。默认情况下,registration_icp 运行直到收敛或达到最大迭代次数(默认情况下为30)。它可以被改变以允许更多的计算时间并进一步改进结果。

最终对准是紧密的。fitness分数提高到0.621123。inlier_rmse降低到0.006583。

2.2 点对面ICP

点对面ICP使用不同的目标函数E(T):
E ( T ) = ∑ ( p , q ) ϵ κ ( ( p − T q ) ⋅ n p ) 2 \bold {E(T)=\sum_{(p,q)\epsilon\kappa} ((p-T_q) \cdot n_p)^2} E(T)=(p,q)ϵκ((pTq)np)2

其中 n p n_p np 是点p的法线,表明点对平面ICP算法比点对点ICP算法具有更快的收敛速度。
使用不同的参数TransformationEstimationPointToPlane调用registration_icp。在内部,此类实现了计算点到平面ICP目标的残差和转换矩阵的函数。

3. 源码

# 使用官方demo点云对俩个原始点云进行点对点icp,点对面icp配准,并进行可视化,原始点云黄色,目标点云青色。
# python icp_registration.py
import copy

import numpy as np
import open3d as o3d


def draw_registration_result(source, target, transformation, title):
    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([source_temp, target_temp], title="Open3D " + title)


def point_to_point_icp(source, target, threshold, trans_init):
    print("Apply point-to-point ICP")
    reg_p2p = o3d.pipelines.registration.registration_icp(
        source, target, threshold, trans_init,
        o3d.pipelines.registration.TransformationEstimationPointToPoint())
    print(reg_p2p)
    print("Transformation is:")
    print(reg_p2p.transformation, "\n")
    draw_registration_result(source, target, reg_p2p.transformation, 'point-to-point ICP')


def point_to_plane_icp(source, target, threshold, trans_init):
    print("Apply point-to-plane ICP")
    reg_p2l = o3d.pipelines.registration.registration_icp(
        source, target, threshold, trans_init,
        o3d.pipelines.registration.TransformationEstimationPointToPlane())
    print(reg_p2l)
    print("Transformation is:")
    print(reg_p2l.transformation, "\n")
    draw_registration_result(source, target, reg_p2l.transformation, 'point-to-plane ICP')


if __name__ == "__main__":
    # 本地没有会自动下载demo点云数据 https://github.com/isl-org/open3d_downloads/releases/download/20220301-data/DemoICPPointClouds.zip
    pcd_data = o3d.data.DemoICPPointClouds()
    source = o3d.io.read_point_cloud(pcd_data.paths[0])
    target = o3d.io.read_point_cloud(pcd_data.paths[1])
    threshold = 0.02
    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]])
    draw_registration_result(source, target, trans_init, 'origin')

    print("Initial alignment")
    # 函数evaluate_registration计算两个主要指标:
    # fitness:其测量重叠区域(内部对应的数量/目标中的点的数量),越高越好。
    # inlier_rmse,其测量所有inlier对应的rmse相似性,越低越好。
    evaluation = o3d.pipelines.registration.evaluate_registration(
        source, target, threshold, trans_init)
    print(evaluation, "\n")

    point_to_point_icp(source, target, threshold, trans_init)
    point_to_plane_icp(source, target, threshold, trans_init)

参考

  • http://www.open3d.org/docs/release/tutorial/pipelines/icp_registration.html
  • http://www.open3d.org/docs/release/python_example/pipelines/index.html#colored-icp-registration-py
  • https://github.com/seminar2012/Open3D
  • http://www.open3d.org/docs/release/getting_started.html
  • https://blog.csdn.net/suiyingy/article/details/124336448

你可能感兴趣的:(Python,OpenCV,Python-pcl,点云,pcd,las/laz,Open3D,python,open3D,ICP迭代最近点配准,点云点对点配准,点云点对面配准)