简介：面向点云配准学习者的一份PDF论文，内容围绕传统ICP算法的缺陷展开改进，基于MATLAB2022完成实现与验证。论文先指出传统ICP在寻找最近点时易出现多对一匹配、匹配点对距离过大、迭代易陷入局部最优等问题；随后提出结合kd-tree加速最近邻搜索，引入指派问题优化对应关系，设置动态阈值剔除异常匹配，并利用delaunay三角剖分增强空间结构稳定性。文中给出了完整的算法原理、伪代码和MATLAB中的部分函数代码（未附完整.m文件），同时通过正弦曲线、随机点云、脸部点云三个实验对比验证了改进效果，对提升配准精度和速度有直观说明。资源为单个PDF文件，大小约1.59MB，目前已有1350人学习，适合正在学习ICP算法、点云配准或准备相关大作业的读者借鉴。

1. 一个配准问题的优化样本：kd-tree 如何把 ICP 从多对一泥潭里拉出来

传统 ICP 点云配准在小数据集上跑起来像个体面的黑匣子：找最近点、算旋转平移、迭代几步就收敛。可点云一密，问题全冒出来了——目标点集里好几个点抢同一个参考点，形成多对一匹配，直接把迭代方向带偏，误差卡住不降。这份项目正是盯准这三个毛病动手：用 MATLAB2022 复现传统 ICP，引入 delaunay 三角剖分和 kd-tree 最近邻查询，把暴力搜索换成空间索引，再用 SVD 求解变换矩阵、动态阈值控制迭代停止，把配准精度和速度都拉上去。实验用 sin 曲线、正方随机点云和公开人脸面部点云三组数据验证，效果对比明显。适合正在做课程设计、想研究 ICP 改进思路、或者要在 MATLAB 里快速验证配准算法的人。项目交付的是 LaTeX 论文加两段核心代码，没有完整 .m 工程，需要自己搭实验台，正好适合动手实践。

2. 传统 ICP 算法拆解：迭代最近点的三个致命短板

2.1 刚性配准目标与 ICP 迭代流程

刚性配准的定义很直接：给定两个点集，产出一个刚性变换，这个变换不改变任意两点之间的距离，一般只包含旋转和平移。ICP 就在这个约束下迭代逼近最优变换，形式化表达为最小化目标函数：

min E = Σ||qi − (R·pi + T)||²

其中 pi 是目标点集里的点，qi 是参考点集里与它匹配的最近点，R 和 T 是待求的旋转矩阵和平移向量。

传统 ICP 的完整流程分四步：第一步选取参考点集和目标点集；第二步遍历目标点集中的所有点，在参考点集里找欧氏距离最小的点作为对应点；第三步建立目标函数并优化求解，得到旋转矩阵 R 和平移向量 T，更新目标点集；第四步检查是否达到最大迭代次数或误差阈值，满足就停止，否则回到第二步。这个流程思路简单，但第二步的"最近点配对"恰恰是整个算法的命门——配对准了，后面 SVD 求解才靠谱；配对错了，迭代方向就歪了。

实验环境是 CPU i7-10710U、Windows10、MATLAB2022。原论文用三组数据做测试：sin 曲线、正方随机点云、脸部点云。实验结果里能直观看到配准图与原始图之间存在明显错位，尤其在脸部点云上配准结果不够理想。这个结论接下来会逐条分析。

2.2 Delaunay 三角剖分：MATLAB 里被低估的最近点索引

传统 ICP 的最初实现里，最近点搜索用的是暴力双层循环，每个目标点都要和所有参考点算一遍距离，开销很大。论文实现中借助 MATLAB 内置的 delaunayTriangulation 做了优化，思路是把参考点集剖分成三角形网格，然后通过近邻查询快速定位最近点。

Delaunay 三角剖分把一个平面点集分割成最匀称的三角形集合，它满足几条关键特性：最接近——以最近的三点形成三角形，且线段互不相交；最优性——任意两个相邻三角形形成的凸四边形，对角线互换后最小角不会变大；最规则——把所有三角形的最小角升序排列，Delaunay 三角网得到的数值最大；区域性——新增、删除、移动某个顶点时，只影响临近的三角形。

