MATLAB二维点云配准：ICP+kd-tree+SVD实现

简介这份面向点云配准初学者与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通用 ICP 函数传统算法基线delaunayTriangulation、nearestNeighbor、svdkd-tree 改进脚本主流程读数据到画图KDTreeSearcher、knnsearch、svdface2.txt/face3.txt源点云与目标点云样例纯文本每行一个点论文LaTeX 排版算法推导、伪代码、实验结果—ShowData.m示例中调用可视化辅助函数资源未附需自写icp.m和改进脚本之间是并列关系不是调用关系。前者是通用实现后者是针对 kd-tree 方案重写的一版两段代码各自独立你可以只跑其中一段做对比。这也是论文里传统 vs 改进对比实验的做法——同样的数据喂两边比较迭代次数和最终残差。要注意ShowData(P, Q)这个函数在片段里出现了两次但资源没给它的实现。自写一个很简单scatter两把不同颜色就行第 3.3 节给代码。3.2 每一步的点云形状与数值变化点云的矩阵形状是这类代码最容易翻车的地方MATLAB 里行是点、列是维度跟很多 C 点云库反着来。整条链路里形状变化如下阶段P源Q参考说明读入N×3M×3文本每行一个三维点截取N×2M×2二维配准只保留 XY建树—M×2树建在参考点集上查询N×1 索引—每个 P 点拿到一个 Q 的行号映射N×2 (mapPoint)—用索引把 Q 展开成与 P 等长去中心N×2N×2各自减质心协方差2×2—tempP * tempMapPoint或反向输出N×2M×2变换后的 P 叠加原 Q关键在于映射这一步knnsearch返回的是索引向量必须用它去索引参考点集把 Q 展开成和 P 一一对应的 N×2 矩阵后面的质心计算和协方差才有意义。忘了这一步直接拿 Q 的原始 M×2 去减 P维度一不一致就报错运气好两边数量相等那更惨代码不报错但算的是两堆毫无对应关系的点。矩阵的转置方向也要盯住。协方差矩阵有两种写法tempP * tempMapPoint2×N 乘 N×2 得 2×2和P * mapPointN×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 或 PCDMATLAB 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表示只取最近的一个邻居。理论上取 K1 可以做加权平均让匹配点在多个候选之间平摊缓解多对一但那样每次迭代的计算量和匹配语义都变了属于二次开发的内容基线上保持 K1 才跟论文的对比实验口径一致。建树是一次性的迭代过程中树的拓扑不变——因为参考点集 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×2P 的质心行向量 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×NQ0是 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×2t 是 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 是点云维度二维配准时 m2 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查询点到最近邻的欧氏距离 enddelaunayTriangulation的输入是点坐标矩阵行是点、列是维度所以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)两行日志的事能省掉一晚上的折腾。希望帮到你。本文还有配套的精品资源点击获取