基于ICP算法

此页主要描述了ICP算法进行鞋子模型点云R矩阵与t向量的计算

简介

本代码实现了输入两个鞋子点云模型，进行ICP算法的迭代，计算出从模型1到模型2的旋转、平移矩阵

代码使用了numpy库进行数值计算，open3d库完成对模型的加载以及对点云的采样

什么是ICP？

ICP（Iterative Closest Point）是一种常见的点云配准算法，用于将多个点云或三维模型对齐到同一坐标系中。

ICP算法的基本思路是通过不断迭代的方式来优化点云之间的配准。算法首先通过一些初步的对齐方式（例如粗略的初始变换或最近点匹配）来估计点云之间的变换关系，然后在此基础上进行迭代。

在每次迭代中，ICP算法会执行以下步骤：

1. 选择一些点作为匹配点对。通常使用最近点匹配算法来找到每个点云中与另一个点云最接近的点。

2. 使用选定的匹配点对来计算点云之间的刚性变换（例如旋转、平移和缩放）。可以使用各种方法来计算变换，例如最小二乘法或SVD（奇异值分解）等。(在本文中使用的是SVD)

3. 应用估计的变换，将一个点云变换到与另一个点云对齐。这意味着每个点在两个点云之间被正确对齐。

4. 重复执行步骤1至3，直到达到一定的迭代次数或满足收敛条件为止。

通过反复迭代，ICP算法可以逐渐改进点云之间的匹配，从而达到更好的配准效果。最终，算法会输出点云之间的最佳匹配变换，使它们在同一坐标系中对齐。

ICP算法广泛应用于各种应用程序中，例如三维重建、机器人导航和医学成像等。由于其简单性和有效性，它已成为三维计算机视觉领域中最常用的配准算法之一。

By:ChatGPT

源码实现

import open3d as o3d
import numpy as np


std_mesh_path = './4425de5e-2c7c-4958-aabd-a52f455b5f62.obj'  # 模版路径
trans_mesh_path = './fc372157-6f0c-4f51-a4de-2751b332eb1d.obj'  # 需要变换路径
show = True  # 显示变换前后模型
output = False  # 保存变换后模型
output_path = './output/fc372157-6f0c-4f51-a4de-2751b332eb1d.obj'
step = 40  # 迭代步数  （测试值40）
number_of_points = 2000  # 在模型中sample的点数量 （测试值2000）


if __name__ == '__main__':
    mesh1 = o3d.io.read_triangle_mesh(std_mesh_path)  # 导入模型
    mesh2 = o3d.io.read_triangle_mesh(trans_mesh_path)  # 导入模型

    mesh1.compute_vertex_normals()
    mesh2.compute_vertex_normals()

    # 均匀采样5000个点转点云 mesh转换成pcd
    pcd1 = mesh1.sample_points_uniformly(number_of_points=number_of_points)
    pcd2 = mesh2.sample_points_uniformly(number_of_points=number_of_points)

    # pcd转换成np数据
    arr1 = np.asarray(pcd1.points)
    arr2 = np.asarray(pcd2.points)

    if show:
        pcd1.paint_uniform_color([1, 0, 0])  # 标准模型显示为红色
        pcd2.paint_uniform_color([0, 1, 0])  # 调整模型指定显示为绿
        o3d.visualization.draw_geometries([pcd1, pcd2])

    # 逐步迭代模型
    for step_i in range(step):
        print("step", step_i)

        centroid1 = arr1.sum(axis=0) / number_of_points
        centroid2 = arr2.sum(axis=0) / number_of_points

        if(step_i < 20):
            pcd2.translate(centroid2 + (centroid1 - centroid2), relative=False)
            arr2 = np.asarray(pcd2.points)
            if output:
                mesh2.translate(centroid2 + (centroid1 - centroid2), relative=False)

        centroid1 = arr1.sum(axis=0) / number_of_points
        centroid2 = arr2.sum(axis=0) / number_of_points

        # 得到去质心坐标
        shoes1_decentroid = arr1 - centroid1
        shoes2_decentroid = arr2 - centroid2

        W = np.zeros(shape=(3, 3))
        # 将目标模型进行排序
        for i in range(number_of_points):
            # 计算出 模型1 中每个点在 模型2 最近的点
            minarg = np.argmin(np.power(np.linalg.norm(shoes2_decentroid-shoes1_decentroid[i], axis=1), 2))  # 得到最近点下标
            W = W + (np.outer(shoes1_decentroid[i], shoes2_decentroid[minarg].T))

        # 对 W 进行SVD分解
        u, sigma, vt = np.linalg.svd(W, full_matrices=0, compute_uv=1)

        rotate_matrix = np.dot(u, vt)  # 旋转矩阵
        transpose_vecter = centroid1 - (np.dot(rotate_matrix, centroid2))  # 平移矩阵

        pcd2.rotate(rotate_matrix)
        pcd2.translate(transpose_vecter, relative=False)

        if output:
            mesh2.rotate(rotate_matrix)
            mesh2.translate(transpose_vecter, relative=False)

    if output:
        o3d.io.write_triangle_mesh(output_path, mesh2)

    if show:
        o3d.visualization.draw_geometries([pcd1, pcd2])

效果

配准前

配准后