DT = delaunayTriangulation(model); % 用参考点集构建三角剖分 [vi, resid] = nearestNeighbor(DT, data'); % 为每个目标点查最近参考点 % vi 返回最近参考点的索引 % resid 返回最近点之间的欧氏距离

nearestNeighbor 函数配合 DT 之后，最近点查询从"遍历全部点"变成了"在三角形邻域里定位"，复杂度明显下降。而且三角剖分的区域性特性让定点增删只影响局部网格，对动态点云有天然的鲁棒性。但这里埋了个隐患：三角剖分让相邻区域共享大量三角形边，配对时更容易出现一对多的情况，也就是同一个参考点被多个目标点同时选中。后面第 3 章会看到，这正是 kd-tree 要接棒的原因。

2.3 三组实验暴露的三个问题

原论文对传统 ICP 做了数据分析和图像对比，问题集中在三个方向。

第一，最近点匹配会出现多对一的情况。目标点集中多个点同时匹配到参考点集中同一个点，这时候目标函数里的对应关系就失真了，迭代方向被带偏，最终误差无法收敛。论文里的图 9 展示的就是这种多对一匹配的典型形态，配准结果明显歪斜。

第二，有些匹配点对的欧氏距离过大。这些"远距离点对"可能是噪声点，也可能是初始位姿偏差太大造成的，它们对目标函数的贡献被平方放大，导致旋转矩阵和平移向量被拉偏，迭代次数增加，甚至彻底改变优化方向。论文的图 10 就展示了这种配对距离过大的情况。

第三，算法容易陷入局部最优。ICP 本质是一个非凸优化问题，初始位姿稍微差一点，迭代就容易收敛到错误的局部极值点。尤其在脸部点云这种结构复杂的数据上，传统 ICP 算出来的结果和真实位姿差距明显，整体轮廓虽然对齐了一部分，但关键区域的错位依然肉眼可见。

还有一个从实验里观察到的附加问题：结合 Delaunay 三角剖分之后，一对多的配对现象反而被加强了。因为三角剖分让邻近区域共享更多三角形边，近邻关系过于丰富，同一个参考点被多个目标点匹配到的概率变大了。这说明单纯加三角剖分做最近点索引还不够，必须从配对策略层面做改进。

3. 改进方案设计：kd-tree 指派、动态阈值与 SVD 的组合拳

3.1 kd-tree 原理：用空间切分换查询速度

kd-tree，即 K-dimensional tree，是一种面向多维空间的高维索引树形结构。它的每个节点都是一个 k 维点的二叉树，所有非叶子节点可以看作一个把空间切成两个半空间的超平面。节点左子树代表超平面左侧的点，右子树代表超平面右侧的点。这种结构天然适合最近邻搜索和近似最近邻搜索。

拿二叉查找树来类比最好理解：二叉查找树本身就是一个一维空间下的 kd-tree。建树过程是一个节点进来，和当前节点比大小，小进左子树，大进右子树，重复到叶节点。这个比较过程其实就是空间切分——每次比较都切掉一半的问题空间，进入另一侧子空间继续搜索。kd-tree 在 ICP 里的价值，就是建立后一帧点云在前一帧点云中的快速最近邻索引。没有这个索引时，每个点查询都可能访问全部数据，开销巨大；有了树结构，平均查询复杂度从 O(NM) 降到 O(N log M)，属于典型的拿空间换时间。

用文字描述一下构建流程：从所有点中选一个维度做切分，取该维度中位数的点作为根节点；小于中位数的点进入左子树，大于的进入右子树；然后在子空间中递归重复这个过程，直到每个节点里只有一个点或达到停止条件。切分维度的选择通常取方差最大的维度，这样可以让空间划分更均衡，降低树的深度。

3.2 knnsearch 与指派问题：把乱序查找变成有序配对

改进算法的核心操作在 MATLAB 里就三行：先建树，再查询，后映射。

kdtree = KDTreeSearcher(Q); % 用参考点集 Q 构建 kd-tree [nearestPoint, ~] = knnsearch(kdtree, P, 'K', 1); % 为 P 中每个点找最近邻 mapPoint = Q(nearestPoint, :); % 用索引映射出真实的对应点坐标

这里的指派问题，可以理解为给每个源点指派一个唯一的最优对应点。传统 ICP 用"全局最近"隐式配对，一堆点可能指向同一个目标点；改进算法用 knnsearch 的 K=1 最近邻，让每个源点独立获得一个最近的参考点，再配合距离阈值剔除明显过远的错误配对，从而缓解一对多的情况。mapPoint 这个映射矩阵是后续 SVD 求解的输入，它直接决定了变换矩阵的质量。

需要注意，kd-tree 找的是每个点独立的最近邻，它不保证全局一一对应。所以论文里说的"指派问题相结合"，实际落地手段是：kd-tree 加速近邻搜索 + 阈值过滤错误匹配 + 必要时打乱源点顺序打断系统偏差。这三层下来，配对准确性比纯暴力搜索高不少。

3.3 SVD 分解求解变换：从协方差矩阵到 R 和 t

配对完成后，需要用 SVD 从对应点关系里解出旋转矩阵 R 和平移向量 t。步骤先算两组点云的质心：

μP = (1/n)Σpi，μQ = (1/n)Σqi

然后将两个点云分别去中心化——每个点减去自身质心，让点云中心平移到坐标原点，得到 P' = {pi − μP}，Q' = {qi − μQ}。这一步的目的是把旋转和平移解耦，先解旋转，再解平移。

接着求去中心后两组点集的协方差矩阵 W = Σqi'·(pi')^T，对 W 做 SVD 分解 W = UΣV^T。当 W 满秩时，旋转矩阵有唯一解 R = UV^T。最后把旋转矩阵代回去，平移向量由质心关系求出 t = μQ − RμP。这样做的好处是绕开了非线性优化的迭代，直接用闭式解求出变换参数，数值稳定性也更好。关键细节是处理反射，SVD 解出的 R 可能包含镜像变换（行列式为负），这时需要翻转 V 的最后一列再重新计算 R，保证输出的是纯旋转。

