简介：面向点云配准学习者的 MATLAB 实现资料，基于改进 ICP 算法，将 kd-tree、指派问题与 delaunay 三角剖分相结合，用于提升二维点云配准的精度与速度，适合计算机视觉、三维几何处理方向的学生及相关算法初学者。资料源自作者的大二期末大作业，结构完整，适合课程设计参考，也可作为算法改进的入门阅读。资源为 1 个 PDF 文档，压缩包 1.59MB；文档详细阐述传统 ICP 匹配不精准、易受特殊点干扰、陷入局部最优等不足，并给出 kd-tree 加速最近邻搜索、指派问题优化对应点匹配、动态阈值剔除误匹配以及三角剖分增强鲁棒性等改进思路。文中包含 MATLAB2022 环境下的核心函数代码（未提供完整 .m 文件）与伪代码，配合 sin 曲线、随机点云、脸部点云等配准前后对比图，便于复现与观察效果。已有 1350 人学习，对理解点云配准原理及算法改造实践均有参考价值。

1. 二维点云配准实战：kd-tree与三角剖分如何救活传统ICP

拿到两片同一物体不同角度的点云，最朴素的想法是叠在一起看哪儿哪儿不对——旋转差几度、平移差几毫米，肉眼又调不准。这正是点云配准要解决的问题：找一个刚性变换把两片点云对齐。这个项目是某高校大二期末大作业完成的改进ICP算法，基于MATLAB2022实现。核心思路是先用kd-tree给点云建立索引，再用三角剖分与最近邻搜索配合完成匹配点“指派”，最后用SVD分解求旋转矩阵和平移向量。与传统ICP相比，它把面部点云这种细节丰富的场景从“经常翻车卡局部最优”救成了“几十次迭代内收敛”。适合正在做点云配准课程设计、或者想理解ICP原理并动手改一版算法的同学拿来拆解。

2. 传统ICP算法拆解：delaunayTriangulation、最近邻搜索与三大痛点

2.1 刚性配准与三角剖分：为什么delaunay能增强鲁棒性

配准分两类：刚性配准和非刚性配准。刚性配准只允许旋转和平移，不改变两点之间的距离，这对应物体本身不变、只是采集角度或位置不同的场景。ICP（Iterative Closest Point，迭代最近点）就是刚性配准里最有名的迭代方法：每轮为每个点找最近点，算出变换，更新点云，再继续找最近点，直到收敛。它简单、好用，但传统实现有两个天生的坑：一是最近点搜索全是暴力遍历，数据量一大就慢；二是匹配关系经常“多对一”——多个源点找同一个目标点当对应点，优化方向直接被带偏。项目里用MATLAB内置的delaunayTriangulation给点云做三角剖分，再用nearestNeighbor做最近邻查询，这是一个非常取巧的加速方案。

Delaunay三角剖分把平面点集分割成“最匀称”的三角形集合，核心特性包括：最接近——以最近的三点形成三角形且各线段不相交；最优性——任意两个相邻三角形形成的凸四边形，对角线互换后六个内角的最小角不会变大；最规则——将所有三角形的最小角升序排列，Delaunay三角网得到最大数值；区域性——新增、删除或移动某个顶点只影响邻近三角形。这些特性让它在二维配准中天然适合构建空间邻域结构。MATLAB的delaunayTriangulation接受“每行一个点”的坐标矩阵，返回三角剖分对象；nearestNeighbor(DT, queryPoints)一次调用就能返回每个查询点所在三角形的最近顶点索引和欧氏距离，比for循环暴力搜索快几个量级。需要注意，三角剖分只适用于m<4的维度，也就是二维或三维点云，高维数据得另想办法。

2.2 迭代最近点的四步流程与无约束优化模型

传统二维ICP的一般步骤如下。步骤一：选取参考点集和目标点集。步骤二：遍历目标点集中所有点，在参考点集中选择欧氏距离最小的点作为对应点。步骤三：建立目标函数并优化求解，得到旋转矩阵R_j和平移矩阵T_j，进而得到新的目标点集。步骤四：判断是否达到最大迭代次数或误差条件，是则停止迭代并输出结果，否则回到步骤一继续迭代。

对应的无约束优化模型是：

min E = Σ_{i=1}^{n} || q_i − (R·p_i + T) ||²

