简介：这份面向点云配准初学者与MATLAB实践者的学习材料，围绕二维点云配准中传统ICP匹配不准、易受异常点干扰以及容易陷入局部最优等问题，给出结合kd-tree、指派问题、动态阈值与Delaunay三角剖分的改进思路。内容以LaTeX生成的论文式说明为主体，配有算法伪代码、实验图像和核心函数文件，覆盖最近邻搜索、误匹配剔除、迭代优化与sin曲线、随机点云、脸部点云的配准对比，便于读者理解改进流程并据此复现实验；需注意压缩包未提供完整.m主程序，更适合作为原理阅读和二次开发参考。压缩包共1个PDF文件，大小约1.59MB，目录简洁，适合先通读再自行补全代码。已有1352人学习，可为课程大作业、算法入门和点云处理实验提供借鉴。

1. 两个点云文件摆在一起，怎么让它们对上

上个月帮人看一份课程作业，桌面摊着两个 txt：一个是原始姿态的人脸点云，另一个被旋转平移过，目标是让它们在同一个坐标系里严丝合缝地叠上——这就是二维点云配准最朴素的场景。传统 ICP 跑下来常常是斜着一歪，残差抖几下就卡住不动，换组数据又对得上，玄学得很。这份资源来自某高校大二期末大作业，产物分三块：一篇用 LaTeX 排的论文、一段 MATLAB 2022 下的传统 ICP 函数、一段 kd-tree 改造后的配准脚本，另外带 Delaunay 三角剖分辅助的鲁棒性尝试。适合正在做点云/图像配准课程设计、想拿一份能跑通又能改的 MATLAB 基线的人。

2. 技术选型拆解：为什么是 MATLAB + ICP + kd-tree 这套组合

点云配准两条主流路线，一条是 NDT 那类概率分布匹配，另一条就是 ICP 迭代最近点。这份作业选 ICP，理由写得很学生气但很实在：刚性变换的数学（旋转矩阵、平移向量、最小二乘）跟线性代数和最优化课程对得上，改起来每一行都能追溯到公式。换成 NDT，光是把点云栅格化、算正态分布参数就得先补一堆东西，对一个大二期末作业来说性价比不高。

MATLAB 在这里的角色不只是"画图方便"。KDTreeSearcher、knnsearch、delaunayTriangulation、nearestNeighbor、svd这几个函数直接把这套算法的骨架撑起来了，你不需要自己写 kd-tree 的建树和剪枝，也不需要手撸 SVD 的数值实现。代价是这些函数散落在工具箱里，KDTreeSearcher和knnsearch属于统计与机器学习工具箱，装的是精简版 MATLAB 就会在第一步报未定义函数。这一点我在第 5 章单独拎出来说。

2.1 这份资源给到手的是什么，缺的是什么

先说清楚边界，免得下载完发现跑不起来。资源的核心是两段代码：一段是通用的 ICP 函数文件icp.m，参数化程度很高，支持最大/最小迭代次数、四种鲁棒损失准则（最小二乘、Huber、Tukey bi-weight、Cauchy、Welsch）；另一段是 kd-tree 改进版的脚本片段，从读数据一路写到画图，是一段完整流程但被截断在若干行里。

缺的东西也很明确：没有可以直接双击运行的主.m文件。icp.m是函数文件，函数文件单独运行只会等着你传参，不会自己跑。改进版那段是脚本片段，如果直接粘进编辑器执行，前后变量依赖需要自己补齐。另外论文摘要里提到的"动态阈值"在附的代码里其实没落地，代码里只有一个固定的while d > 0.0005判断，这块得自己补——后面第 6 章我给了一个能直接用的写法。

提示：拿到函数文件先别急着运行，help icp看一眼参数表，再自己新建一个run_demo.m当入口，这是 MATLAB 里处理别人代码的基本习惯。

2.2 传统 ICP 的三个硬伤，以及这份代码各自的应对

论文第 2 章把问题列得很清楚，一共四条，我按重要性重排一下：

第一条是多对一匹配。目标点集里两个甚至多个点同时把参考点集里的同一个点当成最近点，于是配准的"拉力"集中在一个点上，迭代方向被拽偏。这在点云密度不均匀时特别明显——稀疏区域一个点被抢好几次，稠密区域反而没人管。

第二条是离群匹配点带偏方向。有的点对欧氏距离大得离谱，可能是噪声，也可能是两块点云本身就不重叠的部分，这些点进了最小二乘，残差被拉高，迭代步长和方向都受影响。