3.4 动态阈值与迭代收敛

改进算法的停止条件用了动态阈值，以相邻两次迭代的点云均方误差作为判据。核心逻辑是每次迭代后计算当前源点云与对应点之间的平均距离 d，当 d 小于预设阈值时停止迭代。对应到代码里就是：

d = sum(sum((P - mapPoint).^2, 2)) / n; % 计算平均距离误差 fprintf('迭代次数：%d, 误差为：%f\n', j, d);

这个阈值的作用类似铰链：设置太大，迭代提前结束，配准精度不足；设置太小，噪声点会拖住算法无法收敛，进入死循环。原代码里保留了 while j < 100 的迭代次数上限作为兜底，但注释给关掉了。实际跑的时候，我一般会把迭代上限加回来，两条判据同时生效，避免数据质量差时卡死。

动态阈值的"动态"体现在它每次迭代都在重新评估当前误差，只收敛到真实最优解附近才会停；相比固定迭代次数，它能自适应数据规模和噪声水平。但这也意味着它对阈值参数敏感，后面第 5 章的避坑会重点讲这个问题。

4. 核心代码逐段讲解：从 delaunay 到 knnsearch 再到 SVD 求解

4.1 icp.m 函数骨架：参数默认值与输入检查

论文末尾附带了两段核心代码。第一段是一个完整的 icp.m 函数文件，接口设计得很规整，支持多种鲁棒准则：

function [TR, TT, data] = icp(model, data, maxIter, minIter, critFun, thres) % 参数说明： % model - 参考点云，矩阵 [X_1 X_2 ... X_M] % data - 目标点云，矩阵 [P_1 P_2 ... P_N] % maxIter - 最大迭代次数，默认 100 % minIter - 最小迭代次数，默认 5 % critFun - 准则函数：0 最小二乘 | 1 Huber | 2 Tukey | 3 Cauchy | 4 Welsch % thres - 迭代停止阈值，默认 1e-5 if nargin < 6 thres = 1e-5; if nargin < 5 critFun = 0; if nargin < 4 minIter = 5; if nargin < 3 maxIter = 100; end end end end

这套参数设计很实用。maxIter 控制最大迭代上限，防止死循环；minIter 保证最少迭代次数，防止一开始就误判收敛；thres 是相邻两次迭代的误差差值阈值，小于它就停；critFun 是最关键的设计——默认为 0 是最小二乘，换到 1 到 4 就能切换成 Huber、Tukey、Cauchy、Welsch 四种鲁棒准则。这意味着同一套 ICP 框架，面对不同噪声类型时不需要改主体代码，换一个参数就行。这一点在后面二次开发章节还会用到。

紧接着是维度检查逻辑。代码里用 if size(model,2) < size(model,1) 来判断输入点是按行组织还是按列组织，并设置 mTranspose 标志。这个检查看着不起眼，但曾经坑过不少人——MATLAB 里矩阵按行还是按列组织，直接决定后续矩阵乘法能不能对上维度。函数里还检查了 model 和 data 的特征维度必须一致，否则直接 error 退出。