其中p_i是源点云中的点，q_i是目标点云中对应的最近点，R是旋转矩阵，T是平移向量。这个目标函数在看板上是“变换后的点到对应点的欧氏距离平方和”，本质上是一个最小二乘问题。为什么叫无约束？因为对R和T没有正交性以外的限制，只要R是旋转矩阵即可。传统ICP把“找对应点”和“求解最优变换”这两个子问题交替迭代，每个子问题单独看都不难，难在它们耦合在一起，而且对应点本身可能找错。

2.3 核心代码解析：nearestNeighbor加速与SVD求解变换

传统ICP的工程代码（函数文件）核心部分如下。我做了裁剪，保留建树、最近邻搜索和SVD求解三个关键段。

% 默认参数 thres = 1e-5; % 迭代停止阈值 critFun = 0; % 0=最小二乘, 1=Huber, 2=Tukey, 3=Cauchy, 4=Welsch maxIter = 100; % 最大迭代次数 minIter = 5; % 最小迭代次数 % 创建最近点搜索结构 if m < 4 if mTranspose DT = delaunayTriangulation(model); % model 每行一个点 else DT = delaunayTriangulation(model'); % 转置成每行一个点 end else DT = []; % 高维不适用三角剖分 end % 主迭代 for iter = 1:maxIter oldres = res; % 为每个数据点找最近模型点：vi存索引，resid存欧氏距离 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 % 三角剖分加速：nearestNeighbor 一次查完所有点 [vi, resid] = nearestNeighbor(DT, data'); end % 最小二乘求解：协方差矩阵 + SVD med = mean(data, 2); % 目标点云质心 (m×1) mem = mean(model(:, vi), 2); % 模型对应点质心 (m×1) C = data * model(:, vi)' - (N * med) * mem'; % 协方差矩阵 (m×m) [U, ~, V] = svd(C); Ri = V * U'; if det(Ri) < 0 V(:, end) = -V(:, end); % 防止反射矩阵 Ri = V * U'; end Ti = mem - Ri * med; % 平移向量 % 应用变换，更新累积矩阵 data = Ri * data; for i = 1:m data(i, :) = data(i, :) + Ti(i); end TR = Ri * TR; TT = Ri * TT + Ti; % 收敛判断 res = mean(resid.^2); if iter >= minIter && abs(oldres - res) < thres break; end end

逻辑说明：这段代码把传统ICP的三件套体现得很清楚。先判断维度m是否小于4，决定用delaunayTriangulation建三角剖分还是退化成暴力搜索。nearestNeighbor查询时不只是找最近点，而是先定位查询点落在哪个三角形面片里，再从该面片顶点中找最近者，相当于用三角剖分做了空间加速和邻域约束。随后构造协方差矩阵C，核心技巧是直接对原始坐标做叉乘再减去均值项，等价于去中心化后的协方差。对C做SVD分解得到R=VU'，如果行列式为负说明求出的是反射矩阵，翻转V的最后一列再重算。平移向量用质心差公式T=mem-Rmed。

参数说明：m是点云维度，二维时m=2，三维时m=3；mTranspose是model矩阵转置标记，确保传给delaunayTriangulation的矩阵是“每行一个点”；N是目标点云点数，M是模型点云点数；data是m×N矩阵（每列一个点），model是m×M矩阵；如果数据传成“每行一个点”的N×m格式，代码开头会自动转置。critFun=1到4对应Huber、Tukey、Cauchy、Welsch四种稳健准则，用IRLS给每个匹配点加权，能抑制离群噪声，但代价是每次迭代都要多做一轮权重更新。

2.4 三组实验结果暴露的痛点：多对一、远距离点对、局部最优

项目里用三组数据做了传统ICP实验：sin曲线点集、正方形随机点云、脸部点云。从配准图能直观看到三个问题。

第一个是多对一匹配。目标点集里多个点同时对应参考点集的同一个点，迭代方向被反复拉扯，误差曲线不收敛。更麻烦的是，结合三角剖分之后多对一现象反而加剧——三角形顶点会被多个面片共享，多个查询点落在同一个顶点附近时都把它当作最近点。第二个是部分匹配点对的欧氏距离过大。个别离群点或噪声点给目标函数贡献了很大的误差项，一次迭代就能把优化方向带偏，迭代次数暴增。第三个是局部最优。传统ICP的最近邻搜索是贪婪的，每轮只优化当前最近邻下的变换，初值给得不好就直接掉进局部极小值，配准结果整体错位。

