目录
一、概述
二、代码实现
2.1关键函数
2.2完整代码
三、实现效果
3.1原始点云
3.2配准后点云
一、概述
在Open3D中，CPD（Coherent Point Drift，一致性点漂移）算法是一种经典的点云配准方法，适用于无序点云的非刚性（非刚体）配准问题。CPD算法通过建模点云之间的概率关系，以最大化对应点对之间的一致性来实现配准。以下是CPD算法的详细原理介绍：
CPD算法通过以下步骤来实现点云的非刚性配准：
1.问题建模：
假设有两个点云：源点云（source）和目标点云（target）。目标是找到一个概率转移矩阵和变换矩阵，将源点云的点映射到目标点云上，同时考虑点之间的一致性和概率分布。
2.概率分布建模：
- 假设每个源点在目标点云上的对应位置服从高斯分布。
- 建立概率转移矩阵P来表示每个源点对目标点的概率。
3.优化目标：
CPD算法通过最大化点云之间的一致性来优化配准结果。具体来说，它最小化源点云与目标点云之间的KL散度（Kullback-Leibler divergence），以确定最佳的变换和对应关系。
4.迭代优化：
CPD算法使用迭代方法来逐步优化变换矩阵和概率转移矩阵。每次迭代包括两个主要步骤：
- E步骤（Expectation）：更新每个源点的对应概率，基于当前的变换矩阵和目标点云的位置。
- M步骤（Maximization）：更新变换矩阵，将源点云映射到目标点云上，最大化对应点对之间的一致性。
5.收敛判据：
CPD算法通常会设定迭代次数或者设定一个收敛准则来终止迭代过程，例如KL散度变化小于某个阈值或者达到最大迭代次数。
二、代码实现
2.1关键函数
def registration_cpd(source, target, tf_type_name='rigid',
w=0.0, maxiter=50, tol=0.001,
callbacks=[], **kargs):
- tf_type_name (str, optional):转换类型(‘刚性’，‘仿射’，‘非刚性’)
- w (float, optional):均匀分布的权重，0 < ’ w ’ < 1。
- maxitr (int, optional):EM算法的最大迭代次数。
- tol (float, optional): 停止迭代的最小容忍偏差
2.2完整代码
import open3d as o3d
import numpy as np
from probreg import cpd
import time
import copy
# --------------------读取点云数据------------------
source = o3d.io.read_point_cloud("..//..//standford_cloud_data//Horse.pcd")
target = o3d.io.read_point_cloud("..//..//standford_cloud_data//Horse_trans.pcd")
source = source.uniform_down_sample(every_k_points=10)
target = target.uniform_down_sample(every_k_points=10)
# 点云上色
source.paint_uniform_color([0, 0, 1])
target.paint_uniform_color([0, 1, 0])
o3d.visualization.draw_geometries([source, target], window_name="点云初始位置",
width=1024, height=768,
left=50, top=50,
mesh_show_back_face=False) # 可视化点云初始位置
# CPD算法进行配准
start = time.time()
tf_param, _, _ = cpd.registration_cpd(source, target, # 源点云和目标点云
tf_type_name='rigid', # 计算变换矩阵的类型('rigid', 'affine', 'nonrigid')
w=0.0, # 均匀分布的权重，0 < ' w ' < 1。
maxiter=50, # EM算法的最大迭代次数。
tol=0.001) # 停止迭代的最小容忍偏差
result = copy.deepcopy(source)
result.points = tf_param.transform(result.points)
print("配准用时： %.3f sec.\n" % (time.time() - start))
# 可视化配准结果
target.paint_uniform_color([1, 0, 0])
result.paint_uniform_color([0, 0, 1])
o3d.visualization.draw_geometries([target, result], window_name="CPD算法配准",
width=1024, height=768,
left=50, top=50,
mesh_show_back_face=False)三、实现效果
3.1原始点云
3.2配准后点云
