2026/9/9 2:15:58
FEATURED · 精选文章 
1. 项目概述与核心思路1.1 SLAM里为什么绕不开ICP做SLAM的人不管你是搞激光的、搞视觉的还是搞多传感器融合的大概率都绕不开ICPIterative Closest Point迭代最近点。这东西听起来高深本质就是把两片点云“怼”到一起算出它们之间的旋转和平移矩阵。放在前端配准里它是里程计的核心放在后端回环里它是检测到回环之后精修位姿的工具放在建图环节里它是多帧点云对齐到全局坐标系的关键一步。尤其近几年多线激光雷达价格被打下来之后激光SLAM几乎是机器人导航入门的标配而激光SLAM里最经典、最基础的点云配准方案还是ICP及其各种变体。很多人一开始用PCL里的pcl::IterativeClosestPoint跑通demo容易但真到了自己的数据集上出现漂移、发散、匹配错乱时如果不了解底层实现根本不知道从哪个环节下手排查。这也是我写这篇文章的动机从零把ICP的核心代码完整实现一遍顺带讲清楚它为什么这么设计、怎么调参、怎么避免掉进常见的坑。这篇文章适合的人分两类。一类是刚开始接触SLAM、想把ICP原理吃透的同学你可以照着代码一步步跑通理解每一行在做什么另一类是在工程里被ICP折磨过的开发者看完之后你会明白之前参数调不动、结果反复横跳的深层原因。我不堆公式但核心推导会写清楚不贴大段无关代码但关键实现一定是能直接编译运行的。1.2 这次要实现的版本与工具链ICP从1972年诞生到现在衍生出的版本非常多点到点ICP、点到面ICPPoint-to-Plane、面到面ICPGICP以及带特征权重的Weighted ICP等等。这篇文章我会用C完整实现两个版本经典的Point-to-Point ICP和工程中最常用的Point-to-Plane ICP。前者是理解原理的最佳入口后者是实际项目里效果最稳的起点。开发环境方面我的测试机是Ubuntu 20.04编译器GCC 9依赖Eigen 3.3.7和PCL 1.10。Eigen负责矩阵运算和SVD分解PCL只用来做点云空间搜索和可视化。如果你已经装过ROS这两个库大概率都带了可以直接用。没装的话两条命令的事sudo apt install libeigen3-dev libpcl-dev本文的代码不依赖ROS写的是一个独立的C工程这样你随便丢到哪台Linux机器上都能编译理解起来也更纯粹。后面第3章我会单独说怎么把这段核心逻辑接到SLAM前端里那时候才涉及ROS话题。2. 核心原理与数学建模2.1 ICP要解决的问题本质先给一个直观场景。上一帧激光扫描得到一堆点这一帧又得到一堆点由于机器人动了这两堆点不在同一个坐标系下。ICP要做的就是找到一个刚体变换一个3x3旋转矩阵R和一个3x1平移向量t把第二帧的点变换到第一帧的坐标系下让两片点云的“重叠部分”尽可能重合。数学上这就是一个最小二乘问题E(R, t) (1/N) * Σ|| p_i - (R * q_i t) ||²其中p_i是目标点云比如上一帧中的点q_i是源点云当前帧中与p_i对应的点。这里最关键也最狡猾的问题是一开始我们根本不知道q_i对应p_i是哪个点。所以ICP的实际迭代策略是走两个交替步骤用当前位姿估计把源点云变换过去然后按空间最近邻原则查找对应点对根据找出来的点对重新求解一个更优的R和t更新位姿回到第1步直到收敛。所以ICP本质上是一个“假设-验证-修正”的循环。这也决定了它最大的软肋如果初值差得太远最近邻找出来的对应关系就是错的后面再怎么迭代也救不回来。这个后面详细讲。2.2 SVD求解旋转矩阵的完整推导ICP目标函数里旋转矩阵R的求解是整个算法的心脏。业界最常用的方法是基于SVD奇异值分解的闭式解比基于优化器的方法又快又稳。它的推导过程非常优雅值得完整看一遍。我们先定义两组点的质心p_mean (1/N) * Σ p_i q_mean (1/N) * Σ q_i然后做去质心处理得到每个点相对于质心的偏移p_i p_i - p_mean q_i q_i - q_mean把目标函数展开整理之后可以发现最优平移可以直接由质心和旋转决定t* p_mean - R * q_mean于是优化目标的重点就落在旋转R上问题变成最大化下面这个式子Σ (R * q_i)ᵀ * p_i trace(R * H)其中H (1/N) * Σ q_i * p_iᵀ这个H是3x3矩阵。对H做SVD分解H U * Σ * Vᵀ然后最优旋转矩阵就是R V * Uᵀ到这里如果det(R) 0也就是出现了反射变换工程上要把V的最后一列取负再乘。具体代码我会放在下一章的完整实现里。这个过程每次迭代只需要做一次3x3矩阵的SVD分解计算开销很小所以我个人非常建议能写闭式解就不要用非线性优化的方式去解这个子问题稳定性和效率都好很多。2.3 点到点与点到面的对比和选择Point-to-Point ICP的目标函数上面已经写了它度量的是对应点之间的欧氏距离。这个版本实现简单对小曲率变化、点云分布均匀的场景效果尚可。但它有个明显的问题在平面或者缓变曲面上垂直于平面方向的约束很强沿着平面方向的约束很弱容易产生滑移。打个比方你拿两张贴满相同花纹的瓷砖上下叠在一起你很难通过花纹判断瓷砖往左往右挪了多少——点在平面上滑动最近邻距离变化不大ICP就“迷茫”了。Point-to-Plane ICP的目标函数改成点到对应点所在局部平面的距离E Σ || (p_i - (R * q_i t)) · n_i ||²其中n_i是点p_i处的法向量。这个版本的物理意义是允许点在平面内滑动但严格约束点在法线方向的偏差。实验和工程经验都表明在激光SLAM这种以平面结构墙面、地面、桌面为主的环境里Point-to-Plane无论是收敛速度还是精度都明显优于Point-to-Point这也是LOAM、NDT等主流激光SLAM框架普遍使用类似约束的原因。代价是它没有SVD闭式解通常需要用高斯牛顿法或LM算法迭代求解。不过这个问题规模很小6自由度一二十次迭代足够实时性不用太担心。下面的核心实现章节我会把两个版本的代码都写出来方便你对比验证。3. 核心代码实现从暴力版本到KD-tree加速3.1 先写一个最小可用版本含完整代码为了不让你一上来就被PCL的函数封装蒙住眼我决定先写一个不依赖PCL的底层版本。核心步骤是暴力最近邻 SVD求解。数据量小比如几百个点时可以直接跑数据量大时也能让你看清每一个耗时点在哪。#include Eigen/Dense #include vector #include iostream struct Point3D { double x, y, z; }; // 用SVD计算两个对应点集之间的最优刚体变换 // src是源点云dst是目标点云两者一一对应 Eigen::Matrix4d computeTransformSVD( const std::vectorPoint3D src, const std::vectorPoint3D dst) { int N src.size(); Eigen::Vector3d src_mean(0, 0, 0), dst_mean(0, 0, 0); for (int i 0; i N; i) { src_mean Eigen::Vector3d(src[i].x, src[i].y, src[i].z); dst_mean Eigen::Vector3d(dst[i].x, dst[i].y, dst[i].z); } src_mean / N; dst_mean / N; Eigen::Matrix3d H Eigen::Matrix3d::Zero(); for (int i 0; i N; i) { Eigen::Vector3d sp(src[i].x, src[i].y, src[i].z); Eigen::Vector3d dp(dst[i].x, dst[i].y, dst[i].z); H (sp - src_mean) * (dp - dst_mean).transpose(); } H / N; Eigen::JacobiSVDEigen::Matrix3d svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3d U svd.matrixU(); Eigen::Matrix3d V svd.matrixV(); Eigen::Matrix3d R V * U.transpose(); // 处理反射矩阵的特殊情况 if (R.determinant() 0) { V.col(2) * -1; R V * U.transpose(); } Eigen::Vector3d t dst_mean - R * src_mean; Eigen::Matrix4d T Eigen::Matrix4d::Identity(); T.block3, 3(0, 0) R; T.block3, 1(0, 3) t; return T; } // 暴力查找最近邻遍历所有目标点找欧氏距离最小的 int findClosestPoint(const Point3D q, const std::vectorPoint3D target) { int bestIdx -1; double bestDist std::numeric_limitsdouble::max(); for (int j 0; j target.size(); j) { double dx q.x - target[j].x; double dy q.y - target[j].y; double dz q.z - target[j].z; double dist dx * dx dy * dy dz * dz; if (dist bestDist) { bestDist dist; bestIdx j; } } return bestIdx; } // ICP主流程maxIter最大迭代次数distThresh最大匹配距离阈值 Eigen::Matrix4d icpPointToPoint( const std::vectorPoint3D src, const std::vectorPoint3D target, int maxIter 50, double distThresh std::numeric_limitsdouble::max()) { std::vectorPoint3D transformed src; Eigen::Matrix4d globalT Eigen::Matrix4d::Identity(); for (int iter 0; iter maxIter; iter) { std::vectorPoint3D srcMatched, targetMatched; for (int i 0; i transformed.size(); i) { int j findClosestPoint(transformed[i], target); double dx transformed[i].x - target[j].x; double dy transformed[i].y - target[j].y; double dz transformed[i].z - target[j].z; if (dx*dx dy*dy dz*dz distThresh * distThresh) { srcMatched.push_back(transformed[i]); targetMatched.push_back(target[j]); } } if (srcMatched.size() 10) { std::cout 匹配点太少提前退出 std::endl; break; } Eigen::Matrix4d deltaT computeTransformSVD(srcMatched, targetMatched); globalT deltaT * globalT; // 将源点云应用新变换 for (int i 0; i transformed.size(); i) { Eigen::Vector4d p(transformed[i].x, transformed[i].y, transformed[i].z, 1.0); p deltaT * p; transformed[i] {p.x(), p.y(), p.z()}; } // 计算当前平均误差用于判断收敛 double err 0.0; for (int i 0; i srcMatched.size(); i) { double dx srcMatched[i].x - targetMatched[i].x; double dy srcMatched[i].y - targetMatched[i].y; double dz srcMatched[i].z - targetMatched[i].z; err std::sqrt(dx*dx dy*dy dz*dz); } err / srcMatched.size(); std::cout iter iter 平均误差: err std::endl; if (err 1e-6) break; } return globalT; }这段代码核心逻辑非常清晰每次迭代里先找最近邻再用SVD求变换然后更新点云。你可能会发现这里最耗时的就是findClosestPoint——每对一次O(N*M)的复杂度N是源点云点数M是目标点云点数。如果两片点云各有一万个点那就是一亿次距离计算一次迭代都要卡顿几十次迭代根本没法实时跑。所以工程上必须用空间索引来加速。3.2 引入KD-treeO(NM)变O(NlogM)KD-tree是加速最近邻搜索最经典的数据结构。原理不复杂把点云按维度递归切分构建一棵二叉树搜索最近邻时利用树的分支剪枝跳过大量不可能成为最近邻的节点平均复杂度能降到O(logM)。PCL里封装好了pcl::KdTreeFLANN我们不用重复造轮子但你要理解它替代的是上面哪个环节。用PCL改写整个ICP流程代码会清爽很多#include pcl/point_cloud.h #include pcl/point_types.h #include pcl/kdtree/kdtree_flann.h #include pcl/registration/icp.h using PointT pcl::PointXYZ; using PointCloudT pcl::PointCloudPointT; Eigen::Matrix4f icpWithKdTree( const PointCloudT::Ptr src, const PointCloudT::Ptr target, float maxCorrespondenceDistance, int maxIterations) { pcl::IterativeClosestPointPointT, PointT icp; icp.setInputSource(src); icp.setInputTarget(target); // 对应点最大距离超过这个距离的点对会被丢弃 icp.setMaxCorrespondenceDistance(maxCorrespondenceDistance); // 迭代停止条件 icp.setMaximumIterations(maxIterations); icp.setTransformationEpsilon(1e-8); icp.setEuclideanFitnessEpsilon(1e-6); PointCloudT::Ptr aligned(new PointCloudT); icp.align(*aligned); return icp.getFinalTransformation(); }你看PCL把整个流程都封装好了几行就能跑通。但如果你只停留在这一层出了问题会很被动。比如PCL默认的点到点ICP在某些环境下就是要比点到面差你可能调半天阈值也补不回来精度又比如它默认处理不了动态物体匹配时会拿动态物体的点去硬找对应关系导致位姿抖动。所以下一章我会展开说要真正在工程里用应该在ICP前后做些什么。3.3 完整实现Point-to-Plane ICP的核心迭代Point-to-Plane的原理上节讲过它的优化目标是点到局部平面的距离。求最优刚体变换时我们把它线性化假设旋转量很小可以用一阶近似把R展开成反对称矩阵的形式。这样每个对应点对都能建立一个线性方程把问题变成一个线性最小二乘用高斯牛顿迭代求解。设旋转向量为wso(3)平移为t目标残差对增量求导最终每个点对建立的方程形如aᵀ * delta b其中a是由点坐标和法向量构成的6维向量delta是6维位姿增量旋转3维平移3维b是残差标量。把所有点对的方程堆起来得到A * delta b然后解得delta (Aᵀ * A)⁻¹ * Aᵀ * b代码实现如下#include Eigen/Dense #include vector struct Correspondence { Eigen::Vector3d sourcePoint; Eigen::Vector3d targetPoint; Eigen::Vector3d normal; // 目标点处的法向量 }; Eigen::Matrix4d icpPointToPlane( const std::vectorCorrespondence corrs, int maxIter 30) { // 用单位矩阵初始化 Eigen::Matrix4d T Eigen::Matrix4d::Identity(); for (int iter 0; iter maxIter; iter) { Eigen::Matrixdouble, 6, 6 H Eigen::Matrixdouble, 6, 6::Zero(); Eigen::Matrixdouble, 6, 1 g Eigen::Matrixdouble, 6, 1::Zero(); for (const auto c : corrs) { // 先按当前T变换源点 Eigen::Vector3d p c.sourcePoint; Eigen::Vector3d transformed_p (T.block3, 3(0, 0) * p T.block3, 1(0, 3)); Eigen::Vector3d n c.normal; // 残差点到目标点所在平面的距离 double residual (c.targetPoint - transformed_p).dot(n); // 构建线性化后的雅可比 Eigen::Matrixdouble, 1, 6 J; J.block1, 3(0, 0) -n.transpose() * skew(transformed_p); J.block1, 3(0, 3) -n.transpose(); H J.transpose() * J; g J.transpose() * residual; } Eigen::Matrixdouble, 6, 1 delta; delta H.ldlt().solve(-g); // 把增量更新到T Eigen::Matrix3d dR Eigen::AngleAxisd(delta.head3().norm(), delta.head3().normalized()).toRotationMatrix(); Eigen::Vector3d dt delta.tail3(); Eigen::Matrix4d deltaT Eigen::Matrix4d::Identity(); deltaT.block3, 3(0, 0) dR; deltaT.block3, 1(0, 3) dt; T deltaT * T; if (delta.norm() 1e-8) break; } return T; } Eigen::Matrix3d skew(const Eigen::Vector3d v) { Eigen::Matrix3d S; S 0, -v.z(), v.y(), v.z(), 0, -v.x(), -v.y(), v.x(), 0; return S; }上面代码里solve(-g)中的负号是从高斯牛顿的增量方程H * delta -g来的这是一个很容易写错的地方我当初在这个符号上卡了很久建议你推导的时候也仔细核一遍。另外用ldlt()而不是inverse()解线性方程速度和数值稳定性都要更好。关于法向量估计PCL里有现成函数#include pcl/features/normal_3d.h pcl::PointCloudpcl::Normal::Ptr computeNormals( const PointCloudT::Ptr cloud) { pcl::NormalEstimationPointT, pcl::Normal ne; ne.setInputCloud(cloud); pcl::search::KdTreePointT::Ptr tree(new pcl::search::KdTreePointT()); ne.setSearchMethod(tree); pcl::PointCloudpcl::Normal::Ptr normals(new pcl::PointCloudpcl::Normal); ne.setRadiusSearch(0.3); // 根据你点云的密度调整 ne.compute(*normals); return normals; }法向量估计的半径很关键半径太小法向量噪声大半径太大会平滑掉真实的结构细节还能把墙角处的法向量糊成一团。我习惯先统计一下点云平均间距然后取5到10倍的平均间距作为搜索半径。3.4 我的实现顺序建议经常有人问我代码那么多该按什么顺序看我的建议是先跑通第3.2节的PCL版本把点云数据喂进去观察匹配效果然后用第3.1节的暴力SVD版本替换掉PCL的SVD部分加深理解最后再把Point-to-Plane版本的所有者实现整合进来用同一个数据集对比两个版本的收敛误差和耗时。这样你的知识不是零散的函数记忆而是形成了一条“原理 → 实现 → 工程选型”的完整链路。4. 把ICP接进SLAM前端的工程实践4.1 预处理比ICP本身更影响结果很多人在点云上直接跑ICP结果飘得亲妈都不认识。真相是ICP只是匹配环节它对输入数据极其敏感预处理做不好后面全是白搭。第一步是体素滤波降采样。一帧32线激光点云可能有一万多个点一帧64线的更多。点数太多不仅慢而且密集区域的点会给优化带来不均匀的权重。pcl::VoxelGrid可以把空间划分成固定边长的小立方体每个立方体只保留一个重心点。我常用0.1米到0.3米的体素尺寸具体看你的环境尺度。体素太大细节丢失体素太小点数降不下来。第二步是去除运动畸变。激光雷达从扫描第一点到最后一点的过程中机器人可能已经移动了一段距离导致一帧里的点其实对应着多个时刻的位姿。如果你把这帧当成刚体直接拿去做ICP精度必然打折。常见做法是利用IMU或轮速计提供的短时位姿增量把一帧里的每个点都插值变换到起始时刻。这一步在激光SLAM里几乎是标配我见过不少人在纯做ICP配准时忽略它结果旋转严重时匹配错乱。第三步是剔除无效点和动态物体。NaN点、距离为0的点必须过滤对于动态物体比如移动的行人、车辆最粗暴但有效的方法是用pcl::PassThrough按距离裁剪把过远或过近的点、高度异常的点丢掉。高级一点的做法是用多帧一致性检测或者语义分割来剔除动态目标但那就是后话了。4.2 帧图匹配、关键帧与回环里的ICP用法ICP在SLAM里不是每次都要“全量帧-全量帧”去匹配。最常用的方案是帧图匹配新来的当前帧先和最近的关键帧或附近若干关键帧组成的局部子图做匹配。这样既控制了计算量又能让匹配目标有足够的空间范围不容易漂移。关键帧怎么选我常用的判据是平移超过一定阈值比如0.2米或者旋转超过一定阈值比如5度就新建一个关键帧。这个阈值跟你机器人的速度、地图的复杂度都有关系需要自己调。阈值太小关键帧太密后端优化的规模膨胀阈值太大帧间重叠度不够ICP容易跑飞。回环检测里ICP还有另一种用法。当你用其他手段比如Scan Context、视觉词袋发现机器人回到了曾经去过的地方为了修正累积漂移需要把当前帧和之前的局部地图做一次精细配准。这时要求的不是一个粗略的位姿对齐而是尽可能精确的刚体变换作为后端图优化的约束边。回环场景下点云重叠度可能覆盖角度很大我会用多点分辨率的方式先用降采样较重的点云跑一遍拿到大致变换作为初值再用全分辨率点云精配一次。4.3 退化和特征稀薄场景的检测与处理一个经典翻车场景在一条长走廊里激光雷达左右两边都是墙前后方向没有任何几何特征。这时候ICP在走廊前进方向的约束几乎为零估算出来的位姿沿着走廊方向随便滑动也不会让误差函数的数值变大多少。这个问题在SLAM里面叫退化Degeneracy。怎么检测退化一个简单可靠的方法是看AᵀA矩阵的特征值。在Point-to-Plane ICP里我们构建的H矩阵或者说信息矩阵反映了各个方向上约束的强弱。如果某个方向的特征值显著小于其他方向说明该方向约束不足。实际操作中我会计算H的最小特征值与最大特征值的比值比值小于某个阈值比如0.01就认为发生退化。检测到退化后最简单的策略是退化方向的位姿增量直接用其他传感器IMU的绝对姿态、轮速计的前向位移来补全或者冻结退化方向的估计只更新约束充足的方向。我自己的习惯是前者因为激光退化并不代表机器人真没动只是激光测不准融合一下IMU和轮速计数据能避免位置漂移在后续帧里被放大。5. 配准质量的评估与参数调优5.1 用分数和残差判断ICP收敛得好不好很多人调用align()之后只看一个hasConverged()布尔值就完事。这是不够的。PCL里getFitnessScore()返回的是匹配点对距离的平方均值数值越小说明配准质量越好但它只是一个整体指标。我还会额外做两件事。第一打印残差分布的统计量均值、中位数、90%分位点、最大残差。均值低但最大残差很大的情况很常见说明大部分点对齐了但有一小撮点对歪了可能来自动态物体或者外点这会破坏后续建图的精度。第二可视化检查。把源点云变换后叠加到目标点云上用不同颜色显示在Rviz里转一圈任何数值指标都替代不了肉眼判断。特别是墙角、柱体这类几何特征明显的区域有没有错位一眼就能看出来。5.2 关键参数速查表本节把这些参数整理成表格方便你直接抄作业。注意这些初始值只是起点实际场景一定要做网格搜索微调。参数建议初始值作用调大/调小的影响体素滤波分辨率0.2m降采样变大速度提升但细节丢失变小更精确但更慢最大对应距离2.0m丢弃距离过远的匹配对偏小匹配不上迭代发散偏大外点影响增大最大迭代次数30防止死循环过大浪费时间过小不收敛变换增量阈值1e-8判断是否收敛太小收敛慢太大提前终止法向量搜索半径10倍点间距法向量质量偏小噪声大偏大糊掉结构细节帧-图匹配关键帧阈值平移0.2m/旋转5°控制匹配频率太密计算量大太稀容易丢匹配5.3 多分辨率匹配与多线程加速ICP的收敛性极度依赖初值。一个实用的工程技巧是多分辨率匹配先对两片点云做重降采样比如体素1.0米用ICP或NDT算出一个粗略位姿然后把体素调到0.3米在上一步位姿基础上继续精配。这个过程本质上是一个从粗到细的“金字塔”策略能显著提升收敛范围对回环检测这类初值不确定性大的场景尤其有效。如果追求实时性多线程也是必要的。PCL的KdTreeFLANN在多线程环境下可以并行搜索多个点而积分图的法向量估计也天然适合并行化。更简单的方式是用OpenMP把单帧中“逐点搜索最近邻”的循环并行化实测下来在8线程机器上能拿到约3到5倍的加速。注意OpenMP默认只有一层并行如果点云已经用PCL的并行函数处理过了再嵌套并行反而会拖慢速度尽量保持并行区域单层。6. 常见问题与排查技巧实录6.1 ICP跑飞了先别急着调参每次有人把ICP配准结果发给我画面是源点云和目标点云直接分家第一反应大多是“调大最大迭代次数”或者“调小最大对应距离”。我的经验恰恰相反ICP跑飞90%的原因是初值给得不好。两片点云如果初始距离误差超过最大对应距离ICP根本找不到正确的对应关系往后的迭代全都是瞎蒙。排查顺序是这样先可视化两片点云的初始相对位置确认它们确实有重叠区域并且大致框架是重合的。如果用里程计或者IMU给初值先把原始数据打出来看看那个初值对不对。再检查最大对应距离合理值应该是点云间初始误差的2到3倍但同时又要小于环境典型特征的尺度不然容易匹配到错误的点。6.2 动态物体导致的反复抖动场景里有人在走动、有车开过ICP会拿这些动态点找最近邻等效于给优化引入了错误约束。表现是位姿在小范围内反复横跳地图上出现“鬼影”层叠。初期排查时先把最大对应距离调得保守一些能过滤掉一部分动态点更根治的办法是在预处理阶段做动态物体剔除或对残差较大的点对做鲁棒核函数加权。6.3 关于评估工具的一个提醒网上经常有人问evo怎么评估SLAM位姿精度。evo确实是个好工具但它评估的是轨迹也就是计算出的位姿序列和真值轨迹之间的误差。如果你目前只是做点云配准这一环还没有跑完整的SLAM系统evo其实帮不上什么忙。你更需要的是把第5.1节的残差统计和可视化方法用起来再用人造数据或者仿真环境里的真值变换来验证配准精度。等SLAM系统跑通了再引入evo评估整体轨迹那才是它的用武之地。6.4 平面场景的滑移还有救吗前面提到的走廊退化很多新手会以为是ICP算法不够强换另一个变体试试。其实这类问题是环境几何约束决定的算法能改善的范围有限。最有效的思路是多传感器融合给激光配准加上IMU的绝对滚转和俯仰约束加轮速计的前向速度约束。这些额外约束就相当于在退化方向上补了几根“柱子”信息矩阵不再奇异位姿自然就稳了。结语一点个人经验ICP这个算法看起来老用起来深。很多人学SLAM时最爱追新模型、新框架但工程里翻车最多的还是最基础的配准和滤波环节。我自己的体会是花一个周末把ICP从原理到代码完整走一遍比调通十个demo都值。当你亲手见证SVD求出来的旋转矩阵把两片点云严丝合缝叠在一起再回头去看PCL里那几行封好的函数会有一种从“会用工具”到“懂工具”的质变。做机器人开发拼到最后拼的就是这些基本功希望这篇文章能帮你把这个基础打得再扎实一点。
从原理到工程实践：详解ICP点云配准在SLAM中的应用
1. 项目概述与核心思路1.1 SLAM里为什么绕不开ICP做SLAM的人不管你是搞激光的、搞视觉的还是搞多传感器融合的大概率都绕不开ICPIterative Closest Point迭代最近点。这东西听起来高深本质就是把两片点云“怼”到一起算出它们之间的旋转和平移矩阵。放在前端配准里它是里程计的核心放在后端回环里它是检测到回环之后精修位姿的工具放在建图环节里它是多帧点云对齐到全局坐标系的关键一步。尤其近几年多线激光雷达价格被打下来之后激光SLAM几乎是机器人导航入门的标配而激光SLAM里最经典、最基础的点云配准方案还是ICP及其各种变体。很多人一开始用PCL里的pcl::IterativeClosestPoint跑通demo容易但真到了自己的数据集上出现漂移、发散、匹配错乱时如果不了解底层实现根本不知道从哪个环节下手排查。这也是我写这篇文章的动机从零把ICP的核心代码完整实现一遍顺带讲清楚它为什么这么设计、怎么调参、怎么避免掉进常见的坑。这篇文章适合的人分两类。一类是刚开始接触SLAM、想把ICP原理吃透的同学你可以照着代码一步步跑通理解每一行在做什么另一类是在工程里被ICP折磨过的开发者看完之后你会明白之前参数调不动、结果反复横跳的深层原因。我不堆公式但核心推导会写清楚不贴大段无关代码但关键实现一定是能直接编译运行的。1.2 这次要实现的版本与工具链ICP从1972年诞生到现在衍生出的版本非常多点到点ICP、点到面ICPPoint-to-Plane、面到面ICPGICP以及带特征权重的Weighted ICP等等。这篇文章我会用C完整实现两个版本经典的Point-to-Point ICP和工程中最常用的Point-to-Plane ICP。前者是理解原理的最佳入口后者是实际项目里效果最稳的起点。开发环境方面我的测试机是Ubuntu 20.04编译器GCC 9依赖Eigen 3.3.7和PCL 1.10。Eigen负责矩阵运算和SVD分解PCL只用来做点云空间搜索和可视化。如果你已经装过ROS这两个库大概率都带了可以直接用。没装的话两条命令的事sudo apt install libeigen3-dev libpcl-dev本文的代码不依赖ROS写的是一个独立的C工程这样你随便丢到哪台Linux机器上都能编译理解起来也更纯粹。后面第3章我会单独说怎么把这段核心逻辑接到SLAM前端里那时候才涉及ROS话题。2. 核心原理与数学建模2.1 ICP要解决的问题本质先给一个直观场景。上一帧激光扫描得到一堆点这一帧又得到一堆点由于机器人动了这两堆点不在同一个坐标系下。ICP要做的就是找到一个刚体变换一个3x3旋转矩阵R和一个3x1平移向量t把第二帧的点变换到第一帧的坐标系下让两片点云的“重叠部分”尽可能重合。数学上这就是一个最小二乘问题E(R, t) (1/N) * Σ|| p_i - (R * q_i t) ||²其中p_i是目标点云比如上一帧中的点q_i是源点云当前帧中与p_i对应的点。这里最关键也最狡猾的问题是一开始我们根本不知道q_i对应p_i是哪个点。所以ICP的实际迭代策略是走两个交替步骤用当前位姿估计把源点云变换过去然后按空间最近邻原则查找对应点对根据找出来的点对重新求解一个更优的R和t更新位姿回到第1步直到收敛。所以ICP本质上是一个“假设-验证-修正”的循环。这也决定了它最大的软肋如果初值差得太远最近邻找出来的对应关系就是错的后面再怎么迭代也救不回来。这个后面详细讲。2.2 SVD求解旋转矩阵的完整推导ICP目标函数里旋转矩阵R的求解是整个算法的心脏。业界最常用的方法是基于SVD奇异值分解的闭式解比基于优化器的方法又快又稳。它的推导过程非常优雅值得完整看一遍。我们先定义两组点的质心p_mean (1/N) * Σ p_i q_mean (1/N) * Σ q_i然后做去质心处理得到每个点相对于质心的偏移p_i p_i - p_mean q_i q_i - q_mean把目标函数展开整理之后可以发现最优平移可以直接由质心和旋转决定t* p_mean - R * q_mean于是优化目标的重点就落在旋转R上问题变成最大化下面这个式子Σ (R * q_i)ᵀ * p_i trace(R * H)其中H (1/N) * Σ q_i * p_iᵀ这个H是3x3矩阵。对H做SVD分解H U * Σ * Vᵀ然后最优旋转矩阵就是R V * Uᵀ到这里如果det(R) 0也就是出现了反射变换工程上要把V的最后一列取负再乘。具体代码我会放在下一章的完整实现里。这个过程每次迭代只需要做一次3x3矩阵的SVD分解计算开销很小所以我个人非常建议能写闭式解就不要用非线性优化的方式去解这个子问题稳定性和效率都好很多。2.3 点到点与点到面的对比和选择Point-to-Point ICP的目标函数上面已经写了它度量的是对应点之间的欧氏距离。这个版本实现简单对小曲率变化、点云分布均匀的场景效果尚可。但它有个明显的问题在平面或者缓变曲面上垂直于平面方向的约束很强沿着平面方向的约束很弱容易产生滑移。打个比方你拿两张贴满相同花纹的瓷砖上下叠在一起你很难通过花纹判断瓷砖往左往右挪了多少——点在平面上滑动最近邻距离变化不大ICP就“迷茫”了。Point-to-Plane ICP的目标函数改成点到对应点所在局部平面的距离E Σ || (p_i - (R * q_i t)) · n_i ||²其中n_i是点p_i处的法向量。这个版本的物理意义是允许点在平面内滑动但严格约束点在法线方向的偏差。实验和工程经验都表明在激光SLAM这种以平面结构墙面、地面、桌面为主的环境里Point-to-Plane无论是收敛速度还是精度都明显优于Point-to-Point这也是LOAM、NDT等主流激光SLAM框架普遍使用类似约束的原因。代价是它没有SVD闭式解通常需要用高斯牛顿法或LM算法迭代求解。不过这个问题规模很小6自由度一二十次迭代足够实时性不用太担心。下面的核心实现章节我会把两个版本的代码都写出来方便你对比验证。3. 核心代码实现从暴力版本到KD-tree加速3.1 先写一个最小可用版本含完整代码为了不让你一上来就被PCL的函数封装蒙住眼我决定先写一个不依赖PCL的底层版本。核心步骤是暴力最近邻 SVD求解。数据量小比如几百个点时可以直接跑数据量大时也能让你看清每一个耗时点在哪。#include Eigen/Dense #include vector #include iostream struct Point3D { double x, y, z; }; // 用SVD计算两个对应点集之间的最优刚体变换 // src是源点云dst是目标点云两者一一对应 Eigen::Matrix4d computeTransformSVD( const std::vectorPoint3D src, const std::vectorPoint3D dst) { int N src.size(); Eigen::Vector3d src_mean(0, 0, 0), dst_mean(0, 0, 0); for (int i 0; i N; i) { src_mean Eigen::Vector3d(src[i].x, src[i].y, src[i].z); dst_mean Eigen::Vector3d(dst[i].x, dst[i].y, dst[i].z); } src_mean / N; dst_mean / N; Eigen::Matrix3d H Eigen::Matrix3d::Zero(); for (int i 0; i N; i) { Eigen::Vector3d sp(src[i].x, src[i].y, src[i].z); Eigen::Vector3d dp(dst[i].x, dst[i].y, dst[i].z); H (sp - src_mean) * (dp - dst_mean).transpose(); } H / N; Eigen::JacobiSVDEigen::Matrix3d svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3d U svd.matrixU(); Eigen::Matrix3d V svd.matrixV(); Eigen::Matrix3d R V * U.transpose(); // 处理反射矩阵的特殊情况 if (R.determinant() 0) { V.col(2) * -1; R V * U.transpose(); } Eigen::Vector3d t dst_mean - R * src_mean; Eigen::Matrix4d T Eigen::Matrix4d::Identity(); T.block3, 3(0, 0) R; T.block3, 1(0, 3) t; return T; } // 暴力查找最近邻遍历所有目标点找欧氏距离最小的 int findClosestPoint(const Point3D q, const std::vectorPoint3D target) { int bestIdx -1; double bestDist std::numeric_limitsdouble::max(); for (int j 0; j target.size(); j) { double dx q.x - target[j].x; double dy q.y - target[j].y; double dz q.z - target[j].z; double dist dx * dx dy * dy dz * dz; if (dist bestDist) { bestDist dist; bestIdx j; } } return bestIdx; } // ICP主流程maxIter最大迭代次数distThresh最大匹配距离阈值 Eigen::Matrix4d icpPointToPoint( const std::vectorPoint3D src, const std::vectorPoint3D target, int maxIter 50, double distThresh std::numeric_limitsdouble::max()) { std::vectorPoint3D transformed src; Eigen::Matrix4d globalT Eigen::Matrix4d::Identity(); for (int iter 0; iter maxIter; iter) { std::vectorPoint3D srcMatched, targetMatched; for (int i 0; i transformed.size(); i) { int j findClosestPoint(transformed[i], target); double dx transformed[i].x - target[j].x; double dy transformed[i].y - target[j].y; double dz transformed[i].z - target[j].z; if (dx*dx dy*dy dz*dz distThresh * distThresh) { srcMatched.push_back(transformed[i]); targetMatched.push_back(target[j]); } } if (srcMatched.size() 10) { std::cout 匹配点太少提前退出 std::endl; break; } Eigen::Matrix4d deltaT computeTransformSVD(srcMatched, targetMatched); globalT deltaT * globalT; // 将源点云应用新变换 for (int i 0; i transformed.size(); i) { Eigen::Vector4d p(transformed[i].x, transformed[i].y, transformed[i].z, 1.0); p deltaT * p; transformed[i] {p.x(), p.y(), p.z()}; } // 计算当前平均误差用于判断收敛 double err 0.0; for (int i 0; i srcMatched.size(); i) { double dx srcMatched[i].x - targetMatched[i].x; double dy srcMatched[i].y - targetMatched[i].y; double dz srcMatched[i].z - targetMatched[i].z; err std::sqrt(dx*dx dy*dy dz*dz); } err / srcMatched.size(); std::cout iter iter 平均误差: err std::endl; if (err 1e-6) break; } return globalT; }这段代码核心逻辑非常清晰每次迭代里先找最近邻再用SVD求变换然后更新点云。你可能会发现这里最耗时的就是findClosestPoint——每对一次O(N*M)的复杂度N是源点云点数M是目标点云点数。如果两片点云各有一万个点那就是一亿次距离计算一次迭代都要卡顿几十次迭代根本没法实时跑。所以工程上必须用空间索引来加速。3.2 引入KD-treeO(NM)变O(NlogM)KD-tree是加速最近邻搜索最经典的数据结构。原理不复杂把点云按维度递归切分构建一棵二叉树搜索最近邻时利用树的分支剪枝跳过大量不可能成为最近邻的节点平均复杂度能降到O(logM)。PCL里封装好了pcl::KdTreeFLANN我们不用重复造轮子但你要理解它替代的是上面哪个环节。用PCL改写整个ICP流程代码会清爽很多#include pcl/point_cloud.h #include pcl/point_types.h #include pcl/kdtree/kdtree_flann.h #include pcl/registration/icp.h using PointT pcl::PointXYZ; using PointCloudT pcl::PointCloudPointT; Eigen::Matrix4f icpWithKdTree( const PointCloudT::Ptr src, const PointCloudT::Ptr target, float maxCorrespondenceDistance, int maxIterations) { pcl::IterativeClosestPointPointT, PointT icp; icp.setInputSource(src); icp.setInputTarget(target); // 对应点最大距离超过这个距离的点对会被丢弃 icp.setMaxCorrespondenceDistance(maxCorrespondenceDistance); // 迭代停止条件 icp.setMaximumIterations(maxIterations); icp.setTransformationEpsilon(1e-8); icp.setEuclideanFitnessEpsilon(1e-6); PointCloudT::Ptr aligned(new PointCloudT); icp.align(*aligned); return icp.getFinalTransformation(); }你看PCL把整个流程都封装好了几行就能跑通。但如果你只停留在这一层出了问题会很被动。比如PCL默认的点到点ICP在某些环境下就是要比点到面差你可能调半天阈值也补不回来精度又比如它默认处理不了动态物体匹配时会拿动态物体的点去硬找对应关系导致位姿抖动。所以下一章我会展开说要真正在工程里用应该在ICP前后做些什么。3.3 完整实现Point-to-Plane ICP的核心迭代Point-to-Plane的原理上节讲过它的优化目标是点到局部平面的距离。求最优刚体变换时我们把它线性化假设旋转量很小可以用一阶近似把R展开成反对称矩阵的形式。这样每个对应点对都能建立一个线性方程把问题变成一个线性最小二乘用高斯牛顿迭代求解。设旋转向量为wso(3)平移为t目标残差对增量求导最终每个点对建立的方程形如aᵀ * delta b其中a是由点坐标和法向量构成的6维向量delta是6维位姿增量旋转3维平移3维b是残差标量。把所有点对的方程堆起来得到A * delta b然后解得delta (Aᵀ * A)⁻¹ * Aᵀ * b代码实现如下#include Eigen/Dense #include vector struct Correspondence { Eigen::Vector3d sourcePoint; Eigen::Vector3d targetPoint; Eigen::Vector3d normal; // 目标点处的法向量 }; Eigen::Matrix4d icpPointToPlane( const std::vectorCorrespondence corrs, int maxIter 30) { // 用单位矩阵初始化 Eigen::Matrix4d T Eigen::Matrix4d::Identity(); for (int iter 0; iter maxIter; iter) { Eigen::Matrixdouble, 6, 6 H Eigen::Matrixdouble, 6, 6::Zero(); Eigen::Matrixdouble, 6, 1 g Eigen::Matrixdouble, 6, 1::Zero(); for (const auto c : corrs) { // 先按当前T变换源点 Eigen::Vector3d p c.sourcePoint; Eigen::Vector3d transformed_p (T.block3, 3(0, 0) * p T.block3, 1(0, 3)); Eigen::Vector3d n c.normal; // 残差点到目标点所在平面的距离 double residual (c.targetPoint - transformed_p).dot(n); // 构建线性化后的雅可比 Eigen::Matrixdouble, 1, 6 J; J.block1, 3(0, 0) -n.transpose() * skew(transformed_p); J.block1, 3(0, 3) -n.transpose(); H J.transpose() * J; g J.transpose() * residual; } Eigen::Matrixdouble, 6, 1 delta; delta H.ldlt().solve(-g); // 把增量更新到T Eigen::Matrix3d dR Eigen::AngleAxisd(delta.head3().norm(), delta.head3().normalized()).toRotationMatrix(); Eigen::Vector3d dt delta.tail3(); Eigen::Matrix4d deltaT Eigen::Matrix4d::Identity(); deltaT.block3, 3(0, 0) dR; deltaT.block3, 1(0, 3) dt; T deltaT * T; if (delta.norm() 1e-8) break; } return T; } Eigen::Matrix3d skew(const Eigen::Vector3d v) { Eigen::Matrix3d S; S 0, -v.z(), v.y(), v.z(), 0, -v.x(), -v.y(), v.x(), 0; return S; }上面代码里solve(-g)中的负号是从高斯牛顿的增量方程H * delta -g来的这是一个很容易写错的地方我当初在这个符号上卡了很久建议你推导的时候也仔细核一遍。另外用ldlt()而不是inverse()解线性方程速度和数值稳定性都要更好。关于法向量估计PCL里有现成函数#include pcl/features/normal_3d.h pcl::PointCloudpcl::Normal::Ptr computeNormals( const PointCloudT::Ptr cloud) { pcl::NormalEstimationPointT, pcl::Normal ne; ne.setInputCloud(cloud); pcl::search::KdTreePointT::Ptr tree(new pcl::search::KdTreePointT()); ne.setSearchMethod(tree); pcl::PointCloudpcl::Normal::Ptr normals(new pcl::PointCloudpcl::Normal); ne.setRadiusSearch(0.3); // 根据你点云的密度调整 ne.compute(*normals); return normals; }法向量估计的半径很关键半径太小法向量噪声大半径太大会平滑掉真实的结构细节还能把墙角处的法向量糊成一团。我习惯先统计一下点云平均间距然后取5到10倍的平均间距作为搜索半径。3.4 我的实现顺序建议经常有人问我代码那么多该按什么顺序看我的建议是先跑通第3.2节的PCL版本把点云数据喂进去观察匹配效果然后用第3.1节的暴力SVD版本替换掉PCL的SVD部分加深理解最后再把Point-to-Plane版本的所有者实现整合进来用同一个数据集对比两个版本的收敛误差和耗时。这样你的知识不是零散的函数记忆而是形成了一条“原理 → 实现 → 工程选型”的完整链路。4. 把ICP接进SLAM前端的工程实践4.1 预处理比ICP本身更影响结果很多人在点云上直接跑ICP结果飘得亲妈都不认识。真相是ICP只是匹配环节它对输入数据极其敏感预处理做不好后面全是白搭。第一步是体素滤波降采样。一帧32线激光点云可能有一万多个点一帧64线的更多。点数太多不仅慢而且密集区域的点会给优化带来不均匀的权重。pcl::VoxelGrid可以把空间划分成固定边长的小立方体每个立方体只保留一个重心点。我常用0.1米到0.3米的体素尺寸具体看你的环境尺度。体素太大细节丢失体素太小点数降不下来。第二步是去除运动畸变。激光雷达从扫描第一点到最后一点的过程中机器人可能已经移动了一段距离导致一帧里的点其实对应着多个时刻的位姿。如果你把这帧当成刚体直接拿去做ICP精度必然打折。常见做法是利用IMU或轮速计提供的短时位姿增量把一帧里的每个点都插值变换到起始时刻。这一步在激光SLAM里几乎是标配我见过不少人在纯做ICP配准时忽略它结果旋转严重时匹配错乱。第三步是剔除无效点和动态物体。NaN点、距离为0的点必须过滤对于动态物体比如移动的行人、车辆最粗暴但有效的方法是用pcl::PassThrough按距离裁剪把过远或过近的点、高度异常的点丢掉。高级一点的做法是用多帧一致性检测或者语义分割来剔除动态目标但那就是后话了。4.2 帧图匹配、关键帧与回环里的ICP用法ICP在SLAM里不是每次都要“全量帧-全量帧”去匹配。最常用的方案是帧图匹配新来的当前帧先和最近的关键帧或附近若干关键帧组成的局部子图做匹配。这样既控制了计算量又能让匹配目标有足够的空间范围不容易漂移。关键帧怎么选我常用的判据是平移超过一定阈值比如0.2米或者旋转超过一定阈值比如5度就新建一个关键帧。这个阈值跟你机器人的速度、地图的复杂度都有关系需要自己调。阈值太小关键帧太密后端优化的规模膨胀阈值太大帧间重叠度不够ICP容易跑飞。回环检测里ICP还有另一种用法。当你用其他手段比如Scan Context、视觉词袋发现机器人回到了曾经去过的地方为了修正累积漂移需要把当前帧和之前的局部地图做一次精细配准。这时要求的不是一个粗略的位姿对齐而是尽可能精确的刚体变换作为后端图优化的约束边。回环场景下点云重叠度可能覆盖角度很大我会用多点分辨率的方式先用降采样较重的点云跑一遍拿到大致变换作为初值再用全分辨率点云精配一次。4.3 退化和特征稀薄场景的检测与处理一个经典翻车场景在一条长走廊里激光雷达左右两边都是墙前后方向没有任何几何特征。这时候ICP在走廊前进方向的约束几乎为零估算出来的位姿沿着走廊方向随便滑动也不会让误差函数的数值变大多少。这个问题在SLAM里面叫退化Degeneracy。怎么检测退化一个简单可靠的方法是看AᵀA矩阵的特征值。在Point-to-Plane ICP里我们构建的H矩阵或者说信息矩阵反映了各个方向上约束的强弱。如果某个方向的特征值显著小于其他方向说明该方向约束不足。实际操作中我会计算H的最小特征值与最大特征值的比值比值小于某个阈值比如0.01就认为发生退化。检测到退化后最简单的策略是退化方向的位姿增量直接用其他传感器IMU的绝对姿态、轮速计的前向位移来补全或者冻结退化方向的估计只更新约束充足的方向。我自己的习惯是前者因为激光退化并不代表机器人真没动只是激光测不准融合一下IMU和轮速计数据能避免位置漂移在后续帧里被放大。5. 配准质量的评估与参数调优5.1 用分数和残差判断ICP收敛得好不好很多人调用align()之后只看一个hasConverged()布尔值就完事。这是不够的。PCL里getFitnessScore()返回的是匹配点对距离的平方均值数值越小说明配准质量越好但它只是一个整体指标。我还会额外做两件事。第一打印残差分布的统计量均值、中位数、90%分位点、最大残差。均值低但最大残差很大的情况很常见说明大部分点对齐了但有一小撮点对歪了可能来自动态物体或者外点这会破坏后续建图的精度。第二可视化检查。把源点云变换后叠加到目标点云上用不同颜色显示在Rviz里转一圈任何数值指标都替代不了肉眼判断。特别是墙角、柱体这类几何特征明显的区域有没有错位一眼就能看出来。5.2 关键参数速查表本节把这些参数整理成表格方便你直接抄作业。注意这些初始值只是起点实际场景一定要做网格搜索微调。参数建议初始值作用调大/调小的影响体素滤波分辨率0.2m降采样变大速度提升但细节丢失变小更精确但更慢最大对应距离2.0m丢弃距离过远的匹配对偏小匹配不上迭代发散偏大外点影响增大最大迭代次数30防止死循环过大浪费时间过小不收敛变换增量阈值1e-8判断是否收敛太小收敛慢太大提前终止法向量搜索半径10倍点间距法向量质量偏小噪声大偏大糊掉结构细节帧-图匹配关键帧阈值平移0.2m/旋转5°控制匹配频率太密计算量大太稀容易丢匹配5.3 多分辨率匹配与多线程加速ICP的收敛性极度依赖初值。一个实用的工程技巧是多分辨率匹配先对两片点云做重降采样比如体素1.0米用ICP或NDT算出一个粗略位姿然后把体素调到0.3米在上一步位姿基础上继续精配。这个过程本质上是一个从粗到细的“金字塔”策略能显著提升收敛范围对回环检测这类初值不确定性大的场景尤其有效。如果追求实时性多线程也是必要的。PCL的KdTreeFLANN在多线程环境下可以并行搜索多个点而积分图的法向量估计也天然适合并行化。更简单的方式是用OpenMP把单帧中“逐点搜索最近邻”的循环并行化实测下来在8线程机器上能拿到约3到5倍的加速。注意OpenMP默认只有一层并行如果点云已经用PCL的并行函数处理过了再嵌套并行反而会拖慢速度尽量保持并行区域单层。6. 常见问题与排查技巧实录6.1 ICP跑飞了先别急着调参每次有人把ICP配准结果发给我画面是源点云和目标点云直接分家第一反应大多是“调大最大迭代次数”或者“调小最大对应距离”。我的经验恰恰相反ICP跑飞90%的原因是初值给得不好。两片点云如果初始距离误差超过最大对应距离ICP根本找不到正确的对应关系往后的迭代全都是瞎蒙。排查顺序是这样先可视化两片点云的初始相对位置确认它们确实有重叠区域并且大致框架是重合的。如果用里程计或者IMU给初值先把原始数据打出来看看那个初值对不对。再检查最大对应距离合理值应该是点云间初始误差的2到3倍但同时又要小于环境典型特征的尺度不然容易匹配到错误的点。6.2 动态物体导致的反复抖动场景里有人在走动、有车开过ICP会拿这些动态点找最近邻等效于给优化引入了错误约束。表现是位姿在小范围内反复横跳地图上出现“鬼影”层叠。初期排查时先把最大对应距离调得保守一些能过滤掉一部分动态点更根治的办法是在预处理阶段做动态物体剔除或对残差较大的点对做鲁棒核函数加权。6.3 关于评估工具的一个提醒网上经常有人问evo怎么评估SLAM位姿精度。evo确实是个好工具但它评估的是轨迹也就是计算出的位姿序列和真值轨迹之间的误差。如果你目前只是做点云配准这一环还没有跑完整的SLAM系统evo其实帮不上什么忙。你更需要的是把第5.1节的残差统计和可视化方法用起来再用人造数据或者仿真环境里的真值变换来验证配准精度。等SLAM系统跑通了再引入evo评估整体轨迹那才是它的用武之地。6.4 平面场景的滑移还有救吗前面提到的走廊退化很多新手会以为是ICP算法不够强换另一个变体试试。其实这类问题是环境几何约束决定的算法能改善的范围有限。最有效的思路是多传感器融合给激光配准加上IMU的绝对滚转和俯仰约束加轮速计的前向速度约束。这些额外约束就相当于在退化方向上补了几根“柱子”信息矩阵不再奇异位姿自然就稳了。结语一点个人经验ICP这个算法看起来老用起来深。很多人学SLAM时最爱追新模型、新框架但工程里翻车最多的还是最基础的配准和滤波环节。我自己的体会是花一个周末把ICP从原理到代码完整走一遍比调通十个demo都值。当你亲手见证SVD求出来的旋转矩阵把两片点云严丝合缝叠在一起再回头去看PCL里那几行封好的函数会有一种从“会用工具”到“懂工具”的质变。做机器人开发拼到最后拼的就是这些基本功希望这篇文章能帮你把这个基础打得再扎实一点。 
