点云配准基础知识

引言

什么是点云配准呢，我们分别看下点云和配准

• 点云：你可以想象成 “3D 世界的像素”—— 比如用激光雷达扫描一个桌子，会得到成千上万的 3D 坐标点（每个点有 X、Y、Z 位置信息），这些点凑在一起就形成了桌子的 “点云”，能完整还原桌子的 3D 形状。
• 配准：简单说就是 “把两个不同角度 / 时间扫到的同个物体的点云，对齐成一个完整的 3D 模型”。比如你先扫了桌子的正面（点云 A），再绕到侧面扫了一次（点云 B），这两个点云在电脑里是 “错位” 的；配准的目标就是找到一个 “变换规则”（比如把点云 B 平移 + 旋转一下），让 A 和 B 完美重合，最终得到整个桌子的完整 3D 点云。

还有一个就是刚性变换，它其实就是平移加变换不改变形状，详细一点的解释如下：

• 刚性变换： 只做平移（前后左右移）和旋转（转角度），不会让点云拉伸、压缩或变形（比如桌子的点云不会被拉成长方形），这是 3D 配准里最常用的变换方式，因为现实中物体不会轻易变形。

刚性点云配准

刚性点云配准的主要流程如下：

（1）输入源点云和目标点云；

（2）寻找源点云和目标点云的对应关系；

（3）寻找源点云与目标点云之间的最佳变换；

（4）得到最终的变换矩阵。

其中对应关系的求解与变换矩阵的求解两部分较为重要，接下来进行分别讲述。

对应关系求解

这里以Y为源点云，X为目标点云，他们的最佳对齐可以表示为

g是刚体变换函数，它由一个旋转矩阵R和一个平移变量t组成，g(Y)等同于R * Y + t，其中R∈SO(3)指的意思是三维特殊正交群，这个条件保证了 R 必须是一个合法的旋转矩阵（RRt=1&&det(R)=1），它只改变方向，不改变形状和大小，d是误差函数，表示变换后的g(Y)和X的差异，d通常指的是对应点之间的欧氏距离，最后的||||2²是L2范数的平方,其目标就是让这个总的平方误差最小。最终再使用argmin函数，得到一组使得误差最小的参数，也就是最优的旋转矩阵 R 和平移向量 t。

求解两刚性点云的关系有很多种方法，例如基于迭代的方法、提取特征的方法与匹配对应的方法。

迭代求解方法

刚性点云配准问题中，迭代求解方法主要为ICP算法、NDT算法和CPD算法。

ICP算法

ICP(Iterative Closest Point)，即迭代最近点算法，它的算法流程如下

1、目前并不知道哪种变换方式可以使得源点云的点对应目标点云的点，这里就找最近的建立一个对应关系；

2、基于猜出的关系，计算出最优的变换；

3、应用变换，源点云就离目标点云更为接近；

4、进行阈值检测，满足时停止迭代；

5、在新的位置上继续进行最近点的关系建立，然后再变换，也就是迭代1-3的行为。

具体步骤示例如下：

这里我们设置源点云P={Pi}，目标点云Q={Qi}

接下来执行ICP算法：

一、初始化

选择一个初始的变换估计{R0，t0}（通常没有任何信息的时候，给R0设置单位矩阵，t0=0）

二、迭代

（1）寻找最近关系（最近邻搜索）

对于变换后的Pk=Rk * P + tk中的每一个点Pi，在目标点云Q中找到欧氏距离最近的点qi作为其对应点，此时就建立了临时对应点对集合Ck={（Pi，qi）}

（2）剔除无效点对

为了增加鲁棒性，通常会剔除一些不好的点对，例如距离超过一定阈值的点对（可能是错误的匹配）和互相不是最近邻的点对（双向一致性检查）。

（3）计算最优变换

这里将上一步得到的点对集合 Ck 视为已知的、正确的对应关系。

接着使用封闭解来计算一个使这些对应点对之间距离平方和最小的新变换。

