Open3d实现点云匹配ICP(代码)

最新推荐文章于 2024-05-20 19:08:38 发布

Vertira

最新推荐文章于 2024-05-20 19:08:38 发布

阅读量689

点赞数 5

分类专栏： open3D 文章标签：人工智能 3d

本文链接：https://blog.csdn.net/Vertira/article/details/136018488

版权

open3D 专栏收录该内容

7 篇文章 1 订阅

订阅专栏

1 点云配准原理

配准原理

点云配准本质上是将点云从一个坐标系变换到另一个坐标系。点云配准通常会需要用到两个点云数据。第一类点云数据称为原始点云，用S（source）来表示。第二类点云数据称为目标点云，用T（Target）来表示。配准就是希望这里的原始数据可以对其目标数据的坐标系，让原始点云S在目标点云T的坐标上进行显示。以实现数据的补充。我们可以通过找到点云中具有相似特征的点云来确定坐标的变换关系。例如，同一个物体的点云同时出现在原始点云和目标点云中，并且在两个点云中有特征相似的部分点云，根据这些相似的点云信息来计算出变换关系。

假设原始点云到目标点云发生的是刚体变换，即原始点云通过旋转和平移即可得到目标点云。这里的旋转和平移过程用旋转变换矩阵R和平移变换矩阵T来表示。我们用P(S)表示原始点云中的点，P(T)表示原始点云在目标点云坐标系中的点。那么这种变换关系可以表示为：

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

迭代最近点算法(Iterative Closest Point, ICP)

第一步：初始化R、T矩阵，根据R、T矩阵可以得到P(T)，即原始点云在目标点云坐标系下的坐标。

第二步：在目标点云中寻找与P(T)最近的点，并且距离小于规定的阈值，这个阈值可以自己定义。

第三步：对第二步中匹配到的点计算欧式距离误差，并且通过最小二乘法来优化R、T矩阵。

第四步：将第三步优化后的R、T矩阵带回第一步中，重新进行迭代，直到迭代满足要求后，得到最终优化的R、T矩阵。

ICP方法分类

ICP方法可分为点到点（PointToPoint）和点到平面（PointToPlane）两类。

1）PointToPoint：计算P(t)和目标点云T的距离采用点到点之间的距离形式。

2）PointToPlane：计算P(t)中点到目标点云T的点所在平面的距离，这里通常需要用到目标点云的法向量。

import open3d
import numpy as np
import mayavi.mlab as mlab
# 对两个ply格式的点云数据进行配准
def open3d_registration():
    ply_path = r"E:\xxxx\reconstruction\ipper_res2.ply"
    ply = open3d.io.read_triangle_mesh(ply_path)
    point_s1 = np.array(ply.vertices)
    ply_path = r"E:\xxxxx\reconstruction\er_res3.ply"
    ply = open3d.io.read_triangle_mesh(ply_path)
    point_s2 = np.array(ply.vertices)
    print(point_s1.shape, point_s2.shape)
    source = open3d.geometry.PointCloud()
    source.points = open3d.utility.Vector3dVector(point_s1)
    target = open3d.geometry.PointCloud()
    target.points = open3d.utility.Vector3dVector(point_s2)
    icp = open3d.pipelines.registration.registration_icp(
        source=source,
        target=target,
        max_correspondence_distance=0.2,    # 距离阈值
        estimation_method=open3d.pipelines.registration.TransformationEstimationPointToPoint()
    )
    print(icp)
    source.transform(icp.transformation)
    points = np.array(source.points)
    x = points[:, 0]
    y = points[:, 1]
    z = points[:, 2]
    fig = mlab.figure(bgcolor=(0, 0, 0), size=(640, 640))
    mlab.points3d(x, y, z, x, mode="point", color=(0, 0, 1), figure=fig)   # 蓝色
    x = point_s1[:, 0]
    y = point_s1[:, 1]
    z = point_s1[:, 2]
    mlab.points3d(x, y, z, x, mode="point", color=(0, 1, 0), figure=fig)   # 绿色
    x = point_s2[:, 0]
    y = point_s2[:, 1]
    z = point_s2[:, 2]
    mlab.points3d(x, y, z, x, mode="point", color=(1, 0, 0), figure=fig)   # 红色
    mlab.show()
if __name__ == '__main__':
    open3d_registration()