1. 从2D到3D为什么LOAM是理解激光SLAM的绝佳起点做了好几年2D激光SLAM从Gmapping到Cartographer感觉二维世界已经摸得差不多了。但每次看到机器人或者自动驾驶汽车在复杂多层停车场、或者有上下坡的园区里跑心里就痒痒的——2D地图根本画不出这个世界的全貌啊。于是我下定决心要啃下一个经典的3D激光SLAM算法。在众多开源方案里我最终锁定了LOAM更具体地说是它的一个经典实现版本loam_velodyne。为什么选它原因很简单它足够“原始”也足够“经典”。它没有用任何深度学习的花招纯粹靠几何和数学就把实时建图和定位这事儿给办了这让我觉得吃透了它才算真正理解了3D激光SLAM的筋骨。LOAM全称Lidar Odometry and Mapping in Real-time直译就是“实时激光雷达里程计与建图”。这个名字就点出了它的核心任务一边运动里程计一边画地图建图。和2D SLAM最大的不同在于我们处理的不再是一个平面上的扫描线而是一个由激光雷达扫出来的、布满空间点的“云”。想象一下你拿着一个喷雾罐对着房间喷出一片均匀的雾气然后用特殊的相机拍下这些雾点的三维坐标——这就是一帧3D激光点云。LOAM要做的就是根据这一帧帧连续的点云反推出自己是怎么运动的并且把这些点云拼成一张完整的三维地图。对于刚接触3D SLAM的朋友来说直接看论文里的数学公式可能会有点发怵。但别担心我们可以把LOAM的核心思想先做个生活化的比喻。你可以把它想象成一个人在一片陌生的、布满特征物的森林里蒙眼走路。他每走一步就伸手触摸周围获取一帧点云。他需要记住刚才摸到的几棵特别的树特征点比如一棵特别粗糙的树皮边缘点和一块平坦的大石头平面点。走下一步后他再次伸手寻找并匹配这些特别的树和石头。通过计算这次摸到的位置和上次记忆中的位置之间的差异他就能推断出自己这一步是往前走了还是往左拐了运动估计。同时他会在心里不断更新一幅森林的地图地图构建。LOAM的巧妙之处在于它并不笨拙地去匹配所有“摸到”的点那计算量太大了而是只专注于那些最有代表性的“特征点”从而实现了实时性。那么loam_velodyne这个代码库就是LOAM论文思想的具体工程实现。它最初是针对Velodyne的16线或32线激光雷达设计的代码结构清晰是学习算法如何落地的绝佳材料。接下来我们就一起打开这个“黑匣子”从数据流动开始一步步看明白它到底是怎么工作的。2. 庖丁解牛loam_velodyne的代码骨架与数据流拿到loam_velodyne的代码第一步不是扎进某个文件而是要先理清它的整体结构。这就像看一座建筑的蓝图知道了承重墙和管道走向装修起来才心里有数。整个系统由几个ROS节点组成它们通过Topic串联起来形成一个高效的处理流水线。核心节点就三个scanRegistration、laserOdometry和laserMapping。它们的分工非常明确scanRegistration扫描注册这是流水线的第一站。它订阅原始的激光点云Topic默认是/velodyne_points然后干一件最重要的事——特征点提取。它像是一个挑剔的质检员从海量的、杂乱的点云中筛选出那些“与众不同”的点分为两大类Sharp尖锐点通常是边缘和Flat平坦点通常是平面。处理完后它会发布诸如/laser_cloud_sharp、/laser_cloud_flat这样的Topic。laserOdometry激光里程计这是运动的快速估算者。它订阅scanRegistration提取出的特征点以较高的频率约10Hz快速估计相邻两帧点云之间的运动变换。它的目标是“快”和“准”为后续的建图提供一个不错的初始位姿估计。它发布的是高频的里程计信息/laser_odom_to_init。laserMapping激光建图这是精益求精的地图绘制师。它以较低的频率约1Hz运行订阅laserOdometry的位姿估计和scanRegistration的特征点。它的任务不是简单地累加点云而是进行全局优化和地图管理。它会将新的特征点融合到一个不断增长、并经过优化的全局地图中并发布最终用于导航和显示的稠密点云地图/laser_cloud_surround。这个“高频里程计低频建图”的双层架构是LOAM保证实时性的关键设计。里程计快速响应运动避免累积误差瞬间爆炸建图模块则慢工出细活在后台进行更精确的优化和拼接修正里程计的漂移。这种思想影响深远后来很多激光SLAM方案都借鉴了这种思路。在代码里数据流就像一条河。原始点云流入scanRegistration被提炼成特征点精华这些精华一方面快速流入laserOdometry估算运动另一方面和运动估计一起流入laserMapping进行沉淀形成最终的地图。理解了这个流程我们再深入每个环节就不会迷失在代码细节里了。3. 慧眼识珠特征点提取的奥秘与代码实现特征点提取是LOAM所有后续工作的基石。如果选的点不靠谱后面的匹配和估计全得垮掉。那么LOAM是如何定义和挑选特征点的呢它的标准非常直观看一个点“不平”的程度。在三维空间中一个点是处在边缘拐角还是位于平坦墙面可以通过它和周围邻居点的分布关系来判断。LOAM使用曲率Curvature来量化这种“不平整度”。计算曲率的方法很直接对于一个中心点取其前后各5个点同一扫描线内计算这些邻居点坐标和与中心点坐标的差异。公式在代码里长这样float diffX laserCloud-points[i - 5].x ... laserCloud-points[i 5].x - 10 * laserCloud-points[i].x; float diffY ... // 同理Y坐标 float diffZ ... // 同理Z坐标 cloudCurvature[i] diffX * diffX diffY * diffY diffZ * diffZ;你可以这么理解如果这个点在一个平面上它前后左右的点高度都差不多那么这些diff值会很小曲率也就小。如果这个点在一个边缘上比如桌沿它一侧的点在桌面上另一侧的点在空气中坐标差异会很大曲率值也就很大。所以曲率大的点我们认为是边缘点Sharp曲率特别小的点我们认为是平面点Flat。但直接按曲率排序取最大和最小就行了吗不行这样选出来的特征点可能会扎堆在同一个物体上导致特征分布不均匀。LOAM采用了一个很聪明的策略将每一圈激光扫描Scan均匀分成6段在每一段里分别挑选特征点。这样做保证了特征点能均匀地分布在周围环境中无论近处还是远处都有代表参与匹配提高了系统的鲁棒性。在scanRegistration.cpp中这个挑选过程充满了工程上的细节考量。首先代码会滤除每条扫描线最前和最后的几个点因为这些点常常由于激光雷达的物理特性而不稳定。然后在每一小段里对点的曲率进行升序排序。接着从曲率最大的开始即这段里最“边缘”的点选取不超过2个点作为cornerPointsSharp再往下取一些作为cornerPointsLessSharp。对于平面点则从曲率最小的开始选取不超过4个点作为surfPointsFlat。这里还有一个关键的“去干扰”操作用cloudNeighborPicked数组来标记。如果一个点被选为特征点那么它相邻的几个点就会被“屏蔽”不再参与挑选。这是为了防止在同一个边缘或平面上选取过于密集的特征点同时也是为了排除一些不可靠的点比如“断点”——即激光打在物体边缘时可能一个点打在物体上下一个点打在远处背景上这两个点虽然物理上相邻但在空间上距离很远这种点计算出的曲率会异常大但不是稳定的特征。我刚开始读这段代码时对那个point.intensity的用法印象很深。它巧妙地把scan ID整数部分和该点在此次扫描中的相对时间小数部分编码在了一起。这个相对时间信息非常重要因为激光雷达旋转扫描需要时间一帧点云里的点并不是同一时刻采集的。在后续的运动估计中需要根据这个时间戳进行运动畸变校正假设雷达在扫描一帧期间是匀速运动的以此来补偿每个点因运动而产生的坐标偏差。4. 时空关联特征匹配与运动估计如何实现提取出特征点后下一步就是重头戏如何通过两帧之间的特征点算出雷达在这段时间里到底怎么动的这就是特征匹配与运动估计发生在laserOdometry.cpp中。这个过程可以类比为玩“找相同”游戏。我们有上一帧的“边缘点集合”和“平面点集合”以及当前帧新提取的“边缘点”和“平面点”。对于当前帧的每一个边缘点我们需要在上一帧的边缘点集合里找到一条“最合适”的直线与之匹配。注意是找一条线而不是一个点。为什么因为一个边缘点通常来源于物体的棱线它在上一帧中对应的不应该是一个孤立的点而应该是同一根棱线上的另一个点。所以LOAM的做法是在上一帧的点云里用KD-Tree快速找到距离当前点最近的一个点A然后以A为起点在同一条扫描线或相邻扫描线的附近寻找另一个点B确保A和B能构成一条有意义的线段。找到这条线段点A和点B后就可以计算当前点到这条线段的垂直距离。理想情况下如果运动估计完全正确当前点应该正好落在这条线段上距离为零。但实际上由于估计不准会有一个距离d。我们的目标就是找到一个雷达的位姿变换包括旋转和平移共6个自由度使得所有当前帧特征点到其对应上一帧特征线/面的距离之和最小。对于平面点也是类似的逻辑只不过是在上一帧的平面点集合里找三个点构成一个平面然后计算当前点到这个平面的距离。这里有一个非常关键的细节也是LOAM论文里的一个核心假设雷达的运动是连续的、平滑的。这使得我们可以用上一次估计的变换或者结合IMU数据给出的初始变换作为本次迭代优化的起点。在代码中laserOdometry会进行多次迭代比如25次每次迭代都重新计算距离和雅可比矩阵然后用列文伯格-马夸尔特Levenberg-Marquardt, LM这样的非线性优化方法求解出一个位姿变换的增量不断更新直到收敛。我特别想提一下代码里iterCount % 5 0这个条件。它控制着KD-Tree搜索的更新频率。在迭代优化开始时我们根据初始位姿将当前点变换到上一帧坐标系下然后去KD-Tree里找匹配。随着优化的进行位姿估计在不断更新此时当前点变换后的坐标也在变。如果每次迭代都重新搜索最近点计算量太大如果一直不更新匹配关系可能不准。LOAM采取了一个折中策略每迭代5次才根据最新的位姿估计重新搜索一次最近点。这是一种在精度和计算效率之间的巧妙平衡。最终laserOdometry会输出一个高频的、相对准确的里程计变换transformSum。这个变换是累加的表示了从起始时刻到当前时刻雷达的整体运动。但它存在累积误差这就需要低频但更精确的laserMapping来校正和优化。5. 精益求精地图构建与全局优化详解如果说laserOdometry是先锋负责快速探路那么laserMapping就是大后方负责巩固疆土、绘制精确地图。它的输入是laserOdometry提供的位姿估计作为优化初值和scanRegistration提供的特征点输出是优化后的全局位姿和拼接好的地图点云。laserMapping的核心任务有两个1. 将新的特征点融合到全局地图中2. 基于更庞大的历史地图数据对当前帧的位姿进行优化消除里程计的漂移。它第一个巧妙的设计是地图管理。想象一下如果地图无限增长每次都要在整个地图里搜索匹配点计算量是无法承受的。LOAM采用了一个“滑动窗口”式的子图管理策略。它将整个空间划分成许多个固定大小例如50m x 50m x 50m的立方体Voxel。代码中定义了laserCloudCornerArray和laserCloudSurfArray两个大的数组来管理这些立方体。每个新来的特征点根据其优化后的全局坐标被存入对应的立方体。同时系统只维护机器人周围一定范围内的立方体比如前后左右各10个上下各5个为“活跃区域”。只有活跃区域内的点云才会被用来进行当前帧的匹配优化。当机器人移动时活跃区域也随之滑动并将移出区域的立方体从内存中释放或保存。这种设计极大地限制了每次参与计算的地图规模。第二个关键步骤是在全局地图中寻找更准确的匹配。这个过程和里程计中的匹配类似但搜索范围更大、约束更强。例如对于当前帧的一个边缘点它会在全局地图的活跃区域里寻找最近的5个边缘点然后检查这5个点是否真的能拟合成一条直线通过计算协方差矩阵的特征值来判断。如果能才认为找到了有效的匹配并计算点到直线的距离。这个匹配标准比里程计阶段更严格因为它利用了更丰富、更稳定的历史地图信息。接下来就是迭代优化。laserMapping构建的误差方程和laserOdometry在形式上是一样的都是最小化点到线、点到面的距离。但是这里的待优化变量transformTobeMapped是相对于全局地图坐标系的位姿。优化过程中它会利用所有匹配上的特征点通常数量远多于里程计阶段构建一个更大的线性方程组matA * matX matB求解位姿修正量matX。代码中有一段关于退化Degeneracy的处理非常值得注意。在某些几何特征匮乏的环境比如长长的走廊或空旷的平地所有匹配约束可能都集中在某一个或某几个方向上比如在走廊里约束主要来自两侧的墙而在前进方向上缺乏约束。这会导致求解问题的矩阵matAtA接近奇异解不稳定。LOAM通过特征值分解来检测这种情况如果某个特征值太小就认为对应的那个方向自由度发生了退化在求解时将这个方向的信息“屏蔽”掉通过matP矩阵投影避免优化向不可靠的方向发散。这是一种简单而有效的鲁棒性处理。优化收敛后transformTobeMapped得到更新它就是当前帧相对于世界坐标系的最优位姿。同时当前帧的特征点会被融合到对应的地图立方体中。最后系统会从活跃的立方体中提取点云经过一次下采样滤波后发布为/laser_cloud_surround这就是我们最终看到的、实时更新的三维地图。6. 实战踩坑编译、运行与调试loam_velodyne原理懂了不跑起来看看都是纸上谈兵。loam_velodyne的编译和运行本身不算复杂但有几个坑我踩过值得你注意。环境准备首先确保你有一个ROS环境Melodic或Noetic比较常见。然后从GitHub上克隆代码。依赖主要是PCL和Eigen通常ROS桌面版安装都会包含。cd ~/catkin_ws/src git clone https://github.com/laboshinl/loam_velodyne.git cd .. catkin_make编译过程一般很顺利。最大的坑往往出现在数据接口上。loam_velodyne默认订阅的Topic是/velodyne_points消息类型是sensor_msgs/PointCloud2。但你的雷达驱动发布出来的Topic名字和类型可能不一样。比如我用过的一些国产雷达或者ROS Bag数据Topic可能是/points_raw或者/lidar_points。这时候你有两个选择一是在loam_velodyne.launch文件里修改scanRegistration节点的remap参数更简单的方法是在启动时用ROS的remapping功能roslaunch loam_velodyne loam_velodyne.launch # 如果你的点云topic不同可以这样启动 roslaunch loam_velodyne loam_velodyne.launch points_topic:/your_pointcloud_topic运行与可视化启动launch文件后再播放你的雷达数据包。如果一切正常你应该能在Rviz里添加点云显示订阅/laser_cloud_surround来看到逐渐构建的3D地图订阅/laser_cloud_sharp等能看到提取的特征点。这能帮你直观地判断特征提取是否正常——边缘点是否大多在墙角、桌沿平面点是否在墙面、地面。参数调优代码中有一些关键参数直接影响性能你可能需要根据实际场景调整scanPeriod在scanRegistration.cpp中这是激光雷达扫描一圈的时间秒。对于Velodyne VLP-16是0.1秒10Hz对于HDL-32E等可能不同。这个值直接影响运动畸变校正的精度务必设对。特征点数量阈值在scanRegistration.cpp中有挑选sharp和flat点的数量限制。在特征丰富的室内可以保持默认在空旷的室外可能需要调低阈值否则可能找不到足够特征点。mapFrameNum在laserMapping.cpp中这个参数控制地图发布和更新的频率。默认是1即每处理1帧就更新并发布一次全局地图。增大这个值可以降低计算负载但会降低地图更新的实时性。常见问题地图漂移或发散首先检查特征点提取是否正常。可能是环境特征太少如长走廊或者运动过快导致两帧间重叠区域太小。可以尝试降低雷达帧率或者看看是否能用IMU提供初始旋转估计loam_velodyne支持IMU输入但需要正确配置话题和坐标变换。CPU占用率过高laserMapping模块是计算大户尤其是当活跃地图区域很大时。可以尝试调大划分立方体的尺寸cube大小或者减少mapFrameNum。Z轴方向倾斜这是初学者常问的问题。建出来的地图是歪的。这通常是因为没有提供正确的初始位姿或者雷达安装的俯仰角没有补偿。检查你的雷达安装是否水平或者在启动前通过静态TF发布一个初始的俯仰角变换。调试时多利用Rviz观察中间结果比如特征点、里程计轨迹。把问题定位到是特征提取不好、匹配不对、还是优化出了问题能帮你更快地找到解决方案。
深入解析LOAM：从3D激光SLAM原理到loam_velodyne代码实现
1. 从2D到3D为什么LOAM是理解激光SLAM的绝佳起点做了好几年2D激光SLAM从Gmapping到Cartographer感觉二维世界已经摸得差不多了。但每次看到机器人或者自动驾驶汽车在复杂多层停车场、或者有上下坡的园区里跑心里就痒痒的——2D地图根本画不出这个世界的全貌啊。于是我下定决心要啃下一个经典的3D激光SLAM算法。在众多开源方案里我最终锁定了LOAM更具体地说是它的一个经典实现版本loam_velodyne。为什么选它原因很简单它足够“原始”也足够“经典”。它没有用任何深度学习的花招纯粹靠几何和数学就把实时建图和定位这事儿给办了这让我觉得吃透了它才算真正理解了3D激光SLAM的筋骨。LOAM全称Lidar Odometry and Mapping in Real-time直译就是“实时激光雷达里程计与建图”。这个名字就点出了它的核心任务一边运动里程计一边画地图建图。和2D SLAM最大的不同在于我们处理的不再是一个平面上的扫描线而是一个由激光雷达扫出来的、布满空间点的“云”。想象一下你拿着一个喷雾罐对着房间喷出一片均匀的雾气然后用特殊的相机拍下这些雾点的三维坐标——这就是一帧3D激光点云。LOAM要做的就是根据这一帧帧连续的点云反推出自己是怎么运动的并且把这些点云拼成一张完整的三维地图。对于刚接触3D SLAM的朋友来说直接看论文里的数学公式可能会有点发怵。但别担心我们可以把LOAM的核心思想先做个生活化的比喻。你可以把它想象成一个人在一片陌生的、布满特征物的森林里蒙眼走路。他每走一步就伸手触摸周围获取一帧点云。他需要记住刚才摸到的几棵特别的树特征点比如一棵特别粗糙的树皮边缘点和一块平坦的大石头平面点。走下一步后他再次伸手寻找并匹配这些特别的树和石头。通过计算这次摸到的位置和上次记忆中的位置之间的差异他就能推断出自己这一步是往前走了还是往左拐了运动估计。同时他会在心里不断更新一幅森林的地图地图构建。LOAM的巧妙之处在于它并不笨拙地去匹配所有“摸到”的点那计算量太大了而是只专注于那些最有代表性的“特征点”从而实现了实时性。那么loam_velodyne这个代码库就是LOAM论文思想的具体工程实现。它最初是针对Velodyne的16线或32线激光雷达设计的代码结构清晰是学习算法如何落地的绝佳材料。接下来我们就一起打开这个“黑匣子”从数据流动开始一步步看明白它到底是怎么工作的。2. 庖丁解牛loam_velodyne的代码骨架与数据流拿到loam_velodyne的代码第一步不是扎进某个文件而是要先理清它的整体结构。这就像看一座建筑的蓝图知道了承重墙和管道走向装修起来才心里有数。整个系统由几个ROS节点组成它们通过Topic串联起来形成一个高效的处理流水线。核心节点就三个scanRegistration、laserOdometry和laserMapping。它们的分工非常明确scanRegistration扫描注册这是流水线的第一站。它订阅原始的激光点云Topic默认是/velodyne_points然后干一件最重要的事——特征点提取。它像是一个挑剔的质检员从海量的、杂乱的点云中筛选出那些“与众不同”的点分为两大类Sharp尖锐点通常是边缘和Flat平坦点通常是平面。处理完后它会发布诸如/laser_cloud_sharp、/laser_cloud_flat这样的Topic。laserOdometry激光里程计这是运动的快速估算者。它订阅scanRegistration提取出的特征点以较高的频率约10Hz快速估计相邻两帧点云之间的运动变换。它的目标是“快”和“准”为后续的建图提供一个不错的初始位姿估计。它发布的是高频的里程计信息/laser_odom_to_init。laserMapping激光建图这是精益求精的地图绘制师。它以较低的频率约1Hz运行订阅laserOdometry的位姿估计和scanRegistration的特征点。它的任务不是简单地累加点云而是进行全局优化和地图管理。它会将新的特征点融合到一个不断增长、并经过优化的全局地图中并发布最终用于导航和显示的稠密点云地图/laser_cloud_surround。这个“高频里程计低频建图”的双层架构是LOAM保证实时性的关键设计。里程计快速响应运动避免累积误差瞬间爆炸建图模块则慢工出细活在后台进行更精确的优化和拼接修正里程计的漂移。这种思想影响深远后来很多激光SLAM方案都借鉴了这种思路。在代码里数据流就像一条河。原始点云流入scanRegistration被提炼成特征点精华这些精华一方面快速流入laserOdometry估算运动另一方面和运动估计一起流入laserMapping进行沉淀形成最终的地图。理解了这个流程我们再深入每个环节就不会迷失在代码细节里了。3. 慧眼识珠特征点提取的奥秘与代码实现特征点提取是LOAM所有后续工作的基石。如果选的点不靠谱后面的匹配和估计全得垮掉。那么LOAM是如何定义和挑选特征点的呢它的标准非常直观看一个点“不平”的程度。在三维空间中一个点是处在边缘拐角还是位于平坦墙面可以通过它和周围邻居点的分布关系来判断。LOAM使用曲率Curvature来量化这种“不平整度”。计算曲率的方法很直接对于一个中心点取其前后各5个点同一扫描线内计算这些邻居点坐标和与中心点坐标的差异。公式在代码里长这样float diffX laserCloud-points[i - 5].x ... laserCloud-points[i 5].x - 10 * laserCloud-points[i].x; float diffY ... // 同理Y坐标 float diffZ ... // 同理Z坐标 cloudCurvature[i] diffX * diffX diffY * diffY diffZ * diffZ;你可以这么理解如果这个点在一个平面上它前后左右的点高度都差不多那么这些diff值会很小曲率也就小。如果这个点在一个边缘上比如桌沿它一侧的点在桌面上另一侧的点在空气中坐标差异会很大曲率值也就很大。所以曲率大的点我们认为是边缘点Sharp曲率特别小的点我们认为是平面点Flat。但直接按曲率排序取最大和最小就行了吗不行这样选出来的特征点可能会扎堆在同一个物体上导致特征分布不均匀。LOAM采用了一个很聪明的策略将每一圈激光扫描Scan均匀分成6段在每一段里分别挑选特征点。这样做保证了特征点能均匀地分布在周围环境中无论近处还是远处都有代表参与匹配提高了系统的鲁棒性。在scanRegistration.cpp中这个挑选过程充满了工程上的细节考量。首先代码会滤除每条扫描线最前和最后的几个点因为这些点常常由于激光雷达的物理特性而不稳定。然后在每一小段里对点的曲率进行升序排序。接着从曲率最大的开始即这段里最“边缘”的点选取不超过2个点作为cornerPointsSharp再往下取一些作为cornerPointsLessSharp。对于平面点则从曲率最小的开始选取不超过4个点作为surfPointsFlat。这里还有一个关键的“去干扰”操作用cloudNeighborPicked数组来标记。如果一个点被选为特征点那么它相邻的几个点就会被“屏蔽”不再参与挑选。这是为了防止在同一个边缘或平面上选取过于密集的特征点同时也是为了排除一些不可靠的点比如“断点”——即激光打在物体边缘时可能一个点打在物体上下一个点打在远处背景上这两个点虽然物理上相邻但在空间上距离很远这种点计算出的曲率会异常大但不是稳定的特征。我刚开始读这段代码时对那个point.intensity的用法印象很深。它巧妙地把scan ID整数部分和该点在此次扫描中的相对时间小数部分编码在了一起。这个相对时间信息非常重要因为激光雷达旋转扫描需要时间一帧点云里的点并不是同一时刻采集的。在后续的运动估计中需要根据这个时间戳进行运动畸变校正假设雷达在扫描一帧期间是匀速运动的以此来补偿每个点因运动而产生的坐标偏差。4. 时空关联特征匹配与运动估计如何实现提取出特征点后下一步就是重头戏如何通过两帧之间的特征点算出雷达在这段时间里到底怎么动的这就是特征匹配与运动估计发生在laserOdometry.cpp中。这个过程可以类比为玩“找相同”游戏。我们有上一帧的“边缘点集合”和“平面点集合”以及当前帧新提取的“边缘点”和“平面点”。对于当前帧的每一个边缘点我们需要在上一帧的边缘点集合里找到一条“最合适”的直线与之匹配。注意是找一条线而不是一个点。为什么因为一个边缘点通常来源于物体的棱线它在上一帧中对应的不应该是一个孤立的点而应该是同一根棱线上的另一个点。所以LOAM的做法是在上一帧的点云里用KD-Tree快速找到距离当前点最近的一个点A然后以A为起点在同一条扫描线或相邻扫描线的附近寻找另一个点B确保A和B能构成一条有意义的线段。找到这条线段点A和点B后就可以计算当前点到这条线段的垂直距离。理想情况下如果运动估计完全正确当前点应该正好落在这条线段上距离为零。但实际上由于估计不准会有一个距离d。我们的目标就是找到一个雷达的位姿变换包括旋转和平移共6个自由度使得所有当前帧特征点到其对应上一帧特征线/面的距离之和最小。对于平面点也是类似的逻辑只不过是在上一帧的平面点集合里找三个点构成一个平面然后计算当前点到这个平面的距离。这里有一个非常关键的细节也是LOAM论文里的一个核心假设雷达的运动是连续的、平滑的。这使得我们可以用上一次估计的变换或者结合IMU数据给出的初始变换作为本次迭代优化的起点。在代码中laserOdometry会进行多次迭代比如25次每次迭代都重新计算距离和雅可比矩阵然后用列文伯格-马夸尔特Levenberg-Marquardt, LM这样的非线性优化方法求解出一个位姿变换的增量不断更新直到收敛。我特别想提一下代码里iterCount % 5 0这个条件。它控制着KD-Tree搜索的更新频率。在迭代优化开始时我们根据初始位姿将当前点变换到上一帧坐标系下然后去KD-Tree里找匹配。随着优化的进行位姿估计在不断更新此时当前点变换后的坐标也在变。如果每次迭代都重新搜索最近点计算量太大如果一直不更新匹配关系可能不准。LOAM采取了一个折中策略每迭代5次才根据最新的位姿估计重新搜索一次最近点。这是一种在精度和计算效率之间的巧妙平衡。最终laserOdometry会输出一个高频的、相对准确的里程计变换transformSum。这个变换是累加的表示了从起始时刻到当前时刻雷达的整体运动。但它存在累积误差这就需要低频但更精确的laserMapping来校正和优化。5. 精益求精地图构建与全局优化详解如果说laserOdometry是先锋负责快速探路那么laserMapping就是大后方负责巩固疆土、绘制精确地图。它的输入是laserOdometry提供的位姿估计作为优化初值和scanRegistration提供的特征点输出是优化后的全局位姿和拼接好的地图点云。laserMapping的核心任务有两个1. 将新的特征点融合到全局地图中2. 基于更庞大的历史地图数据对当前帧的位姿进行优化消除里程计的漂移。它第一个巧妙的设计是地图管理。想象一下如果地图无限增长每次都要在整个地图里搜索匹配点计算量是无法承受的。LOAM采用了一个“滑动窗口”式的子图管理策略。它将整个空间划分成许多个固定大小例如50m x 50m x 50m的立方体Voxel。代码中定义了laserCloudCornerArray和laserCloudSurfArray两个大的数组来管理这些立方体。每个新来的特征点根据其优化后的全局坐标被存入对应的立方体。同时系统只维护机器人周围一定范围内的立方体比如前后左右各10个上下各5个为“活跃区域”。只有活跃区域内的点云才会被用来进行当前帧的匹配优化。当机器人移动时活跃区域也随之滑动并将移出区域的立方体从内存中释放或保存。这种设计极大地限制了每次参与计算的地图规模。第二个关键步骤是在全局地图中寻找更准确的匹配。这个过程和里程计中的匹配类似但搜索范围更大、约束更强。例如对于当前帧的一个边缘点它会在全局地图的活跃区域里寻找最近的5个边缘点然后检查这5个点是否真的能拟合成一条直线通过计算协方差矩阵的特征值来判断。如果能才认为找到了有效的匹配并计算点到直线的距离。这个匹配标准比里程计阶段更严格因为它利用了更丰富、更稳定的历史地图信息。接下来就是迭代优化。laserMapping构建的误差方程和laserOdometry在形式上是一样的都是最小化点到线、点到面的距离。但是这里的待优化变量transformTobeMapped是相对于全局地图坐标系的位姿。优化过程中它会利用所有匹配上的特征点通常数量远多于里程计阶段构建一个更大的线性方程组matA * matX matB求解位姿修正量matX。代码中有一段关于退化Degeneracy的处理非常值得注意。在某些几何特征匮乏的环境比如长长的走廊或空旷的平地所有匹配约束可能都集中在某一个或某几个方向上比如在走廊里约束主要来自两侧的墙而在前进方向上缺乏约束。这会导致求解问题的矩阵matAtA接近奇异解不稳定。LOAM通过特征值分解来检测这种情况如果某个特征值太小就认为对应的那个方向自由度发生了退化在求解时将这个方向的信息“屏蔽”掉通过matP矩阵投影避免优化向不可靠的方向发散。这是一种简单而有效的鲁棒性处理。优化收敛后transformTobeMapped得到更新它就是当前帧相对于世界坐标系的最优位姿。同时当前帧的特征点会被融合到对应的地图立方体中。最后系统会从活跃的立方体中提取点云经过一次下采样滤波后发布为/laser_cloud_surround这就是我们最终看到的、实时更新的三维地图。6. 实战踩坑编译、运行与调试loam_velodyne原理懂了不跑起来看看都是纸上谈兵。loam_velodyne的编译和运行本身不算复杂但有几个坑我踩过值得你注意。环境准备首先确保你有一个ROS环境Melodic或Noetic比较常见。然后从GitHub上克隆代码。依赖主要是PCL和Eigen通常ROS桌面版安装都会包含。cd ~/catkin_ws/src git clone https://github.com/laboshinl/loam_velodyne.git cd .. catkin_make编译过程一般很顺利。最大的坑往往出现在数据接口上。loam_velodyne默认订阅的Topic是/velodyne_points消息类型是sensor_msgs/PointCloud2。但你的雷达驱动发布出来的Topic名字和类型可能不一样。比如我用过的一些国产雷达或者ROS Bag数据Topic可能是/points_raw或者/lidar_points。这时候你有两个选择一是在loam_velodyne.launch文件里修改scanRegistration节点的remap参数更简单的方法是在启动时用ROS的remapping功能roslaunch loam_velodyne loam_velodyne.launch # 如果你的点云topic不同可以这样启动 roslaunch loam_velodyne loam_velodyne.launch points_topic:/your_pointcloud_topic运行与可视化启动launch文件后再播放你的雷达数据包。如果一切正常你应该能在Rviz里添加点云显示订阅/laser_cloud_surround来看到逐渐构建的3D地图订阅/laser_cloud_sharp等能看到提取的特征点。这能帮你直观地判断特征提取是否正常——边缘点是否大多在墙角、桌沿平面点是否在墙面、地面。参数调优代码中有一些关键参数直接影响性能你可能需要根据实际场景调整scanPeriod在scanRegistration.cpp中这是激光雷达扫描一圈的时间秒。对于Velodyne VLP-16是0.1秒10Hz对于HDL-32E等可能不同。这个值直接影响运动畸变校正的精度务必设对。特征点数量阈值在scanRegistration.cpp中有挑选sharp和flat点的数量限制。在特征丰富的室内可以保持默认在空旷的室外可能需要调低阈值否则可能找不到足够特征点。mapFrameNum在laserMapping.cpp中这个参数控制地图发布和更新的频率。默认是1即每处理1帧就更新并发布一次全局地图。增大这个值可以降低计算负载但会降低地图更新的实时性。常见问题地图漂移或发散首先检查特征点提取是否正常。可能是环境特征太少如长走廊或者运动过快导致两帧间重叠区域太小。可以尝试降低雷达帧率或者看看是否能用IMU提供初始旋转估计loam_velodyne支持IMU输入但需要正确配置话题和坐标变换。CPU占用率过高laserMapping模块是计算大户尤其是当活跃地图区域很大时。可以尝试调大划分立方体的尺寸cube大小或者减少mapFrameNum。Z轴方向倾斜这是初学者常问的问题。建出来的地图是歪的。这通常是因为没有提供正确的初始位姿或者雷达安装的俯仰角没有补偿。检查你的雷达安装是否水平或者在启动前通过静态TF发布一个初始的俯仰角变换。调试时多利用Rviz观察中间结果比如特征点、里程计轨迹。把问题定位到是特征提取不好、匹配不对、还是优化出了问题能帮你更快地找到解决方案。