什么是封闭解呢，他它指的是通过一套精确的、有限的数学公式，直接计算出问题的最优解。比如计算二元一次方程可以用求根公式直接求解。

这里也是一样的，我们在得到了一组临时的对应点对后，求解最优变换 (R, t) 的问题就存在一个漂亮的封闭解。

我们要找到一个旋转矩阵R和一个平移向量t，最小化均方误差：

基于SVD的封闭解求解步骤

计算质心，质心就是点云在空间的中心位置

接下来计算去中心化的点集

现在，两个新的点集 {xi}和 {yi} 的质心都在坐标原点，将平移分量 t 从问题中分离了出来，因为这个时候只需要考虑旋转对齐，而不需要考虑平移的事情。

接下来计算最优旋转矩阵R

步骤1：构造协方差矩阵H（3*3矩阵），这个矩阵捕获了两个去中心化点集之间的相关性

步骤2：对矩阵进行奇异值分解，其中U和V是3*3的正交矩阵，Σ是3*3的对角矩阵，对角线上的元素非负

步骤3：计算最优旋转矩阵R，这个解保证了 R 是正交矩阵，但不能保证其行列式为 +1（一个合法的旋转矩阵行列式必须为+1）

步骤4：反射校正，计算det（R）的值，如果为1就按上述公式求出R即可，如果是-1则需要将 V矩阵的最后一列取相反数，得到 V‘，再乘上UT得到R

接下来就是最后一个，求出最优平移向量t，已经有了最佳矩阵，我们求t就很简单了。最优的平移，就是把源点云的质心旋转后，刚好移动到目标点云质心上所需要的那段位移。

为什么是最优的平移，证明如下

NDT算法

NDT（Normal Distributions Transform），正态分布变换，是一种广泛使用的点云配准算法，它的核心思想与ICP截然不同：NDT不直接计算点与点之间的对应关系，而是通过概率模型来描述和匹配点云的表面结构。

NDT算法的基本流程如下：

1、目标点云预处理：将目标点云划分为体素网格，并为每个网格计算正态分布参数

2、初始化变换：设置初始变换估计（通常为单位变换或粗略对齐）

3、 迭代优化：计算梯度与海森矩阵，求解更新量，更新变换

4、收敛判断 ：检查梯度大小、参数变化或迭代次数，满足则停止

5、输出结果：返回最优变换矩阵

算法具体步骤如下：

步骤一：预处理目标点云（构建NDT表示）

这步操作使得目标点云将不再被当作离散的点，而是被描述为一个概率场。

（1）体素网格划分

首先将目标点云所在的空间划分为一个个均匀的体素（实际上就是立方体网格）

（2）计算概率分布

对于每一个网络，搜集在其内部的所有目标点，假设这些点都服从一个三维正态分布

计算均值向量μ，其用于描述点的平均位置

计算协方差矩阵 Σ，它用于描述点的分散情况（形状、朝向）

这样，每个体素就可以用一个正态分布N(μ,Σ) 来进行拟合。

步骤二：定义优化目标函数

现在我们要找一个变换T（包含旋转和平移），将其应用于源点云P，使得变换后的源点“落入”目标点云概率场的可能性最大。

其实也就是P'=T(P)

（1）计算似然

对于一个变换后的源点P'=T(P)，我们找到它所在的网格，然后计算它在这个网格的正态分布下的概率密度

（2）构建目标函数

NDT的目标是最大化所有变换后的源点概率密度的乘积。

为了计算方便，我们通常最大化其对数似然

最终的评分函数是所有点得分的总和：

步骤三：优化变换函数

目标是找到变换 T 使得评分函数 E(T) 最小化，这是一个标准的非线性优化问题。通常采用牛顿法进行迭代优化，需要计算评分函数对变换参数 T 的梯度和海森矩阵 。在3D空间中，T通常由6个参数表示

通常使用牛顿法进行优化，通过迭代逼近最优解：

H是Hessian矩阵（二阶导数），∇E是梯度（一阶导数）

（1）计算梯度