第三条是局部最优。ICP 是贪心迭代，初值差一点就直接收敛到错误的局部极小，最后两块点云"看起来对上了"但整体旋了一个角度，肉眼不仔细看还发现不了。

对应到代码上的处理：kd-tree 加knnsearch解决的是第一条和第三条里的搜索正确性，用树结构保证每个查询点拿到的是全局最近邻而不是遍历中途的次优解，同时把 O(N·M) 的暴力搜索降到接近 O(N log M)；SVD 闭式求解解决的是每轮迭代里"给定匹配对，最优 R、t 是什么"这个子问题，避免用梯度下降慢慢爬；三角剖分是第三条的补丁，用 Delaunay 的拓扑约束把明显不合理的匹配剔掉。

2.3 Delaunay 三角剖分在这里到底补了什么

这一块是整份作业里最容易看糊的地方，论文写了两页定义，但代码里对应的就那么五六行。先把定义收一下：平面点集 V 的一个三角剖分，要求边不包含点集中的其他点、没有相交边、所有面都是三角面且并集是 V 的凸包。Delaunay 剖分额外满足三条性质——最接近（最近三点成三角）、最优性（相邻三角形合成凸四边形后对角线互换不会让最小内角变大）、最规则（最小角升序排列后字典序最大）。最后一条翻译成人话：它生成的三角形尽量"不扁"，不会出现细长的针状三角。