这三个问题本质上都出在“点云对应点的指派”上。所谓指派，就是确定源点云里的每个点到底该配目标点云里的哪个点。改进算法要做的，是把“最近点+最小二乘”的朴素组合，升级成“kd-tree空间索引+更严格的匹配约束+动态阈值”，对应到代码里就是KDTreeSearcher、knnsearch和循环阈值判断。

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

3.1 kd-tree数据结构：从二叉查找树到高维空间切分

kd-tree（K-dimensional tree）是一种高维索引树形结构，每个节点都是k维点的二叉树。所有非叶子节点可以看作一个将空间分割成两个半空间的超平面：节点左子树代表在分割维度上小于超平面的点集，右子树代表大于超平面的点集。建树过程类似二叉查找树：来一个节点就与当前节点比较切分维度上的大小，小进左子树，大进右子树，重复直到到达叶节点。这个过程本身就是空间切分——每次比较都切掉一半问题空间，进入另一侧的子树。

本科熟知的二叉查找树其实就是1维kd-tree。把它推广到k维，就是kd-tree。对ICP来说，最耗时的步骤是“给每个点找最近点”。数据量大的时候，每次查询都可能访问全部数据，代价是O(M·N)。kd-tree把这个过程变成O(logN)的树搜索，本质是拿空间换时间，用分治思想给高维数据建立索引。项目里用MATLAB的KDTreeSearcher对目标点云建树，再用knnsearch查询，这一步直接替换掉传统ICP里的暴力循环。kd-tree的另一个好处是它不依赖维度是否小于4，三维、四维点云都能直接用，比delaunayTriangulation适用面更广。

3.2 指派问题与knnsearch：一对一匹配怎么落地

“指派问题”原本是运筹学里的概念：有n个任务要分配给n个人，每个分配都有代价，目标是总代价最小。借用这个思路，点云配准里的指派问题就是：让每个源点匹配一个最合适的目标点，尽量避免多个源点抢同一个目标点。这不是简单的最近邻搜索，因为最近不代表全局最优。kd-tree在这里的作用是快速给出“候选最近点”，而配对准确性通过限制查询条件和后续的剔除逻辑来保证。

MATLAB里的实现非常直接：

% 用目标点云Q建树 kdtree = KDTreeSearcher(Q); % 查询P中每个点在Q中的最近邻 [nearestIdx, dist] = knnsearch(kdtree, P, 'K', 1); % 通过索引取对应点坐标 mapPoint = Q(nearestIdx, :);

逻辑说明：KDTreeSearcher按点云坐标自动切分空间，建树后knnsearch查询P中每行点在Q中的最近邻。返回的nearestIdx是Q中的行索引，dist是对应的欧氏距离；mapPoint是Q中这些索引对应的坐标矩阵，也就是P的“当前对应点集”。相比传统ICP里delaunayTriangulation+nearestNeighbor的组合，KDTreeSearcher对数据维度没有限制，三维也能稳定建树；而knnsearch的返回值更直白，索引和距离一次拿到。传统ICP里通过delaunay找最近点的做法，到这里其实已经被kd-tree取代——三角剖分在这个过程中扮演的是“空间邻域约束”的配角，kd-tree负责“索引加速”的主角。

参数说明：knnsearch的'K'参数控制每个查询点返回几个近邻。K=1就是严格取最近的一个，这是最朴素的指派；如果把K改成5，拿到的就是5个候选点，可以再按距离阈值或空间方向挑一个最合理的，这是抑制多对一误匹配的常用手段。dist变量顺手就能当残差用，配合动态阈值判断迭代是否稳定。需要说明的是，这里的“指派”并未真正求解全局最优的匈牙利算法，而是用kd-tree最近邻快速逼近一个合理的一一对应关系，这是工程上更常见的取舍。

3.3 SVD分解求R和T的数学流程

对应点确定之后，求旋转矩阵和平移向量有标准流程，分四步。

第一步，计算两个点集的质心并中心化。设P和Q的质心分别为：

μ_P = (1/n)·Σp_i，μ_Q = (1/n)·Σq_i

中心化后的坐标为：

P' = {p_i − μ_P}，Q' = {q_i − μ_Q}