以X为例的话进行计算如下

（2）计算海森矩阵

（3）求解线性系统

每次迭代需要求解

通过这个来算出来更新量Δx。

CPD算法

CPD算法是基于概率模型的点云配准方法。它通过计算点云之间的概率密度函数来估计它们的对应关系。

算法的基本流程如下：

1. 初始化：选择两个点云，并计算它们的概率密度函数；
2. 迭代：在每一迭代中，根据概率密度函数计算两个点云之间的对应关系；
3. 更新：根据对应关系更新点云的位置，重复迭代直到满足终止条件。

具体步骤如下：

CPD算法将点集配准问题转换为概率密度估计问题，其将点集M的分布表示成混合高斯模型，当点集M与点集S完成配准后，对应的似然函数达到最大。

一、分布模型表示

将点集M中每个点mi作为混合高斯模型中每个成分的中心，并且假设每个成分概率相等，那么其分布模型可以表示为：

其中

考虑噪声的影响，在分布模型中又加入了均匀分布函数，其权重为w，这样上式分布模型可以进一步表示为

混合高斯模型的中心在配准过程中与变换参数θ相关，为了求得模型参数，需要极小化负对数似然函数：

二、求解θ和σ2

过程可以使用期望最大算法，算法包含两步：

（1）E-Step：通过旧的分布模型参数计算后验概率分布Pold(i|sj)

M-Step：极小化损失函数来得到新的模型参数θ和σ2

特性	ICP	NDT	CPD
全名	Iterative Closest Point	Normal Distributions Transform	Coherent Point Drift
核心思想	最近邻点对匹配 + SVD求解	概率分布匹配 + 牛顿法优化	高斯混合模型 + EM算法
理论基础	几何优化	概率密度估计	统计概率模型
对应关系	硬对应（一对一）	隐式对应（分布匹配）	软对应（概率分配）
收敛速度	快	中等	慢
优点	• 实现简单 • 计算高效 • 内存消耗小 • 收敛快	• 鲁棒性强 • 不需要点对点对应 • 收敛域较大 • 效率较高	• 最强的鲁棒性 • 概率对应关系 • 支持非刚性变换 • 全局收敛性
缺点	• 对初始位置敏感 • 对噪声敏感 • 需要高重叠度 • 对应关系脆弱	• 网格大小敏感 • 分布假设限制 • 对空旷区域敏感 • 数学复杂	• 计算复杂度高 • 收敛速度慢 • 参数调节复杂 • 内存消耗大
适用场景	• 刚体变换 • 良好初始位置 • 高重叠度 • 实时应用	• 激光SLAM • 自动驾驶 • 室外大场景 • 噪声较多情况	• 医学图像 • 生物特征识别 • 非刚性配准 • 高精度要求场景
时间复杂度	O(NlogN)	O(N)	O(NM)
内存消耗	低	中	高
鲁棒性	低	中	高
实现难度	简单	中等	困难

特征提取方法

点线面

基于点的采样方法

随机采样法：其核心就是随机性，他的思想是纯粹通过随机性，从原始点云中抽取一个子集，而不考虑任何几何信息。

其流程可以简单的描述为以下：

1. 确定采样数量：用户指定希望保留的点数 M，或者指定一个采样比率（如 10%）。
2. 随机选择：从原始点云的 N 个点中，无放回地随机选取 M 个点。
3. 形成新点云：将这 M 个被选中的点组成一个新的点云。

因为这个方法是没有考虑几何信息，可能位置分布十分不均匀，所以这里接着提出了最远点采样的方法，保证分布均匀。

最远点采样：是一种旨在最大化点云空间覆盖的采样方法，它的核心思想是：每次选择的点都距离当前已选点集最远，从而保证采样点在空间中尽可能均匀分布。

其算法流程如下：

1、初始化：
假定要取M个点的子集，从点云中随机选定一个点，加入采样集合S，初始化一个距离数组D，长度为N，记录每个点到采样集合S的最近距离。初始时，S 中只有一个点，所以 D 记录每个点到这个起始点的距离。