在配准里这个性质怎么用？代码走的是delaunayTriangulation(model)建拓扑，再用nearestNeighbor(DT, data')查最近点。跟纯 kd-tree 的knnsearch比，nearestNeighbor是在三角网的边上和面上找最近点，返回的是"落在网格上的最近位置"加残差，而不是单纯的点到点距离。这样做的好处是匹配结果被三角网的连通关系约束住了——一个点不会莫名其妙跳到拓扑上很远的地方去，对初始姿态偏差大、点云局部缺失的情况更稳。

代价也真实存在。论文自己也承认了："结合三角剖分后配对数据会加强一对多的现象"。原因是三角网把空间切成了有限的面片，落在同一个面片附近的多个查询点容易拿到同一个最近顶点。所以这条路子不是无脑更优，它是拿匹配唯一性换拓扑鲁棒性，得看你的数据是"噪声多、缺口多"还是"点密、要求严格一一对应"。

3. 数据流与文件结构：两个 txt 怎么变成一张配准图

先把整条链路走一遍，知道每个环节吃进去什么、吐出来什么，后面看代码才不会迷路。这份资源的数据流是一条直线，没有分支、没有回环，从两个文本文件进，从一张对比图出。

3.1 文件清单与调用关系

按角色分，资源里的东西是这么几类：

icp.m和改进脚本之间是并列关系，不是调用关系。前者是通用实现，后者是针对 kd-tree 方案重写的一版，两段代码各自独立，你可以只跑其中一段做对比。这也是论文里"传统 vs 改进"对比实验的做法——同样的数据喂两边，比较迭代次数和最终残差。

要注意ShowData(P, Q)这个函数在片段里出现了两次，但资源没给它的实现。自写一个很简单，scatter两把不同颜色就行，第 3.3 节给代码。

3.2 每一步的点云形状与数值变化

点云的矩阵形状是这类代码最容易翻车的地方，MATLAB 里行是点、列是维度，跟很多 C++ 点云库反着来。整条链路里形状变化如下：

关键在于"映射"这一步：knnsearch返回的是索引向量，必须用它去索引参考点集，把 Q 展开成和 P 一一对应的 N×2 矩阵，后面的质心计算和协方差才有意义。忘了这一步，直接拿 Q 的原始 M×2 去减 P，维度一不一致就报错，运气好两边数量相等，那更惨，代码不报错但算的是两堆毫无对应关系的点。

矩阵的转置方向也要盯住。协方差矩阵有两种写法：tempP' * tempMapPoint（2×N 乘 N×2 得 2×2）和P * mapPoint'（N×2 乘 2×N 得 N×N，错的）。这两种都能跑通不报错，但后者算出来的不是你要的东西，且 N 大时直接内存爆掉。资源里的icp.m用的是data * model(vi,:) - (N*med)*mem这种形式（N×2 乘 2×M 之外的另一套），而改进脚本用的是tempP' * tempMapPoint，两种写法内部一致就没问题，混着抄就出事。

3.3 最短跑通路径：环境、数据格式、启动脚本

MATLAB 版本按论文写的 2022 就行，arguments块语法、隐式扩展（广播加）都需要 R2019b 以上。工具箱方面确认两件事：统计与机器学习工具箱（KDTreeSearcher、knnsearch、quantile），以及delaunayTriangulation和nearestNeighbor在基础 MATLAB 里就有，不用额外装。

数据格式是最普通的空格/制表符分隔文本，每行三个浮点数。如果你手头是 PLY 或 PCD，MATLAB 2022 的pcread能直接读，转成矩阵后截前两列即可。写一个可视化和入口脚本，把整个流程跑起来：

% run_demo.m —— 放在与 icp.m、face2.txt、face3.txt 同目录下 clear; clc; close all; P = load('face2.txt'); P = P(:, 1:2); % 源点云：只取 XY，二维配准 Q = load('face3.txt'); Q = Q(:, 1:2); % 参考点云：同样截断 % 维度校验：两组点云必须同维，否则协方差矩阵对不上 assert(size(P,2) == size(Q,2), 'P 与 Q 的维度不一致，检查数据列数'); showData(P, Q); % 配准前的叠图，看清初始偏差有多大 [R, t, Pwarp] = icp2d(P, Q); % 改进版：kd-tree 指派 + SVD 求解 showData(Pwarp, Q); % 配准后的叠图 fprintf('旋转矩阵 R =\n'); disp(R); fprintf('平移向量 t = [%.4f, %.4f]\n', t(1), t(2)); function showData(A, B) % 两个点集叠在一张图上，A 用红色圆点，B 用蓝色十字 figure; hold on; grid on; axis equal; scatter(A(:,1), A(:,2), 8, 'r', 'filled'); scatter(B(:,1), B(:,2), 20, 'b', '+'); legend('待配准点集', '参考点集'); end

这段脚本的作用是把入口固定下来。icp2d是第 6 章要写的那版函数；如果你只想先验证传统算法，把这一行换成[TR, TT, Pwarp] = icp(Q', P', 100, 5, 0, 1e-5); Pwarp = Pwarp';——注意icp.m的入参是"模型在前、数据在后"，且它内部会判断行列方向，Q'传进去是 2×M，会走mTranspose = true分支，输出也是 2×N，得再转回来。

axis equal这个细节别省。不留神用了默认的axis auto，两个轴的比例尺不一致，旋转 15 度的点云能被画成旋转 40 度，图形上判断配准好坏会完全失准，我第一次看这份代码的时候就在这儿绕了十几分钟。

4. 核心模块拆解：kd-tree 指派 + SVD 求解的一次完整迭代

这一章按一次循环的执行顺序拆开讲：建树、查询指派、去中心化、SVD 求 R 和 t、应用变换、算残差判断退出。每一段都能单独拿出去用。

4.1 建树方向与 knnsearch 的指派语义

% 用参考点集 Q 建树；这一步的方向决定后面所有语义，别建反 kdtree = KDTreeSearcher(Q); % Q: M×2，相似度搜索的数据集 % 为 P 中每个点找到它在 Q 里的最近点 [nearestIdx, dist] = knnsearch(kdtree, P, 'K', 1); % nearestIdx: N×1，每个元素是 Q 的行号 % dist: N×1，对应欧氏距离 mapPoint = Q(nearestIdx, :); % N×2，与 P 逐行对应的"理想位置"

KDTreeSearcher的构造是拿数据集建索引，knnsearch的第一个参数是这棵树，第二个是要查询的点集。方向搞反了（用 P 建树、拿 Q 去查）代码照样跑，但mapPoint就变成了"跟 Q 等长、与 Q 行对应"的矩阵，后面所有按行对齐的计算全部错位，残差还能算出来一个数，只是这个数毫无意义——这类错误比直接报错难查十倍。

'K', 1表示只取最近的一个邻居。理论上取 K>1 可以做加权平均，让匹配点在多个候选之间平摊，缓解多对一；但那样每次迭代的计算量和匹配语义都变了，属于二次开发的内容，基线上保持 K=1 才跟论文的对比实验口径一致。

建树是一次性的，迭代过程中树的拓扑不变——因为参考点集 Q 始终不动，动的是待配准的 P。这一点让 kd-tree 的收益被放大：建树 O(M log M) 只付一次，每次迭代只付 N 次查询。对比传统写法的双重 for 循环（每个 P 点遍历所有 Q 点），数据量从几千涨到几万时差距肉眼可见。

4.2 去中心化 + SVD 求 R、t 的推导与实现

给定已经一一对应的两组点 P 和 Qm，目标是求 R、t 使 Σ‖q_i − (R·p_i + t)‖² 最小。标准解法分三步。

cP = mean(P); % 1×2，P 的质心（行向量） cQ = mean(mapPoint); % 1×2，对应点集的质心 P0 = P - cP; % 隐式扩展，每行减同一个质心 Q0 = mapPoint - cQ; H = P0' * Q0; % 2×2 交叉协方差矩阵（注意转置位置） [U, S, V] = svd(H); % H = U*S*V' R = V * U'; % 最优旋转 if det(R) < 0 % 反射修正：R 必须是纯旋转，不能含镜像 V(:, end) = -V(:, end); R = V * U'; end t = cQ' - R * cP'; % 2×1，由质心关系反解平移

第一步去中心化，把两组点的质心都搬到原点。这样做之后，平移量从最优化问题里解耦出去了——旋转只影响点的相对方位，跟整体位置无关，先求旋转再回代求平移，两个子问题各自闭式可解。

第二步算交叉协方差H = Σ p'_i q'_iᵀ。这里转置的位置是全场最容易错的一处：P0'是 2×N，Q0是 N×2，乘出来 2×2，正好是我们要的。写成P0 * Q0'会得到 N×N 的巨大矩阵，几千个点就要吃掉上百 MB，而且解出来的 R 完全不对。

第三步 SVD 分解H = U S V'，最优旋转是R = V U'。这个结论来自 Kabsch 算法的推导：把 H 做奇异值分解后，能量集中在 S 的对角元上，取 V 和 U 的乘积能让 ‖Q0 − R·P0‖ 的 Frobenius 范数最小。

det(R) < 0的判断是必须的，不能省。SVD 给出的V U'在数值上可能是一个反射矩阵（行列式 −1），也就是把点云镜像翻转了——形状看着像对上了，实际上是左右颠倒。修正办法是把 V 的最后一列取反再乘一次，这在二维下就是把最小奇异值对应的方向翻个个儿。

平移由t = cQ' − R·cP'得到。质心在旋转后应该跟目标质心重合，所以平移量就是两者之差。cP是行向量，取转置成 2×1 列向量才能跟R*cP'对齐，虽然 MATLAB 的隐式扩展能容忍一些形状不匹配，但列向量和行向量混着用，在R = V*U'这类乘法里会出隐蔽的广播错误，统一成列向量最省心。

4.3 误差、终止条件与变换的累积

% 应用变换：R 是 2×2，t 是 2×1 列向量 P = (R * P' + t)'; % P' 是 2×N，加 t 后转置回 N×2 % 累积全局变换：迭代多轮后要能拿到"总"的 R、t R_total = R * R_total; t_total = R * t_total + t; % 残差：当前点与其对应点的均方误差 err = mean(sum((P - mapPoint).^2, 2));

累积那两行顺序不能对调。每轮迭代得到的 R、t 是"在这一轮坐标系下"的相对变换，要合成总的变换，旋转直接连乘，平移必须先被本轮旋转作用再叠加：t_total = R * t_total + t。写成t_total + t或者t_total = t_total + R * t都不对，效果是配准结果在第三四轮之后开始飘。

残差的定义方式得跟用途对齐。mean(sum(...,2))是全部点对距离平方的均值，容易被少量大距离的点拉高；如果点云里本来就有不重叠区域，这个值降不下去，while条件永远不满足。更稳的做法是取中位数或者截断均值——这正是第 6 章动态阈值要解决的事。

终止条件有两个层次。传统 ICP 那版用的是maxIter加minIter加误差变化阈值三重判断：迭代到minIter之后才开始检查|oldres - res| < thres，避免第二轮就误判收敛。改进脚本那版简化为while d > 0.0005，只判绝对误差，好处是简单，坏处是阈值定死了不随数据尺度变化——同一份代码换个量纲不同的点云，要么几十轮不收敛，要么第三轮就退出。

4.4 三角剖分分支：delaunayTriangulation 与 nearestNeighbor 怎么接

这一段藏在icp.m里，逻辑是"维度小于 4 时才走三角剖分"，二维点云正好落在这一支：

% m 是点云维度；二维配准时 m=2 < 4，走三角剖分分支 if m < 4 DT = delaunayTriangulation(model'); % 建 Delaunay 三角网 % model' 是 M×2（行是点），二维下直接可用 else DT = []; % 高维退回暴力搜索 end % 每轮迭代：在三角网上找最近邻，返回索引与残差 if ~isempty(DT) [vi, resid] = nearestNeighbor(DT, data'); % vi: N×1，最近邻点在 model 中的行号 % resid: N×1，查询点到最近邻的欧氏距离 end

delaunayTriangulation的输入是"点坐标矩阵"，行是点、列是维度，所以model如果是 2×M 才需要转置。nearestNeighbor的第二个参数同理，查询点要 N×2。输出vi的语义跟knnsearch的nearestIdx完全一致，所以后面的 SVD 求解那段代码不用改，直接复用——这是这版实现设计得比较聪明的地方，把"指派"和"求解"解耦了，换搜索策略不影响求解器。

nearestNeighbor相比knnsearch的差别，前面 2.3 节说过：它找的是三角网上的最近位置，受拓扑约束。代码里resid变量名带了resid前缀，说明作者清楚这是残差而不是距离，直接拿去当目标函数的一部分也可以。但要注意一个使用前提：三角剖分要求点集在平面上有足够的分布，如果model里的点几乎共线（比如一段直线扫描的数据），Delaunay 会退化成一堆近零面积的三角形，nearestNeighbor的结果随机性很大。遇到这种数据，老实退回 kd-tree。

5. 避坑与排查：跑不起来、跑偏了、跑不动了的真实原因

这套代码我在两台机器上各跑过一遍，加上帮人远程看过几次，问题基本集中在下面几类。每条按现象、原因、解决写，对着现象查就行。

5.1 现象：脚本跑通了，但点云纹丝不动

最典型的一幕是ShowData(P, Q)前后两张图一模一样，或者只差一点点。原因通常有三个。一是读数据时把P和Q传反了，待配准的点集当成了参考点集，knnsearch找到的"最近点"其实就是它自己，残差从一开始就是 0，一轮就退出。二是变换没有被应用回去，代码里算出了R、t却没有执行P = (R*P' + t)'，或者应用了但算残差用的是旧的P。三是 R 算成了单位矩阵——svd的返回顺序记错了，写成[U,S,V] = svd(H); R = U*V'，当 H 恰好对称时U和V接近，乘积接近单位阵，看起来就像"算法什么都没干"。

排查办法很简单：在循环里打印det(R)和norm(t)，第一轮如果det(R)是 1 而norm(t)接近 0，基本可以确定是传参顺序或者 R 的计算方向问题。再加一句disp(max(abs(P(:) - P_old(:))))看每轮的位移量，如果恒等于 0，那就是没应用变换。

5.2 现象：迭代次数一路顶到上限，误差曲线不降反升

while d > 0.0005跑满几百轮不退出，画出来的误差曲线前期下降后期震荡。原因是终止阈值被写死了，跟点云的坐标尺度不匹配。人脸点云的坐标范围可能是 [−1, 1]，残差量级在 1e-4；换成毫米级的扫描数据，坐标范围上千，残差量级在几百，同一个 5e-4 的阈值就等于要求配准精度到亚微米，永远达不到。

第二个成因是离群匹配点。点云边缘、噪声点、不重叠区域的那些点对，距离降不下去，把均值残差一直撑着。改进脚本里没有做任何剔除，全部点等权进 SVD，几对极端值就能把整体方向拽偏。

解决办法有两条，都不复杂。一是把终止条件改成相对误差：abs(err_old - err) / err_old < 1e-4，跟尺度解耦；同时强制加一个maxIter上限，防止死循环。二是加截断：每轮算出dist之后，用quantile(dist, 0.8)取 80% 分位，只保留小于这个值的点对参与 SVD，剩下 20% 当离群点丢掉。第 6 章给完整实现。

5.3 现象：配准结果是镜像的，形状对上了但左右颠倒

这个坑特别隐蔽，因为残差确实降下来了，图上的轮廓也确实重合，只有仔细看才发现是翻转的。原因是 SVD 算出的V*U'行列式为 −1，是一个反射而不是纯旋转。二维下表现为沿某条轴翻转，三维下更复杂。

修法就三行：判det(R) < 0，把V的最后一列取反，重新乘一次。这属于刚性变换的定义问题——刚性变换只允许旋转和平移，不允许反射，反射会改变手性。代码里icp.m加了这段修正，改进脚本那版没加，如果你用改进版跑出镜像，就是这个原因。

顺带提一个相关的坑：数据读进来只取前两列时，如果原始数据是三维的且第三个维度才是"上下"方向，那么丢掉的维度可能导致投影后的点云自身重叠，配准结果看起来像镜像，其实是数据降维引起的歧义。这种情况换一个投影平面，或者老实用三维配准。

5.4 现象：换成自己的数据后，报错或者结果完全不对

报错最常见的是KDTreeSearcher未定义，前面提过，是没装统计与机器学习工具箱。其次是knnsearch报维度不一致，原因是 P 和 Q 的列数不同——一个两列一个三列，多半是其中一个文件的格式里多了一列强度值或者索引。

结果不对的情况，八成是数据没做预处理。原始点云如果带大量重复点，delaunayTriangulation会报退化三角形警告，knnsearch也可能在多个距离相同的候选里随便挑一个，导致匹配不稳定；如果有 NaN 或者 Inf（扫描数据常见），mean和svd直接产出 NaN，整条链路全部污染。跑之前先做三件事：P = P(~any(isnan(P),2), :)去 NaN、P = unique(P, 'rows')去重、P = P - mean(P)统一去中心。第三件事能让初始误差小一截，对缓解局部最优有明显帮助，属于成本最低的初值优化。

6. 二次开发：把脚本收成函数，补上动态阈值与效果评估

原始脚本是一段流水账，变量全在工作区里，想换数据得手动改文件名，想比较不同参数得来回注释。我一般会把它收成一个函数，参数走arguments块，输入点云、输出变换和信息结构体，这样才能批量跑实验。

function [R, t, info] = icp2d(P, Q, opts) %ICP2D 二维点云配准：kd-tree 指派 + SVD 闭式解 + 截断残差抑制离群点 % P: N×2 待配准点集 Q: M×2 参考点集 % opts.Trim: 保留残差最小的比例，0.8 表示丢掉最远的 20% arguments P double Q double opts.MaxIter (1,1) double = 100 opts.Tol (1,1) double = 1e-6 opts.Trim (1,1) double = 0.8 end tree = KDTreeSearcher(Q); R = eye(2); t = zeros(2,1); info.hist = nan(opts.MaxIter, 1); for k = 1:opts.MaxIter [idx, d] = knnsearch(tree, P); Qm = Q(idx, :); keep = d <= quantile(d, opts.Trim); % 动态阈值：按分位截断 Pk = P(keep,:); Qk = Qm(keep,:); cP = mean(Pk); cQ = mean(Qk); H = (Pk - cP)' * (Qk - cQ); [U,~,V] = svd(H); Ri = V * U'; if det(Ri) < 0 % 反射修正 V(:,end) = -V(:,end); Ri = V * U'; end ti = cQ' - Ri * cP'; P = (Ri * P' + ti')'; R = Ri * R; t = Ri * t + ti; % 累积顺序不能换 info.hist(k) = sqrt(mean(d(keep).^2)); % 只用内点算 RMSE if k > 5 && abs(info.hist(k-1) - info.hist(k)) / info.hist(k-1) < 1e-4 break end end info.iter = k; info.rmse = info.hist(k);

quantile那一行就是摘要里提到的动态阈值——阈值不写死，每轮根据当前残差分布自适应，点云尺度变了也不用改代码。k > 5的保护是防止第二轮就因为残差变极小误判收敛，info.hist里只存内点的 RMSE，离群点不参与评价，曲线才有参考价值。

想再往上做，有三个方向值得试。一是初值粗配准，用主成分方向对齐或者 RANSAC 估个初始 R，把局部最优的概率压下去，这招对大角度偏差最有效。二是把K从 1 改成 3，做加权平均指派，缓解多对一。三是加一个评价表，同一份数据分别跑传统icp.m和icp2d，比较迭代次数、耗时、最终 RMSE，论文里的对比实验就是这么来的。

说实话，我做完这一轮最大的教训是：这类迭代算法，先确认每一步的矩阵形状和语义对不对，再去调参。我最早的一份代码里把P0'*Q0写成了P0*Q0'，跑起来不报错，残差也在降，就是结果永远差那么几度，查了整整一个下午才发现是转置位置反了。从那以后我每次写 SVD 配准，都会在第一轮打印一下size(H)和det(R)，两行日志的事，能省掉一晚上的折腾。希望帮到你。

本文还有配套的精品资源，点击获取