第二步，构造协方差矩阵：

W = Σ q_i'·(p_i')^T

第三步，对W做SVD分解：

W = UΣV^T

当W满秩时旋转矩阵有唯一解R = U V^T。注意这里用的是U和V直接相乘，不需要中间的Σ。

第四步，反推平移向量：

t = μ_Q − R·μ_P

整个过程用MATLAB的svd函数实现，代码只有几行，数值稳定性远好于手动解Euler角或四元数。这也是项目里选择SVD而不是其他参数化方式的原因。需要特别注意的是，如果R的行列式为-1，说明求出来的是反射而非旋转，必须对V的最后一列取反再重算，否则点云会变成镜像。

3.4 改进算法主循环与伪代码

把上面的模块拼起来，改进ICP的主循环就清晰了。完整流程写成伪代码：

Algorithm 1 KD-tree + ICP(P, Q) Input: 源点云P，目标点云Q Output: 旋转矩阵R，平移向量T 1: 根据目标点云Q建立kd-tree 2: 在kd-tree中搜索P中每个点的最近邻，得到匹配点对 3: 用SVD分解计算最佳旋转矩阵R和平移向量T 4: 应用变换更新P，计算平均距离误差d 5: 若d大于阈值则回到步骤2，否则返回R、T

和传统ICP最大的区别在第2步和第5步。传统版本用delaunay+nearestNeighbor找对应点，固定迭代次数作为终止条件；改进版用KDTreeSearcher+knnsearch找对应点，用平均距离误差做动态阈值。动态阈值的意思是阈值不随迭代固定不变，而是每一轮用当前匹配关系下的平均残差判断是否收敛，噪声多时不会被个别离群点卡死，噪声少时又能快速收敛。这个阈值就是代码里的d>0.0005，单位取决于点云坐标的量纲。

4. 落地运行：MATLAB2022环境下的代码实现与效果对比

4.1 数据准备：face2.txt与face3.txt的读取与清洗

实验数据是两片人脸面部点云，存成txt文本文件，每行一个点的坐标。face2.txt作为源点云P，face3.txt作为目标点云Q。读取时有个细节要注意：文件里可能有非坐标列，比如法向量或颜色值，所以要用P=P(:,1:3)只取前三列，确保拿到的是xyz坐标。项目环境是MATLAB2022 + Windows10，CPU是Intel移动版i7-10710U，这个配置跑几百个点的配准完全没压力。

P = importdata('face2.txt'); P = P(:, 1:3); % 只取坐标列 Q = importdata('face3.txt'); Q = Q(:, 1:3);

逻辑说明：importdata会自动识别txt里的数值矩阵，但如果文件里有注释文字或表头，它返回的就不是纯矩阵而是struct，此时需要改用readmatrix。取前三列是为了防文件里混入法向量、强度、RGB等附加信息。如果不做1:3截取，后面算质心、协方差矩阵时维度对不上，MATLAB会直接报矩阵乘法错误，而且这种报错不太好排查。

4.2 完整运行代码：KDTreeSearcher+knnsearch+SVD迭代

改进ICP的核心运行代码集中在这段脚本里：