4.2 最近点搜索的路径选择：三角剖分查询还是暴力循环

icp.m 内部对最近点搜索做了分支处理。当维度小于 4 时，用 Delaunay 三角剖分构建索引；否则退化为暴力双层循环。这里贴的是二维情况的建树与查询代码：

if m < 4 if mTranspose DT = delaunayTriangulation(model); % 输入按行组织时直接建 else DT = delaunayTriangulation(model'); % 输入按列组织时转置后建 end else DT = []; % 高维时三角剖分不可用 end if isempty(DT) % 暴力循环：每个目标点遍历所有参考点找最近邻 for i = 1:N mival = 9e99; for j = 1:M val = norm(data(:,i) - model(:,j)); if val < mival mival = val; vi(i) = j; resid(i) = val; end end end else [vi, resid] = nearestNeighbor(DT, data'); % 三角剖分快速查询 % vi 存储最近参考点的索引 % resid 存储最近点之间的欧氏距离 end

这个分支设计符合实际：delaunayTriangulation 在 MATLAB 里主要支持二维和三维，维度高了建不了，只能走暴力路线兜底。vi 是索引数组，resid 是残差数组，两者配对使用就是后面 SVD 求解的输入对应关系。原作者在注释里特别标注了"nearestNeighbor 和 DT 相结合增加了 Robust 性质"，同时提醒多对一问题在这条路径上会更明显——这也是改进版选择 KDTreeSearcher 的原因之一。

4.3 SVD 变换估计：协方差矩阵与反射处理

配好对之后，进入变换矩阵求解环节。icp.m 里 critFun=0 的最小二乘分支写得最精简，也是理解整个 SVD 求解流程的最佳入口：

res = mean(resid.^2); % 当前均方误差 med = mean(data, 2); % 目标点集质心 mem = mean(model(:, vi), 2); % 对应参考点质心，vi 是索引 C = data * model(:, vi)' - (N * med) * mem'; % 去中心化后的互协方差矩阵 [U, ~, V] = svd(C); % 奇异值分解 Ri = V * U'; % 旋转矩阵 if det(Ri) < 0 V(:, end) = -V(:, end); % 反射修正：翻转 V 的最后一列 Ri = V * U'; % 重算旋转矩阵 end Ti = mem - Ri * med; % 平移向量

这段代码的精妙在去中心化的实现方式：data * model(:,vi)' 是两组点云的原始互相关，减去 (N·med)·mem' 就等价于先把两组点云各自减去质心再算协方差。SVD 分解后，旋转矩阵 R = V·U' 是闭式最优解。注意 if det(Ri) < 0 这个判断——当 SVD 解出的矩阵是反射而不是旋转时，翻转 V 的最后一列后再乘回去就是纯旋转。

平移向量的计算也跟着质心走：Ti = mem − Ri·med。因为旋转分量已经求出，平移就是参考点云质心减去旋转后的目标点云质心。整个求解过程没有用到任何非线性优化迭代，一步到位，数值稳定性好，这也是 SVD 方法成为点云配准主流求解手段的原因。

4.4 改进版脚本完整解读：KDTreeSearcher 最近邻 + 动态阈值

第二段代码是改进版的核心脚本，它把 kd-tree 查最近点、SVD 解变换、动态阈值收敛三条逻辑串了起来。原文代码有 OCR 噪音，这里做了清理和格式化：