2、迭代采样

对于接下来的M-1次迭代，每次都进行以下操作：

（1）寻找最近点：在距离数组D中，找到值最大的点，这个点就是当前距离已选点集 S 最远的点。

（2）更新集合：将该点加入采样集合 S。

（3）计算所有点到这个新加入点的距离，并更新距离数组 D，其中D[i] = min(D[i], 点 i 到新加入点的距离)

但是它们都无法保证两个点云中提取的关键点，也无法保证有更多相似的位置，即保证检测的可重复性。比如检测一个小猫，它没有办法保证能检测到鼻子，眼睛这些具有特征的点，从不同视角检测也无法提取出相同的关键点。

基于线的采样方法

核心思想是：通过降维（3D→2D）和图像处理的方法，来简化三维线特征提取的复杂性。

其流程大致如下：

1、将三维点云投影到重力方向变为二维点云

2、通过密度约束获得高密度部分：

使用密度聚类算法（如DBSCAN）或计算点的局部密度，来识别出这些2D平面上的高密度区域，比如那些对应着墙边、门窗边缘等线性结构的地方，点的密度会显著高于空旷区域（如地板中间、房间中央）。

3、执行区域生长算法得到线特征

具体步骤是先选定一个高密度中的点，然后选择其他的点可以与它形成一条合理的直线方向，将其归为一组。接下来迭代重复此操作，直至没有新的点可以加入，最终就形成了一条线段。最后将2D线段进行反向映射重新回到3D点云中的3D线段

基于RANSAC的平面提取方法

RANSAC是一种鲁棒的模型拟合算法，其核心思想与“少数服从多数”的投票原则类似。它不试图去拟合所有数据点，而是致力于寻找一个被足够多内点支持的模型，从而对异常值具有极强的免疫力。具体算法流程如下：

1、随机采样：从点云中随机选取3个点（这里假设点云是平面，3个不共线的点确定一个平面）

2、模型拟合：通过计算三个点定义的两个向量的叉积来得到法向量得到(A,B,C)，在代入一个点得到D，从而得到平面方程Ax+By+Cz+D=0

3、共识验证：计算每个点距离平面的距离，小于阈值的为内点，否则为外点，统计该模型获得的内点数量

4、迭代：重复步骤1-3 K次，选择那个获得最多内点支持的平面模型作为当前最佳模型。其中，K的计算方式为

5、模型精炼：找到最佳模型后，使用所有被识别为内点的点，通过更精确的方法（如最小二乘法）重新拟合一个更精确的平面方程。这一步旨在消除初始随机采样可能带来的误差。

6、点云更新：首先从原始点云中移除已提取出的平面的所有内点，接着在剩余的点云上重复上述整个过程，以提取下一个平面，直到找不到足够大的平面（即内点数量小于某个阈值）为止。

旋转不变性

旋转不变性特征是指一种在旋转变换下保持不变的特征。常见的有Pointnet++网络。

匹配对应方法

特征匹配用于找到源点云的特征之间的正确对应关系，从而计算出变换矩阵。常见的方法有最近邻搜索、软匹配等，接下来分别进行讲解。

最近邻搜素

最近邻搜索是一种常见的匹配对应的方法，主 要 方 法 包 括 K 近 邻（K-nearest neighbor，KNN）、KD-tree和Octree等。

K近邻：方法简单粗暴，可以理解为我们设定一个查询点P，然后在点云中依次遍历每一个点，计算与P点的距离，然后选出前K个值最小的。

KD-tree：方法稍显复杂一点，比如当前点云的点是三维，即(x,y,z)，它首先按照x，对所有点进行排序，找出中位数作为根节点，接下来在中位数左边的为左子树，右边则为右子树。接下来就到了第二层，我们对左子树的节点按y进行排序，找出中位数，然后再拆分成左子树和右子树；接下来对第一层拆分下来的右子树进行相同操作，第三层则是按z，不再赘述。