clear; close all; clc; P = importdata('face2.txt'); P = P(:, 1:3); Q = importdata('face3.txt'); Q = Q(:, 1:3); ShowData(P, Q); % 可视化原始两片点云 difference = zeros(size(Q,1), 3); % 坐标差值矩阵 mapPoint = zeros(size(P,1), 3); % 对应点映射矩阵 distance = zeros(size(Q,1), 1); % 距离记录矩阵 n = size(P, 1); j = 0; d = 100; kdtree = KDTreeSearcher(Q); % 用Q构建kd-tree tic; while d > 0.0005 % 动态阈值：平均距离误差 j = j + 1; % 找P中每个点在Q中的最近邻，得到对应点索引 [nearestIdx, ~] = knnsearch(kdtree, P, 'K', 1); mapPoint = Q(nearestIdx, :); % 取对应点坐标 % SVD求解旋转矩阵R和平移向量T centerP = mean(P); % P的质心 (1x3) centerMap = mean(mapPoint); % 对应点集质心 (1x3) tempP = P - centerP; % 去中心化 tempMap = mapPoint - centerMap; H = tempP' * tempMap; % 3x3协方差矩阵 [U, ~, V] = svd(H); R = V * U'; T = -R * centerP' + centerMap'; % 平移向量 (3x1) P = (R * P' + T)'; % 应用变换得到新点集 % 计算平均距离误差 d = sum(sum((P - mapPoint).^2, 2)) / n; fprintf('迭代次数：%d, 误差为：%f\n', j, d); end toc; ShowData(P, Q); % 可视化配准结果

逻辑说明：这个循环就是改进ICP的完整实现。先用KDTreeSearcher(Q)建树，每次迭代用knnsearch查P中每个点在Q里的最近邻，得到nearestIdx后mapPoint=Q(nearestIdx,:)把对应点坐标取出来。然后进入SVD求解：mean(P)和mean(mapPoint)分别计算两个点集的质心，减去质心完成中心化，H=tempP'tempMap得到3×3协方差矩阵，svd分解后R=VU'；平移向量用T=-RcenterP'+centerMap'求解，这和理论推导的t=μ_Q−R·μ_P完全等价，只是把centerP写成列向量再左乘R。P=(RP'+T)'是先对转置后的P'做旋转，再加平移，最后转置回每行一个点的格式。

参数说明：初值d=100是为了确保进入循环；while d>0.0005是动态阈值，单位是坐标平方，对面部点云来说0.0005已经足够精细，如果数据量纲是毫米，这个阈值意味着亚微米级误差。j记录迭代次数，实际跑面部数据一般几十次就收敛，比传统ICP动辄上百次明显减少。tic/toc统计循环耗时，可以直观对比加速效果。代码里预分配的difference、mapPoint、distance三个矩阵是给调试用的，mapPoint在循环里被真正使用，另外两个可以留着观察中间变量。

4.3 配准效果对比与可视化验证

项目里配了ShowData函数用于可视化。对比传统ICP的面部点云配准结果和改进版结果，改进版的点云重叠度明显更高，轮廓线更贴合，而且迭代次数大幅下降，这说明kd-tree索引加上匹配约束确实解决了传统ICP“对应点找不准”的核心问题。有效性主要来自三点：kd-tree让最近邻搜索不再是全量遍历，速度上去了；对应点指派更合理，多对一的情况减少，迭代方向更稳定；动态阈值在误差足够小时及时跳出，避免在局部最优里空转。

需要注意，这份资源里传统ICP是函数文件icp()，改进版是脚本文件，没有打包成完整的.m工程。拿到手后需要把两段代码分别保存，ShowData函数不在资源里，需要自己写一个简单的可视化函数，比如用MATLAB的plot3分别画P和Q，配不同颜色区分即可。

5. 避坑指南：ICP与kd-tree结合的常见问题排查

5.1 迭代发散或误差越算越大

现象：while循环跑了上百次，d值不但没降反而增大，最终卡死或配准结果完全错乱。

原因：knnsearch返回的最近邻里混入了大量错误匹配，尤其是两片点云初始位置差距很大时，最近邻根本对应不上；另一种可能是动态阈值设置太严格（比如1e-6），算法在局部极小附近反复横跳，误差不降反升。

解决：先做粗配准再跑改进ICP，最简单的方法是用质心对齐——把P和Q的质心挪到同一个点；或者用PCA求主方向，把两个点云的主轴先转到一致。阈值放宽到0.001先看收敛趋势，如果趋势没问题再收紧。另外可以在knnsearch之后加一个最大距离判断，超过某个距离的点对直接判为无效，不参与SVD求解。

5.2 多对一匹配仍然存在

现象：配准结果整体看起来对，但局部区域点云重叠处有“重影”，同一个目标点被多个源点对应。

原因：kd-tree只保证每个查询点找到“最近的点”，不保证匹配关系的全局唯一性。改进版通过先找最近再取坐标的方式，出现多对一的概率比暴力搜索低，但点云密度不均匀时仍然会发生，特别是在曲率变化大的地方。

解决：加一个配对占用表，让每个目标点最多被一个源点占用，类似贪心匹配；或者用knnsearch返回K=3个候选，再按距离加方向约束选最优；更稳妥的做法是互最近匹配——P查Q的最近点，Q再查P的最近点，两个方向互相指向才接受这对匹配。互最近匹配的代码量不大，但能显著减少误匹配。

5.3 SVD求出来的是反射矩阵

现象：R的行列式等于-1，配准结果是“镜像”而不是“旋转”，点云整体变形了。

原因：点云中心化后协方差矩阵不满秩或秩亏，SVD分解得到的U、V矩阵不是良态，导致R=V*U'混入反射分量。常见触发条件是点云近似共面、点数太少，或者两片点云初始姿态差异太大。

解决：每次算完R都检查一下det(R)，小于0就把V的最后一列取反再重算R。这个检测在二维点云尤其重要，因为三四个点就可能共面，协方差矩阵的秩很容易不够。上述完整代码里我写了这段，但第4章的运行脚本里没有加——实际跑面部点云时基本不会触发，但换成平面数据就必须加上。

5.4 读取txt时报错或维度对不上

现象：importdata('face2.txt')执行成功，但P(:,1:3)之后size(P,2)不等于3；或后续H=tempP'*tempMap时报维度不一致。

原因：txt文件里可能有多余列（法向量、时间戳、强度），或者文件前几行有注释文字，importdata读进来的不是纯数值矩阵而是struct，甚至可能把标头当数据读进来。

解决：读取后先用size看一眼形状，确认是N×M的数值矩阵；如果是struct，改用readmatrix或textread；如果文件里有注释行，用readmatrix时要指定NumHeaderLines参数跳过。还有一个习惯：无论原始文件长什么样，统一用P=P(:,1:3)截取前三列，后续代码就不会被多余列干扰。

5.5 死循环但实际已经收敛

现象：d值稳定在一个很小的数（如0.0006）不再下降，但while d>0.0005的判断永远为真，程序一直空转。

原因：阈值定得太死，浮点误差导致d永远差那么一点点，这就是“动态阈值”最常见的坑。我最初跑这份代码时，在这个坑里卡了大半天，最后发现单纯用绝对值判收敛不够用。

解决：在循环里加一个最大迭代次数上限，比如while d>0.0005 && j<1000；或者改成用变化量判断——记录上一轮的d，if abs(d - pre_d) < 1e-7则break。从那以后我写迭代优化循环，第一件事就是加“最大迭代次数+误差变化量”双保险，再也没有被死循环耗过时间。

6. 二次开发指南：把改进ICP扩展到自己项目的三种思路

6.1 维度扩展：从二维到三维点云

原始项目的脸面部数据实际上已经取了xyz三维坐标，算法核心逻辑对m=2和m=3都适用。改成三维只需注意两点：确认P和Q都是N×3矩阵；KDTreeSearcher默认用欧氏距离，三维和二维的计算方式完全一致，不用额外改。真正的区别在三角剖分——delaunayTriangulation在三维时要生成四面体，nearestNeighbor的查询逻辑和二维完全不同，所以如果坚持用传统ICP那套，三维建议直接跳过delaunay，改用KDTreeSearcher。这一点项目里的改进方向是对的：直接绕开delaunay依赖，用kd-tree做索引，天然兼容三维配准。

6.2 匹配策略升级：稳健权重与候选点约束

传统ICP代码里的critFun参数暴露了四种稳健准则：Huber、Tukey、Cauchy、Welsch。这些准则本质是给每个匹配点一个权重，离群点权重压到0附近，正常点权重接近1，比最小二乘的“一视同仁”抗噪声能力强很多。改进代码里用的是平方残差均值做阈值判断，如果数据里有明显噪声，我一般会把knnsearch的K从1改成3，用最近三点中距离最小且方向一致的那个作为对应点；或者把while d>0.0005改成加权残差，先用Huber准则迭代10轮做粗配，再切回最小二乘精配。这个组合拳在真实扫描数据上比单跑一版效果好很多。

6.3 效果验证：迭代次数、平均误差、旋转矩阵条件数

改造完不能只看图。我一般会算三个量化指标：收敛迭代次数、最终平均距离误差d、旋转矩阵条件数。如果d比改造前降了一个数量级、迭代次数从上百降到几十，说明kd-tree的加速和匹配约束起作用了。还有一个验证技巧：把变换后的P和Q画在同一张图里用不同颜色标注，叠加部分越多越好；再用MATLAB的plot3绕视角旋转一圈，从不同方向确认没有局部错位。这套改进代码本质是课程作业级别，直接用于工业级扫描配准还有差距，但作为理解ICP、kd-tree、SVD三者协作的完整链路，是很好的起点。希望帮到你。

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