clear; close all; clc; % 读取源点云数据 P 和目标点云数据 Q P = importdata('face2.txt'); P = P(:, 1:3); % 人脸数据取前三列坐标 Q = importdata('face3.txt'); Q = Q(:, 1:3); ShowData(P, Q); % 可视化原始点云（需自己实现） n = size(P, 1); % 源点云点数 j = 0; d = 100; tic kdtree = KDTreeSearcher(Q); % 用参考点集 Q 构建 kd-tree while d > 0.0005 % 动态阈值：平均距离降到阈值以下停止 % while j < 100 % 可选的迭代次数上限，原代码注释掉了 j = j + 1; % 1. kd-tree 最近邻搜索：为 P 中每个点找 Q 中的最近点 [nearestPoint, ~] = knnsearch(kdtree, P, 'K', 1); mapPoint = Q(nearestPoint, :); % 映射出对应点坐标 % 2. SVD 求解旋转矩阵 R 和平移向量 T centerP = mean(P); % P 点集质心 centerMap = mean(mapPoint); % 对应点集质心 tempP = P - centerP; % 去中心化 tempMapPoint = mapPoint - centerMap; H = tempP' * tempMapPoint; % 协方差矩阵 H [U, ~, V] = svd(H); % SVD 分解 R = V * U'; % 旋转矩阵 T = -R * centerP' + centerMap'; % 平移向量 % 3. 应用变换，更新源点云 P = (R * P' + T)'; % 等价于 P = R * P + T % 4. 计算新的平均距离误差 d = sum(sum((P - mapPoint).^2, 2)) / n; fprintf('迭代次数：%d, 误差为：%f\n', j, d); end toc ShowData(P, Q); % 可视化配准结果

逐段拆解这段脚本的逻辑。

第一步 kd-tree 建树与查询：KDTreeSearcher(Q) 以 Q 为数据构建 kd-tree 索引结构；knnsearch 的 'K', 1 参数表示每个源点只找最近的一个参考点。这是改进算法最核心的替换——相比 icp.m 里的 delaunayTriangulation，kd-tree 对维度不敏感，人脸数据取三维坐标也能直接处理，而且空间切分更均匀。mapPoint = Q(nearestPoint, :) 用一个索引矩阵把参考点坐标取出来，和源点 P 形成一一对应的配对关系。

第二步 SVD 求解：centerP 和 centerMap 分别是 P 和对应点集的质心，去中心化后算协方差矩阵 H = tempP' * tempMapPoint。注意这里的 H 和 icp.m 里的 C 略有差异，一个是 tempP' * tempMapPoint，一个是去中心化后的 p'·q 互相关形式，但本质都是互协方差矩阵，SVD 解出的 R = V·U' 是同一个思路。T = -R·centerP' + centerMap' 由质心关系直接推出。

第三步应用变换：P = (R * P' + T)' 这行看起来绕，其实是 MATLAB 行向量组织下的紧凑写法。P 是 N×3 行矩阵，P' 变成 3×N，左乘 R（3×3）得到旋转后的结果，加上 T（3×1），再转置回 N×3。等价于 P = R·P + T 逐个点应用变换。

第四步收敛判断：d 是均方误差的平均值，逐点求 P 与 mapPoint 的距离平方，累加后除以点数 n。当 d 小于 0.0005 时循环退出。这个阈值对 sin 曲线和正方向量级的数据没问题，但对人脸这种密集点云，噪声稍多就可能卡死。

代码最后用 tic/toc 包住整个迭代过程，可以精确统计配准耗时。实验结果显示，改进版在脸部点云上的配准效果比传统 ICP 显著提升，迭代次数明显减少——这正是 kd-tree 带来的最直接收益。

5. 运行避坑指南：阈值、数据维度与点云顺序的踩坑记录

5.1 坑一：importdata 读 txt 读成了结构体，脚本第一行就翻车

现象：P(:, 1:3) 直接报错，提示下标类型不对；或者 P 变成了带表头的单元格数组，取出来的数据根本不是数值矩阵。

原因：face2.txt 如果带注释行、列名或者混合格式，importdata 会自动返回 struct 或 cell，而不是纯数值矩阵。很多人第一次跑这段脚本就栽在这一步，还以为是自己环境的问题。

解决：换成 readmatrix('face2.txt') 或 dlmread('face2.txt')，纯数字文件直接读出矩阵。读完之后先 size(P) 确认维度方向，再进入后续流程。如果数据确实带表头，用 readmatrix 的 'NumHeaderLines' 参数跳过即可。

5.2 坑二：点云按行组织还是按列组织，转置搞混直接维度爆炸

现象：SVD 分解时输入矩阵维度不匹配，直接 error 退出；或者 R、T 算出来明显不对，点云在迭代中飞出去。

原因：icp.m 函数内部用 mTranspose 标志处理了行列组织问题，但改进版脚本里没有这个保护。如果自己换数据时把 P 和 Q 读成了不同的组织方向，H = tempP' * tempMapPoint 的乘法结果维度就错了。

解决：统一约定 N×D 行矩阵组织，P 是 n 行 3 列，每一行是一个点。改进版脚本里全是按这个约定写的，换数据时注意保持。如果从 icp.m 函数拿输入，先按它的 mTranspose 逻辑判断一下，或者直接加一个 if size(P,2) < size(P,1)，P = P' 的保险。

5.3 坑三：动态阈值设太小，while d > 0.0005 跑成死循环

现象：迭代次数跑到几百次，误差曲线震荡不收敛，脚本卡住无法结束。

原因：人脸点云含噪声，平均距离可能永远降不到 0.0005 以下。原代码里 while j < 100 的迭代上限被注释掉了，失去兜底机制，阈值又定得太小，就变成死循环。这个我在跑人脸数据时踩过，迭代了六百多次误差还在 0.0006 左右晃。

解决：把迭代上限加回来，改回 while j < 100 && d > 0.0005 的双条件；或者把阈值放宽到 0.001~0.002 量级，先看配准效果再逐步收紧。另外可以在配对前做一次简单的离群点剔除，用 knnsearch 查每个点的近邻距离分布，距离超过 3σ 的直接删掉。

5.4 坑四：暴力最近邻搜索跑人脸点云，慢到怀疑人生

现象：脸部点云几千个点，传统 ICP 每轮迭代要做 N×M 次距离计算，一轮就要几十秒，整体配准耗时让人无法接受。

原因：icp.m 函数在高维分支里退化为双层 for 循环遍历所有点，没有空间索引加速。人脸点云规模下这个开销是 O(NM)，而且每轮迭代都要重算一次。

解决：直接用改进版脚本里的 KDTreeSearcher + knnsearch 替换暴力搜索，复杂度降到 O(N log M)。这个替换是改进算法提升速度的核心因素，实测迭代次数不变的情况下，单轮耗时能下降一个数量级。

5.5 坑五：多对一在 kd-tree 改进后也没完全消失

现象：mapPoint 里出现多个源点映射到同一个参考点的情况，配准精度比预期差，但误差指标已经收敛了。

原因：kd-tree 找每个源点独立的最近邻，它本质上不保证全局一对一。当点云密度不均或者初始位姿偏差较大时，局部区域还是会出现多个源点抢同一个参考点的现象，这时 SVD 解出的变换矩阵会被这些重复匹配加权拉偏。

解决：两个手段。一是配对后做一个去重过滤，遍历 mapPoint 的索引，重复索引只保留欧氏距离最小的那对，其余丢弃；二是打乱源点顺序再配对，打断系统偏差——原论文代码注释里也提示了"可以尝试提前打乱顺序再进行配对"。我实际测试，先过滤重复索引再进 SVD，配准精度能提升明显。

6. 二次开发思路：把二维改进推广到三维与多鲁棒准则

这份资源的扩展空间其实不小。先说最直接的：把算法从二维测试数据推广到三维真实点云。数据集替换上，face2.txt 换成 PCD 或 PLY 文件，MATLAB 里用 pcread 读成 pointCloud 对象，取 Location 属性赋给 P 和 Q。有意思的是，改进版代码里的 KDTreeSearcher 和 SVD 分解对维度天然不敏感，前三列三维坐标直接接进现有循环就能跑，算法主体改动很小。如果要做严格二维实验，P = P(:,1:2) 即可，delaunayTriangulation 本来就是二维三角剖分，取前两维更贴合论文标题。

鲁棒准则的切换值得单独说。icp.m 函数里的 critFun 参数是个隐藏功能，但改进版脚本没有继承这个接口。二次开发时可以把改进版的误差计算和权重更新部分接到 icp.m 的 critFun 分支上：点云噪声符合高斯分布时用最小二乘（critFun=0）就够了；有离群点时换 Huber（critFun=1），它在残差超过阈值后把损失函数从平方换线性，抑制大残差的拉偏；重尾噪声分布用 Cauchy（critFun=3）或 Welsch（critFun=4），它们的权重衰减更快。接入方式不复杂，在每轮配对后加一个权重向量 wghs，将 H = tempP' * (tempMapPoint .* wghs) 即可。

动态阈值也可以改成自适应。当前版本用固定阈值 0.0005，我一般会改成前 30% 迭代放宽两倍，后程逐步收紧，类似模拟退火的温度曲线。这样前期给算法足够空间逃离错误方向，后期精细收敛。验证方法上，建议用带扰动的多次初始化测试稳定性——给初始 R 加一个 ±5° 的角度扰动，连续跑 10 次，看是否收敛到同一结果并记录迭代次数和最终 MSE。从那以后我每次跑配准前都会先画一下原始点云分布，看一眼数据量级和噪声情况再决定阈值，这个习惯帮我避开了很多玄学问题。希望帮到你。

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