未知点序、点数不等的两个3D数据集刚性对齐方法咨询

本次任务目标为：对源3D点集施加刚性旋转、平移变换，求解得到与目标点集对齐的最优旋转矩阵R与平移向量T，需要解决两类核心约束：点集间点对应顺序未知、两个点集包含的点数量不相等。 #

针对等点数、无噪声测试场景的解法 #

你给出的测试场景中，两个点集点数完全相等、仅点序被打乱、无额外噪声和外点，点之间存在严格一一对应关系，测试代码如下：

rng(11);
N = 10 %10 points
data1 = randn(10,3); %10 coordinates
order = randperm(N)
data2 = data1(order,:); %Reordered data

%Rotation matrix and translation vector
rotM = [0.5771    0.1728    0.7982;
   -0.4940    0.8521    0.1728;
   -0.6503   -0.4940    0.5771]
TT = [1 2 3];

data3 = data2*rotM + TT;

这个场景不需要复杂迭代，利用刚性变换的距离不变性即可快速求解：

• 第一步：分别计算data1和data3的点间欧氏距离矩阵，刚性变换不会改变任意两点之间的距离，两个距离矩阵仅存在和点序对应的排列差异
• 第二步：逐行匹配距离矩阵的特征，找到data1中每个点在data3中对应的点，得到100%正确的一一对应关系
• 第三步：基于匹配好的点对，用SVD分解法直接求解精确的R和T：
  1. 分别计算两个点集的质心，将所有点减去对应质心完成去中心化
  2. 计算两点集的协方差矩阵$H = \sum_{i=1}^{N} (p_i - c_p)(q_i - c_q)^T$，其中$p_i$是目标点集的点，$q_i$是源点集的对应点，$c_p、c_q$分别是两个点集的质心
  3. 对H做SVD分解得到$H = U\Sigma V^T$，旋转矩阵$R = VU^T$，如果计算得到det(R)=-1，需要将V的最后一列取反修正，保证结果是纯旋转而非反射
  4. 平移向量$T = c_p - R c_q$
     按这个流程计算得到的R、T和代码中预设的rotM、TT完全一致。

针对点数不等、存在噪声/外点的实际场景的解法 #

实际采集的3D数据集通常不存在严格一一对应，会有噪声、非重叠区域（外点），两个点集大小也往往不一致，这类场景按粗配准+精配准的流程处理即可：

• 粗配准阶段（无初始位姿要求）
  • 特征匹配方案：对两个点集分别计算FPFH、SHOT等3D局部不变特征，通过特征相似度匹配得到初始对应点对，结合RANSAC框架剔除错误匹配的外点，求解得到初始的旋转、平移矩阵，让两个点集大致对齐
  • 无特征方案：如果点集纹理少、特征区分度低，采用4PCS、Super4PCS这类基于共面四点集不变量的配准算法，不需要提前计算点特征，对噪声和外点的容忍度较高
• 精配准阶段（需要粗配准提供初始位姿）
  • 最常用的是带外点剔除的ICP（迭代最近点）算法：每一轮迭代为源点集的每个点搜索目标点集中的最近点作为临时对应，设置距离阈值过滤掉距离过远的外点，再用上述SVD方法求解当前轮次的最优变换，迭代直到变换参数收敛即可
  • 如果外点占比高、非重叠区域大，替换为Trimmed ICP、Point-to-Plane ICP等鲁棒变种，配准精度和收敛稳定性比原生ICP更好

注意：所有求解刚性变换的步骤都必须校验旋转矩阵的行列式值，保证det(R)=1，避免得到镜像反射的错误结果。

内容的提问来源于stack exchange，提问作者SEU