Open3D 点云配准-GICP（Generalized ICP）算法配准（精配准）

目录

一、概述

1.1实现步骤

1.2数学描述

二、代码实现

三、实现效果

3.1原始点云

3.2配准后点云

3.3计算数据

---

 Open3D点云算法汇总及实战案例汇总的目录地址：

Open3D点云算法与点云深度学习案例汇总（长期更新）-CSDN博客

---

一、概述

        GICP（Generalized ICP）是一种用于点云配准的改进ICP算法。它综合了点对点ICP和点对面ICP的优点，通过引入协方差矩阵，来更准确地描述点的局部几何特性，从而提高配准精度。GICP主要应用于机器人定位、三维重建和计算机视觉等领域。

        GICP算法将每个点云中的点视为带有协方差的高斯分布，这样可以更准确地描述点的局部几何形状。配准的目标是通过迭代优化，使得源点云与目标点云之间的距离最小化。

1.1实现步骤

1. 初始化：选择一个初始变换（通常是单位变换）将源点云与目标点云进行初步对齐。
2. 协方差矩阵计算：对于源点云和目标点云中的每个点，计算其邻域点的协方差矩阵。协方差矩阵描述了该点的局部几何形状。
3. 最近点配对：对于源点云中的每个点，找到目标点云中的最近点作为对应点。
4. 误差计算：使用基于协方差的马氏距离（Mahalanobis Distance）来计算源点与对应点之间的误差。
5. 最小化误差：通过优化方法最小化误差，获得新的刚体变换（旋转和平移）。
6. 应用变换：对源点云应用计算得到的刚体变换。
7. 迭代：重复步骤3到步骤6，直到误差收敛或达到最大迭代次数。

1.2数学描述

二、代码实现

import open3d as o3d
import numpy as np
import copy


def draw_registration_result(src, tar, transformation):
    source_temp = copy.deepcopy(src)  # 由于函数transformand paint_uniform_color会更改点云，
    target_temp = copy.deepcopy(tar)  # 因此调用copy.deepcoy进行复制并保护原始点云。
    source_temp.paint_uniform_color([1, 0, 0])  # 点云着色
    target_temp.paint_uniform_color([0, 1, 0])
    source_temp.transform(transformation)
    # o3d.io.write_point_cloud("trans_of_source.pcd", source_temp)  # 保存点云
    o3d.visualization.draw_geometries([source_temp, target_temp],
                                      window_name="GICP配准结果", width=800, height=600)


if __name__ == "__main__":
    # --------------------读取点云数据-------------------
    source = o3d.io.read_point_cloud("hand.pcd")
    target = o3d.io.read_point_cloud("hand_trans.pcd")
    # ------------------可视化点云初始位置---------------
    o3d.visualization.draw_geometries([source, target], window_name="原始点云的位置", width=800, height=600)
    threshold = 1  # 距离阈值
    trans_init = np.asarray([[0.98194534, -0.18295687, -0.04806395, 0.65088957],
                             [0.11626176, 0.78413388, -0.60960419, 4.19087836],
                             [0.14921985, 0.59300999, 0.79124749, 0.42555584],
                             [0, 0, 0, 1]])  # 初始变换矩阵，一般由粗配准提供
    # -------------------------------------------------
    # 计算两个重要指标，fitness计算重叠区域（内点对应关系/目标点数）。越高越好。
    # inlier_rmse计算所有内在对应关系的均方根误差RMSE。越低越好。
    print("Initial alignment")
    evaluation = o3d.pipelines.registration.evaluate_registration(source, target, threshold, trans_init)
    print(evaluation)  # 这里输出的是初始位置的 fitness和RMSE
    print("Apply GeneralizedICP")
    generalized_icp = o3d.pipelines.registration.registration_generalized_icp(
        source, target, threshold, trans_init,
        o3d.pipelines.registration.TransformationEstimationForGeneralizedICP(),
        o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=35))  # 设置最大迭代次数
    print(generalized_icp)  # 输出GeneralizedICP相关信息
    print("Transformation is:")
    print(generalized_icp.transformation)  # 输出变换矩阵
    # -----------------可视化配准结果--------------------
    draw_registration_result(source, target, generalized_icp.transformation)

三、实现效果

3.1原始点云

3.2配准后点云

3.3计算数据

Initial alignment
RegistrationResult with fitness=1.000000e+00, inlier_rmse=1.838722e-01, and correspondence_set size of 327323
Access transformation to get result.
Apply GeneralizedICP
RegistrationResult with fitness=1.000000e+00, inlier_rmse=1.661125e-07, and correspondence_set size of 327323
Access transformation to get result.
Transformation is:
[[ 1.00000001e+00  1.60936690e-09  6.03521144e-09 -3.66212183e-08]
 [ 8.59615544e-10  7.07106783e-01 -7.07106785e-01  4.99999999e+00]
 [-1.99964480e-09  7.07106784e-01  7.07106779e-01  1.00000000e+00]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]