CPD点云配准

一、CPD点云配准

Python

这是github上一位大佬写的Python包，链接：neka-nat/probreg: Python package for point cloud registration using probabilistic model (Coherent Point Drift, GMMReg, SVR, GMMTree, FilterReg, Bayesian CPD) (github.com)你要安装open3d、probreg和cupy

import open3d as o3d
from probreg import cpd
import numpy as np


def view_show(point1, point2):
    # 关键点绿色
    point1.paint_uniform_color([0, 1, 0])
    # 点云红色
    point2.paint_uniform_color([1, 0, 0])
    # 可视化
    vis = o3d.visualization.Visualizer()
    vis.create_window(window_name="CPD", width=1200, height=1200)

    # 设置背景颜色
    vis.get_render_option().background_color = [0, 0, 0]  # 白色背景

    # 添加点云到Visualizer
    vis.add_geometry(point1)
    vis.add_geometry(point2)

    # 运行可视化循环
    vis.run()
    vis.destroy_window()


if __name__ == "__main__":
    # 读取点云文件
    source = o3d.io.read_point_cloud('data/pig_view1.pcd')
    target = o3d.io.read_point_cloud('data/pig_view2.pcd')
    # 去除无效点
    source.remove_non_finite_points()
    target.remove_non_finite_points()
    # 下采样
    source = source.voxel_down_sample(voxel_size=15)
    target = target.voxel_down_sample(voxel_size=15)
    view_show(target, source)
    # CPD配准
    tf_param, _, _ = cpd.registration_cpd(source=source,
                                          target=target,
                                          tf_type_name="rigid",
                                          w=0.0,
                                          maxiter=30,
                                          tol=0.001)
    # 方法一，使用自带方法
    # source.points = tf_param.transform(source.points)

    # 方法二，使用该类的属性
    rotation_matrix = tf_param.rot  # 3x3 numpy格式旋转矩阵
    translation_vector = tf_param.t  # 3x1 numpy格式平移矩阵
    scale_factor = tf_param.scale  # 缩放因子

    # 缩放后的旋转矩阵
    scaled_rotation_matrix = rotation_matrix * scale_factor
    # 创建最终变换矩阵
    transformation_matrix = np.eye(4)  # Create a 4x4 identity matrix
    transformation_matrix[:3, :3] = scaled_rotation_matrix  # Set the top-left to the scaled rotation matrix
    transformation_matrix[:3, 3] = translation_vector  # Set the top-right to the translation vector
    source.transform(transformation_matrix)

    view_show(target, source)

关键代码解析：

from probreg import cpd
import open3d as o3d
tf_param, _, _ = cpd.registration_cpd(source=source,
                                          target=target,
                                          tf_type_name="rigid",
                                          w=0.0,
                                          maxiter=30,
                                          tol=0.001)

source.points = tf_param.transform(source.points)

这段代码是使用 probreg 库中的 CPD（Coherent Point Drift）算法进行点云配准（registration）的过程。点云配准是将两个或多个点云之间进行空间转换，使它们在某种度量下对齐的过程。

让我逐行解析代码并解释每个参数的设置以及可能产生的影响：

from probreg import cpd: 这行代码导入了 probreg 库中的 CPD 模块，使我们可以使用其中的函数和类。
import open3d as o3d: 这行代码导入了 Open3D 库，并将其重命名为 o3d。Open3D 是一个用于处理 3D 数据的现代库，其中包含了许多用于点云处理和可视化的功能。
tf_param, _, _ = cpd.registration_cpd(source=source, target=target, tf_type_name="rigid", w=0.0, maxiter=30, tol=0.001): 这行代码调用了 cpd.registration_cpd 函数，执行 CPD 点云配准。参数如下：
- source: 源点云，即需要配准的原始点云。
- target: 目标点云，即源点云需要配准到的目标点云。
- tf_type_name="rigid": 指定了配准的变换类型。在这里，设置为 "rigid" 表示使用刚体变换，即平移和旋转。
- w=0.0: CPD 算法的权重参数，控制了两个点云之间的对应关系。当 w 设置为 0 时，表示不考虑点云之间的对应关系，仅通过点云的空间分布来进行配准。
- maxiter=30: CPD 算法的最大迭代次数，即算法将尝试寻找最佳解的最大次数。
- tol=0.001: CPD 算法的收敛容差，当迭代过程中的变化小于此值时，算法将停止迭代。
函数的返回值 tf_param 包含了配准后的变换参数。
source.points = tf_param.transform(source.points): 这行代码将源点云根据配准得到的变换参数进行变换，从而得到最终的配准结果。这里假设 source.points 是一个 numpy 数组，表示源点云的点坐标。通过 tf_param.transform 方法，将源点云进行变换，使其与目标点云对齐。

以上是代码的解析和参数设置的影响。根据具体的应用场景和需求，您可以调整参数来获得更好的配准结果。例如，通过调整 maxiter 和 tol 参数可以控制算法的精度和速度；通过调整 w 参数可以控制对点云之间对应关系的考虑程度。

    rotation_matrix = tf_param.rot  # 3x3 numpy格式旋转矩阵
    translation_vector = tf_param.t  # 3x1 numpy格式平移矩阵
    scale_factor = tf_param.scale  # 缩放因子

    # 缩放后的旋转矩阵
    scaled_rotation_matrix = rotation_matrix * scale_factor
    # 创建最终变换矩阵
    transformation_matrix = np.eye(4)  # Create a 4x4 identity matrix
    transformation_matrix[:3, :3] = scaled_rotation_matrix  # Set the top-left to the scaled rotation matrix
    transformation_matrix[:3, 3] = translation_vector  # Set the top-right to the translation vector
    source.transform(transformation_matrix)

这种转换方法是根据其属性来计算变换矩阵实现的，这是源码的截图

这是变换矩阵计算公式

rotation_matrix (旋转矩阵):
- rotation_matrix 是一个3x3的NumPy数组，表示旋转矩阵。通常，旋转矩阵用于描述物体的旋转变换。这个矩阵包含了绕坐标轴的旋转信息。
translation_vector (平移矩阵):
- translation_vector 是一个3x1的NumPy数组，表示平移矩阵。它包含了物体在三个坐标轴上的平移信息。
scale_factor (缩放因子):
- scale_factor 是缩放因子，用于控制物体的缩放比例。它是一个标量值，表示将物体沿着每个坐标轴进行缩放的比例。
scaled_rotation_matrix (缩放后的旋转矩阵):
- scaled_rotation_matrix 是通过将原始旋转矩阵 rotation_matrix 与缩放因子 scale_factor 相乘得到的。这样可以实现在旋转的基础上进行缩放。
transformation_matrix (最终变换矩阵):
- transformation_matrix 是一个4x4的单位矩阵，用于表示仿射变换矩阵。在这个矩阵的左上角（3x3的子矩阵）存放了经过缩放后的旋转矩阵，而在右上角的第1至第3行、第4列存放了平移向量。
source.transform(transformation_matrix):
- 通过调用其 transform 方法，将应用变换矩阵 transformation_matrix 对其进行变换。这会导致源对象相应地发生旋转、缩放和平移。