具体示例如下

Octree：这个方法则是把所有点云涵盖在方框内，然后选定大小，比如一个框内只能涵盖2个点，如果超过就进行四份划，然后一直划到最后，找K近邻的时候就从他这个区域的先找，不够的话则按照动态设置的半径画圆，找圆圈内的作为K近邻。

示例如下

软匹配

软匹配不再是一对一的，而是一对多的，其流程可以简单理解如下：

（1）假设源点云有 M 个点，目标点云有 N 个点。

（2）我们定义一个大小为 M x N 的矩阵 W。

（3）矩阵中的每一个元素 W_{i,j} 表示 源点 i 与目标点 j 对应的概率或权重。

变换矩阵求解

对应关系求解这部分就是找到源点云和目标点云的对应关系，而矩阵求解部分就是实现刚刚找到的对应关系，完成转换。

常见的刚性变换如上面提到的平移、旋转和（可能的）缩放等，它们通过最小化点云之间的差异确定最佳的变换参数。这些变换参数可以通过不同的方法求解，例如RANSAC方法、SVD方法和深度学习的方法。接下来进行依次介绍。

RANSAC方法

RANSAC方法，即随机抽样一致算法，简单来说，它就是根据点云的形状拟合出图像，比如点云是线段就两点拟合出直线，是平面就三点拟合平面，是圆也用三个点来拟合一个圆（这里的点是随机从点云中抽取的），然后计算点云到直线的距离，小于设定的阈值则为内点，否则为外点，记录内点数量然后循环K次，K是按照给定的公式计算得出的，然后选择内点数量最多的作为最佳模型。

具体可以看上文特征提取方法的点线面部分，有对RANSAC的详细介绍。

SVD方法

SVD方法在之前ICP算法中就已经进行过解释，详细的可以回看ICP部分，它主要是寻找到一个最佳旋转矩阵R和平移t，实现Pi=R*qi+t，这里的R可以通过SVD分解得出，我们是已知矩阵H的，H是去中心化后的源点云和目标点云之间的协方差矩阵，其实就是减去质心的x和y转置相乘，再整体相加，然后通过这个H我们经过SVD分解可以得到矩阵U和VT，而R就等于U * VT，所以R就解出来了，t也随之代入公式就可以解得。

深度学习方法

传统方法（如 ICP、SVD）是通过迭代或代数计算求解最优变换 R,t，而深度学习方法旨在从数据中学习一个映射函数，直接根据两个点云的特征预测变换参数。基本流程如下：

1. 特征提取：使用神经网络（如 PointNet、CNN、图网络）从源点云 P和目标点云 Q中提取全局特征或局部特征
2. 特征融合/匹配：将两个点云的特征进行交互，找到对应关系
3. 变换预测：基于融合后的特征，通过回归网络直接输出 6 自由度参数（3 个旋转，3 个平移）或完整的 4×4 变换矩阵
4. 迭代细化（可选）：很多网络会采用由粗到精的策略，逐步优化变换估计。

比如3DRegNet网络模型

其流程如下

（1）特征提取：使用 PointNet 类网络 或 3D 稀疏卷积 提取每个点的局部特征，生成原始点云和目标点云的特征矩阵Fp和Fq

（2）配准块：将两个点云的特征进行组合，常用方法包括：特征拼接、特征差值、注意力机制加权融合。然后通过全连接层回归变换参数，输出旋转矩阵R和平移向量t

（3）细化网络：将R和t用于源点云，P' = R*P+t

（4）重复步骤2-3，不过后面是慢慢修正，是残差旋转矩阵R和平移向量t，直至满足要求。

非刚性点云配准

刚性点云是寻找一个旋转矩阵R和平移向量T，使得源点云移动后可以与目标点云对应。而非刚性点云则是通过变形场进行映射，直接将源点云映射到源点云，非刚性点云的配准可以用如下公式描述，其中，F(Pi)是变形场函数。

变形场的表示

逐点位置变量表示法

