一、文章信息与摘要
文章标题：Least-Squares Rigid Motion Using SVD（使用奇异值分解的最小二乘刚性运动）
说明本文的核心目标：计算对齐两组对应点的最佳拟合刚性变换的步骤
二、问题描述
假设P={p1,p2,...,pn}和Q={q1,q2,...,qn}是两组Rd空间中的对应点集，现在想要根据这个两个点集的数据来计算出它们之间的刚性转置信息，可以知道这其实是一个最小二乘求优问题。
目标：我们希望找到一个使两个集合在最小二乘意义下最优对齐的刚性变换，即寻找一个旋转矩阵 R 和平移向量 t，满足如下关系：
其中， 𝑤𝑖 表示每个点的权重。 𝑆𝑂(𝑑) 表示 d 维空间的一组旋转群。
三、计算平移向量t和旋转矩阵
计算平移向量t的推导过程：
计算旋转矩阵的推导过程：
四、反射修正
为什么要进行反射修正？
答：通过之前的推导，我们用 SVD 求解的 R 一定是一个正交矩阵，但并非所有正交矩阵都是旋转矩阵，还可能是反射，因此要进行判断。
旋转矩阵的定义：旋转矩阵是一个用来表示在欧几里得空间中的点或向量围绕一个固定轴的旋转的正交矩阵。
旋转矩阵—>正交矩阵，但正交矩阵不能推导出旋转矩阵。举个反例：
- 因此我们还需要对所求得的 R 进行行列式判断，判断方法：
- 如果 det(𝑉𝑈⊤)=−1，则所求的 R 包含了镜像；
- 如果 det(𝑉𝑈⊤)=1，则所求的 R 是我们所求的旋转矩阵。
-
反射的最优化问题
如果确定了𝑅包含反射（det(𝑉𝑈𝑇〖VU〗^T)=−1），我们接下来的目标是找到一个反射矩阵𝑀，它能最大化迹的函数𝑡𝑟(Σ𝑀) 。这里的迹函数只依赖于 𝑀的对角线元素𝑚𝑖𝑖。我们将这些对角线元素视作变量，它们构成了所有反射矩阵对角线的集合。由于反射矩阵可以通过反转旋转矩阵的一行来构建，所以优化集合是点 (±1,...,±1) 的凸包，其中坐标−1的个数是奇数。
凸多面体和极值
由于我们处理的领域是一个凸多面体，线性函数𝑓在其顶点达到极值。因此，我们寻找的最优解是在这些顶点之一。其中对角线全为1（即没有 −1）不在考虑的范围内，所以次优的选择是在最后一个元素为 −1的情况，即 𝑡𝑟(Σ𝑀)=𝜎1"σ" _1+ 𝜎2 "σ" _2 +…+ 𝜎d−1 "σ" _(d-1) − 𝜎d "σ" _d 。这个值是在我们的域的一个顶点上取得的，除了全1情况外，是所有可能组合中最大的，因为 𝜎d"σ" _d是最小的奇异值（因为SVD 分解特征值是从大到小排序）。
总结
如果 det(𝑉𝑈𝑇〖VU〗^T)=−1，我们需要的 𝑀=𝑉𝑇𝑅𝑈V^T RU的形式，其中最后一个元素为 −1。即：
-
我们还可以总结出一个更一般的公式，无论 det(𝑉𝑈𝑇〖VU〗^T)等于−1还是−1，R 都可以表示为：
-
五、总结
经过上面的推导和镜像修正，我们可以总结出一套完整的使用 SVD 求解 ICP 问题的流程：
我们的问题是求解 R, t 使得下面的误差函数最小：
步骤如下：
- 1.计算两个点的加权质心；
2.对所有点做归一化；
3.计算d x d 的协方差矩阵（d代表数据维度）；
4.计算S的SVD分解S= 𝑈Σ𝑉𝑇〖UΣV〗^T，则得到想要求的旋转矩阵R如下：
5.计算平移向量t；
六、本文的扩展——点云配准中的ICP算法
ICP（ Iterative Closest Point,迭代最近点）算法是一种广泛应用于计算机视觉和机器人领域的技术，主要用于在两组数据点之间找到最佳的对齐方式。通过不断地迭代，每次在前一次的计算结果之上再计算出新的变换矩阵，最终当迭代次数满足条件或者变换矩阵收敛时停止。ICP广泛应用于3D重建、机器人定位、医疗影像处理等领域。
原理
ICP算法的基本步骤如下：
优点：简单，不必对点云进行分割和特征提取；初值较好情况下，精度和收敛性都不错
缺点：对初始变换敏感，容易陷入局部最优解；只考虑了点与点距离，缺少对点云结构信息的利用
实际使用中的一些注意事项
| ICP改进算法名称 | 简介 |
| Point-to-Plane ICP | 原始 ICP 算法的代价函数中使用的 point-to-point 距离，point-to-plane 则是考虑源顶点到目标顶点所在面的距离，比起直接计算点到点距离，考虑了点云的局部结构，精度更高，不容易陷入局部最优；但要注意 point-to-plane 的优化是一个非线性问题，速度比较慢，一般使用其线性化近似； |
| Plane-to-Plane ICP | point-to-plane 只考虑目标点云局部结构， plane-to-plane 顾名思义就是也考虑源点云的局部结构，计算面到面的距离 |
| Generalized ICP (GICP) | 综合考虑 point-to-point、point-to-plane 和 plane-to-plane 策略，精度、鲁棒性都有所提高； |
| Normal Iterative Closest Point (NICP) | 考虑法向量和局部曲率，更进一步利用了点云的局部结构信息，其论文中实验结果比 GICP 的性能更好。 |
七、本文的扩展——常见点集配准技术：RPM
Robust point matching（RPM）
RPM算法的核心是通过软对应和确定性退火过程来逐步达到精确匹配：
1.软对应：RPM算法中每个点可以部分地与多个点对应，这种软对应由一个“对应矩阵”来表示，该矩阵中的每个元素表示两点之间的匹配程度。
2.确定性退火：算法引入了退火技术，通过逐渐减小“温度”参数来从宽松的匹配逐步过渡到严格的匹配。高温度允许点对之间有更大的自由度，随着温度的降低，算法逐步锁定更精确的对应关系。
3.变换优化：在每个退火步骤中，算法通过优化变换参数（旋转、缩放、平移）来最小化配准误差，通常使用迭代最小化方法。
优点：由于引入了退火过程，RPM算法不如ICP算法那样依赖于初始位置，能够从较差的初始状态恢复；软对应机制使得RPM能够处理局部遮挡问题，不要求两个点集具有完全相同的点数或结构。
缺点：RPM算法的计算复杂度高于传统ICP算法，尤其是在点集较大时，计算量和时间消耗显著增加。算法效果很大程度上依赖于退火过程中温度下降策略和其他参数的选择，这些参数的设定需要根据具体问题仔细调整，可能需要较多的实验和专业知识。
八、本文的扩展——常见点集配准技术：KC
Kernel correlation（KC）
KC算法的核心是利用核函数来转换和比较数据集，其基本步骤如下：
1.数据转换：使用核函数将原始数据映射到一个高维特征空间。常用的核函数包括高斯核、多项式核等
2.核相关计算：在特征空间中，通过计算两个映射后的数据集的内积（或相似度）来评估它们的相关性。核相关的值越高，表示两个数据集越相似。
3.归一化：通常需要对核相关值进行归一化处理，确保结果的稳定性和可比性。
优点：通过使用核函数，KC算法能有效处理非线性的数据，显著提高模型的灵活性和适用范围；提供了一种在复杂数据集间进行深入相似度评估的有效工具。
缺点：映射数据到高维空间可能导致计算成本高；算法性能在很大程度上依赖于核函数的选择，不恰当的核函数可能导致性能不佳；参数敏感：核参数（如在高斯核中的带宽）的选择对结果有显著影响，需要仔细调整以获得最佳性能。
九、本文的扩展——常见点集配准技术：CPD
Coherent point drift（CPD）
CPD算法的核心思想基于概率方法，将点集配准问题视为一个概率密度估计问题。算法的基本步骤包括：
1.概率模型：将一个点集视为高斯混合模型（GMM）中心的“静态”点集，而另一个点集则被视为来自这个模型的“动态”样本。
2.期望最大化（EM）算法：CPD使用EM算法来估计配准的参数。在期望步骤（E-step），算法计算每个点对模型中每个高斯成分的责任度（即该点来自某个高斯成分的概率）。在最大化步骤（M-step），根据这些责任度更新变换参数和GMM参数。
3.变换估计：CPD支持刚体、仿射和非刚体变换，可以根据实际应用需求选择相应的模型进行点集配准。
优点：CPD算法对噪声和异常值具有很好的鲁棒性，可以有效处理不完整或部分遮挡的数据。支持多种类型的变换，包括刚体、仿射和非刚体变换；无需手动标定或预处理，算法能够自动对点集进行有效配准。
缺点：由于涉及复杂的迭代计算和概率模型，CPD算法在处理大规模数据时可能会有较高的计算成本；算法性能在一定程度上依赖于参数的选择，如高斯混合模型的宽度参数等，这些参数需要根据具体的数据情况仔细调整；在某些情况下，CPD算法可能需要较多的迭代次数才能收敛，特别是在点集规模较大或数据复杂度较高的情况下。
十、本文的扩展——常见点集配准技术的对比
参考文章：
点集配准技术（ICP、RPM、KC、CPD） - 算法小丑 - 博客园 (cnblogs.com)