逐点位置表示法可以这样理解：我们现在有一张面具，我们需要将其从面无表情改为微笑的样子，这个时候我们如果手动去进行，我们就需要一个点一个点进行变换，但这其中可能因为一个点的变换，其他点没变化而导致撕扯力度过大直接扯坏面具，这个就被称为冗余的自由度，所以我们需要保证点A移动的同时，邻居点B也在移动，这就是正则化。

具体来说，分为如下几个步骤：

（1）逐点位置变量表示法将形变后网格的顶点坐标直接作为优化变量：

（2）拟合和正则化：

（3）能量最小化框架：该方法通过构建并最小化一个总能量函数来求解最优形变

逐点仿射变换表示法

这里不对点进行直接变换，而是对每一个点都类似刚性点云配准的方法，为其配置一个旋转矩阵A和一个平移向量T，然后对其进行优化，不同的是这里是对每一个点都有不同的A和T，而刚性里则视为一个整体。

其步骤如下：

（1）为每个点猜测一个初始的仿射变换

（2）应用当前变换：使用当前每个点的仿射变换 (A_i, t_i)，计算变换后的源点云，进行拟合和正则化，使用能量最小化框架求解

（3）迭代步骤2直至收敛。

变形图表示法

由于之前的方法需要计算每一个点，内存消耗和计算量过大，所以这里引进了变形图表示法，变形图选定一部分具有代表性的节点，为每个节点分配仿射变换。一个节点的移动直接导致身边其他点的变形。

具体如下：

（1）从源点云或网格中，通过均匀采样或体素化下采样等方式，选取一组有代表性的点作为变形图的节点

（2）将空间上相邻的节点用边连接起来，形成一个图结构。这定义了节点之间的邻域关系，用于后续的正则化。

（3）通过加权平均，用少数节点的变换来驱动整个密集表面的平滑形变。

非刚性配准的求解

匹配对应

目标函数可以表示为

接着就是这两个E的计算了，对于对齐项的计算，思路主要有三种

（1）点到点的距离

这个最为直观，直接最小化变形后源点T（Pi）与其在目标点云对应点Qi的欧式距离

• 优点：计算简单。
• 缺点：收敛较慢，对初始位置敏感，容易陷入局部最优。对噪声和异常值鲁棒性差。

后续改进后为每个点引进权重，以降低离群点的影响

（2）点到平面距离

最小化变形后源点 T(pi)到目标点云中对应点 Qj 的切平面的距离。这允许源点在目标表面上“滑动”，收敛更快。

• 优点：收敛速度显著快于点到点，是当前许多主流方法的首选。
• 缺点：需要准确计算目标点云的法向量。

（3）概率方法

将目标点云看作一个概率分布（由多个高斯函数混合而成），然后寻找一个形变，使得变形后的源点云在这个概率分布下的似然概率最大。

然后是正则化学Ereg的选择，大概有以下几种

（1）平滑性正则化

主要目的是保证形变场是光滑的，相邻点的运动是连续的，不会出现突兀的撕裂或褶皱。

（2） 局部刚性正则化

虽然整体是非刚性形变，但物体的每一个小局部区域应尽可能地做刚性运动（只包含旋转和平移，避免非物理的拉伸和剪切）。

基于深度学习的求解方法

使用的方法有CNN、图卷积神经网络、MLP和Transfrom。

CNN中的PR-Net网络模型，大致思想如下：

（1）创建2D网格空间

（2）将源点云和目标点云进行映射到2D网格上，源点云和目标点云分别被编码成了两个具有相同空间结构的2D特征图。

（3）将源点云和目标点云对应的2D特征图进行拼接形成一个融合的特征图。

（4）将上一步得到的融合2D特征图，输入到一个标准的2D卷积神经网络中，然后全连接层输出参数，前四个维度是矩阵R，后三个是平移变量t

（5）应用变换

Transfrom中的NrtRet模型如下

（1）输入源点云和目标点云

（2）使用DGCNN提取特征

（3） 特征融合与变换估计