1. 这个文件整体是干什么的这个laserOdometry.cpp是 A-LOAM 里面的前端激光里程计模块。它不负责原始点云特征提取也不负责维护大地图而是接收前面特征提取模块发布出来的角点、面点和全分辨率点云然后做当前帧与上一帧之间的 scan-to-scan 匹配估计当前 LiDAR 相对于上一帧 LiDAR 的相对运动最后把相对运动一帧一帧累计成/laser_odom_to_init这个里程计位姿。整个链路可以理解成原始点云 ↓ scanRegistration / featureExtraction ↓ 提取角点 sharp / less sharp面点 flat / less flat ↓ laserOdometry当前帧特征点 与 上一帧特征点 scan-to-scan 匹配 ↓ 输出较高频的前端里程计 /laser_odom_to_init ↓ laserMapping接收 odometry 和特征点做 scan-to-map 精修也就是说laserOdometry的核心目标是快。它只看当前帧和上一帧不维护大范围局部地图所以计算量比较小能比较实时地输出位姿。但缺点也明显只做相邻帧匹配误差会逐帧累计。因此 A-LOAM 后面还要靠laserMapping用局部地图再修正。这个文件里面最关键的思想是当前帧角点去上一帧角点里找一条线构造点到线残差当前帧平面点去上一帧平面点里找一个平面构造点到面残差然后用 Ceres 优化当前帧相对于上一帧的旋转和平移。2. 头文件部分ROS、PCL、Eigen、Ceres 和 A-LOAM 工具文件开头第 37 到 57 行引入了一批依赖。ROS 相关头文件负责订阅和发布消息比如nav_msgs/Odometry用来发布里程计nav_msgs/Path用来发布轨迹sensor_msgs/PointCloud2用来接收和发布点云。PCL 相关头文件负责点云存储、点云转换和 KD-Tree 最近邻搜索其中pcl::KdTreeFLANN很关键因为每个当前帧特征点都要去上一帧特征点里找最近点如果暴力搜索会很慢。Eigen用来表示四元数、平移向量和三维点坐标。lidarFactor.hpp是 A-LOAM 里非常关键的优化因子文件里面定义了LidarEdgeFactor和LidarPlaneFactor。前者对应点到线残差后者对应点到面残差。common.h里一般定义PointType等通用类型tic_toc.h用来统计耗时。这里要注意A-LOAM 相比原始 LOAM一个明显变化就是用Ceres Solver来做非线性优化。这样代码比原始 LOAM 手写雅可比更清晰角点约束、面点约束都可以作为残差块加入优化问题。3. 全局参数控制匹配、时间、点云和位姿第 59 行#define DISTORTION 0这个宏控制是否启用点云运动畸变补偿。当前代码里是0表示默认关闭点云去畸变。也就是说后面TransformToStart()里插值比例s会直接取1.0。如果打开畸变补偿代码会用点的intensity小数部分表示该点在一帧扫描周期里的相对时间再根据s对位姿做插值。第 62 到 66 行int corner_correspondence 0, plane_correspondence 0; constexpr double SCAN_PERIOD 0.1; constexpr double DISTANCE_SQ_THRESHOLD 25; constexpr double NEARBY_SCAN 2.5;corner_correspondence和plane_correspondence分别统计当前优化中找到的角点约束数量和平面点约束数量。SCAN_PERIOD 0.1表示一帧 LiDAR 扫描周期是 0.1 秒也就是 10Hz。DISTANCE_SQ_THRESHOLD 25表示最近邻距离平方阈值是 25也就是实际距离大约不能超过 5 米。超过这个范围代码认为匹配不可靠不加入优化。NEARBY_SCAN 2.5表示只在相邻若干条扫描线内寻找第二个角点或第三个面点避免跨太远线束找错误几何关系。第 68 到 75 行是系统初始化标志和时间戳变量。A-LOAM 这里会接收五类点云sharp 角点、less sharp 角点、flat 面点、less flat 面点、全分辨率点云。它们必须来自同一帧所以后面主循环里会检查这些消息的时间戳是否一致。如果时间戳不一致就会打印unsync messeage!并中断。这是为了防止拿 A 帧角点和 B 帧面点一起优化导致匹配完全错乱。4. 点云容器哪些点参与优化哪些点作为上一帧地图第 80 到 87 行定义了几类点云cornerPointsSharp cornerPointsLessSharp surfPointsFlat surfPointsLessFlat laserCloudCornerLast laserCloudSurfLast laserCloudFullRes这里很容易混淆重点区分一下。cornerPointsSharp是当前帧最尖锐的角点数量较少质量较高主要用于构造点到线约束。cornerPointsLessSharp是当前帧较宽松的角点集合数量比 sharp 多主要用于保存成上一帧角点云给下一帧做匹配也会发布给后端 mapping。也就是说优化用的是cornerPointsSharp但作为上一帧 KD-Tree 输入的是cornerPointsLessSharp。surfPointsFlat是当前帧最平坦的面点数量较少主要用于构造点到面约束。surfPointsLessFlat是更宽松的面点集合数量更多用于保存成上一帧面点云也会发给 mapping。优化用的是surfPointsFlat但下一帧匹配的目标点云是surfPointsLessFlat。laserCloudCornerLast和laserCloudSurfLast表示上一帧用于匹配的角点集合和面点集合。第二帧开始当前帧 sharp / flat 会去这两个上一帧点云里找匹配。laserCloudFullRes是当前帧完整点云它不直接参与 laserOdometry 的优化主要用于后续发布和可视化。所以这部分可以概括成当前帧 sharp 角点 → 用来构造点到线残差 当前帧 flat 面点 → 用来构造点到面残差 当前帧 less sharp → 处理完后变成下一帧的 laserCloudCornerLast 当前帧 less flat → 处理完后变成下一帧的 laserCloudSurfLast5. 位姿变量q_w_curr、t_w_curr 和 q_last_curr、t_last_curr第 92 到 101 行是整个文件最关键的位姿变量。Eigen::Quaterniond q_w_curr(1, 0, 0, 0); Eigen::Vector3d t_w_curr(0, 0, 0); double para_q[4] {0, 0, 0, 1}; double para_t[3] {0, 0, 0}; Eigen::MapEigen::Quaterniond q_last_curr(para_q); Eigen::MapEigen::Vector3d t_last_curr(para_t);q_w_curr和t_w_curr表示当前帧在初始坐标系/camera_init下的累计位姿也就是最终要发布出去的 laser odometry。可以理解为T_w_curr 当前 LiDAR 帧相对于初始世界坐标系的位姿para_q和para_t是 Ceres 要优化的参数块。它们表示当前帧到上一帧之间的相对位姿也就是T_last_curr 上一帧坐标系下的当前帧位姿代码通过Eigen::Map把普通数组映射成 Eigen 四元数和平移向量。这样做的好处是Ceres 优化时直接改para_q和para_t而代码里可以方便地用q_last_curr和t_last_curr做数学运算。这里有一个重点Ceres 优化的不是全局位姿而是当前帧相对上一帧的位姿。全局位姿是后面通过递推累计出来的t_w_curr t_w_curr q_w_curr * t_last_curr; q_w_curr q_w_curr * q_last_curr;对应公式可以理解为T_w_curr_new T_w_last × T_last_curr也就是上一帧的全局位姿乘上当前帧相对上一帧的增量得到当前帧的全局位姿。6. TransformToStart()把当前帧点变换到上一帧起始坐标系第 111 到 129 行是TransformToStart()void TransformToStart(PointType const *const pi, PointType *const po) { double s; if (DISTORTION) s (pi-intensity - int(pi-intensity)) / SCAN_PERIOD; else s 1.0; Eigen::Quaterniond q_point_last Eigen::Quaterniond::Identity().slerp(s, q_last_curr); Eigen::Vector3d t_point_last s * t_last_curr; Eigen::Vector3d point(pi-x, pi-y, pi-z); Eigen::Vector3d un_point q_point_last * point t_point_last; }这个函数的作用是把当前帧里的某个点根据当前估计的相对位姿变换到上一帧坐标系下。因为 scan-to-scan 匹配时当前帧点要和上一帧点做几何对齐所以必须先把当前点投到上一帧坐标系里再找最近邻。如果开启DISTORTION每个点会根据自己在扫描周期中的时间比例s使用插值位姿。比如一帧扫描过程中前面的点和后面的点采集时间不同LiDAR 也在运动所以不能简单用同一个刚体变换处理所有点。这里会通过四元数球面插值slerp和平移线性插值做去畸变。但是当前代码里DISTORTION 0所以s 1.0也就是每个点都直接使用完整的q_last_curr和t_last_curr变换。这样代码逻辑更简单但严格来说没有补偿一帧扫描内部的运动畸变。公式可以写成P_last R_last_curr × P_curr t_last_curr其中P_curr是当前帧 LiDAR 坐标系下的点P_last是变换到上一帧坐标系后的点R_last_curr和t_last_curr是当前帧相对上一帧的位姿增量。7. TransformToEnd()把点变换到当前扫描结束时刻第 133 到 148 行是TransformToEnd()。它先调用TransformToStart()做去畸变然后再用q_last_curr.inverse()和t_last_curr把点变到扫描结束时刻坐标系Eigen::Vector3d point_end q_last_curr.inverse() * (un_point - t_last_curr);这个函数原本用于把特征点和全分辨率点云统一到当前帧结束时刻方便发布给后端。但是在你这个代码里第 532 行附近有if (0) { ... TransformToEnd(...) }也就是说这段实际被关闭了不会执行。所以当前文件主要还是使用TransformToStart()完成匹配前的坐标变换。8. 回调函数只负责把消息放进队列第 150 到 183 行是五个 ROS 回调函数laserCloudSharpHandler laserCloudLessSharpHandler laserCloudFlatHandler laserCloudLessFlatHandler laserCloudFullResHandler这些函数逻辑都很简单加锁把收到的 ROS 点云消息 push 到对应队列里然后解锁。它们不做优化、不做匹配、不做点云处理。真正处理是在main()的 while 循环里完成的。这么设计的原因是 ROS 回调最好不要做太重的计算。回调只负责收数据主循环再统一判断五类点云是否都到齐并检查时间戳是否一致。这样可以避免某一个回调耗时过长导致其他消息阻塞。9. main() 初始化订阅输入发布输出第 186 到 218 行是 ROS 节点初始化。节点名是ros::init(argc, argv, laserOdometry);它订阅五个话题/laser_cloud_sharp /laser_cloud_less_sharp /laser_cloud_flat /laser_cloud_less_flat /velodyne_cloud_2这些通常来自前面的特征提取模块。/laser_cloud_sharp是尖锐角点/laser_cloud_less_sharp是较宽松角点/laser_cloud_flat是平面点/laser_cloud_less_flat是较宽松平面点/velodyne_cloud_2是处理后的全分辨率点云。它发布五个话题/laser_cloud_corner_last /laser_cloud_surf_last /velodyne_cloud_3 /laser_odom_to_init /laser_odom_path其中/laser_odom_to_init是最重要的输出也就是当前 LiDAR 相对于初始坐标系的前端里程计。/laser_odom_path用于 RViz 显示轨迹。/laser_cloud_corner_last、/laser_cloud_surf_last、/velodyne_cloud_3会给后面的 mapping 使用。10. 主循环第一步等待五类消息并做时间同步第 224 到 263 行是主循环的数据准备部分。代码先判断五个队列是否都不为空if (!cornerSharpBuf.empty() !cornerLessSharpBuf.empty() !surfFlatBuf.empty() !surfLessFlatBuf.empty() !fullPointsBuf.empty())只有五类数据都到了才会继续处理。然后取五个消息的时间戳timeCornerPointsSharp timeCornerPointsLessSharp timeSurfPointsFlat timeSurfPointsLessFlat timeLaserCloudFullRes接着检查它们是否完全等于timeLaserCloudFullRes。如果不同步直接ROS_BREAK()。这里之所以能用精确相等是因为这些点云通常是同一个 feature extraction 节点从同一帧原始点云拆出来的header 时间戳应该完全一致。通过时间同步检查后代码把 ROSPointCloud2转成 PCL 点云并把队列头部 pop 掉。到这里当前帧的角点、面点、全点云就准备好了。11. 初始化逻辑第一帧只建立上一帧参考第 267 到 271 行if (!systemInited) { systemInited true; std::cout Initialization finished \n; }第一帧不会做匹配优化因为还没有上一帧可以匹配。系统只是把systemInited置为 true。然后后面仍然会进入发布和点云交换逻辑把当前帧的cornerPointsLessSharp和surfPointsLessFlat变成laserCloudCornerLast和laserCloudSurfLast并建立 KD-Tree。所以第一帧的作用是不估计相对运动 把当前帧 less sharp / less flat 保存成上一帧参考点云 给第二帧匹配做准备第二帧开始系统就有上一帧角点和面点了可以做 scan-to-scan 匹配。12. Ceres 优化框架外层两次内层最多四次迭代第 274 到 291 行开始进入优化部分int cornerPointsSharpNum cornerPointsSharp-points.size(); int surfPointsFlatNum surfPointsFlat-points.size(); for (size_t opti_counter 0; opti_counter 2; opti_counter) { ... ceres::Problem problem(problem_options); problem.AddParameterBlock(para_q, 4, q_parameterization); problem.AddParameterBlock(para_t, 3); }这里外层循环跑 2 次。每次外层循环都会重新找角点和面点对应关系然后构建 Ceres 问题并求解。这个设计很重要因为刚开始的位姿初值可能不够准第一次根据初值找的对应点不一定最好优化一次之后位姿更准再重新找一次对应点约束会更可靠。Ceres 参数块有两个para_q四元数表示当前帧到上一帧的旋转 para_t三维向量表示当前帧到上一帧的平移ceres::EigenQuaternionParameterization()保证四元数优化过程中保持单位四元数。否则四元数四个参数随便被优化可能会偏离单位长度导致旋转不合法。这里还用了ceres::HuberLoss(0.1)Huber 核函数用于降低异常匹配点的影响。如果某些点匹配错了残差会很大HuberLoss 可以避免这些大残差过度拉偏位姿。13. 角点匹配当前 sharp 点找上一帧两点成线第 298 到 383 行是角点约束构建。核心流程是遍历当前帧 cornerPointsSharp ↓ 把当前角点 TransformToStart 到上一帧坐标系 ↓ 在上一帧 laserCloudCornerLast 里 KD-Tree 找最近点 A ↓ 在 A 附近不同扫描线里找第二个角点 B ↓ 用 A 和 B 构造一条线 ↓ 当前点到这条线的距离作为残差代码先对当前角点做坐标变换TransformToStart((cornerPointsSharp-points[i]), pointSel);然后用 KD-Tree 找上一帧角点中的最近点kdtreeCornerLast-nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);如果最近点距离平方小于 25说明这个最近点比较可靠。然后代码取它的扫描线 IDint closestPointScanID int(laserCloudCornerLast-points[closestPointInd].intensity);这里intensity的整数部分一般保存 scan line ID也就是这个点来自第几根激光线。A-LOAM 用这个信息来避免选到同一条扫描线上的两个点。因为角点约束需要一条空间线如果两个点来自完全相同的扫描线几何稳定性可能比较差所以代码会向扫描线 ID 增大的方向和减小的方向搜索找附近不同 scan line 的另一个点。找到closestPointInd和minPointInd2后就有了上一帧的两个角点last_point_a last_point_b当前帧角点是curr_point然后创建点到线残差ceres::CostFunction *cost_function LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s); problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);点到线残差可以理解成e_edge 当前点到上一帧两角点构成直线的距离公式大概是e_edge || (P - A) × (P - B) || / || A - B ||其中P是当前帧角点经过当前估计位姿变换后的点A和B是上一帧中的两个角点。优化希望这个距离越小越好也就是让当前帧角点落到上一帧对应边缘线上。这就是 A-LOAM 角点约束的核心角点不和角点做一一重合而是让当前角点贴近上一帧的边缘线。14. 面点匹配当前 flat 点找上一帧三点成面第 386 到 483 行是面点约束构建。流程和角点类似只不过面点需要找三个点构造平面遍历当前帧 surfPointsFlat ↓ TransformToStart 到上一帧坐标系 ↓ 在上一帧 laserCloudSurfLast 中 KD-Tree 找最近点 A ↓ 在相邻扫描线附近继续找点 B 和点 C ↓ A、B、C 构造平面 ↓ 当前点到这个平面的距离作为残差代码同样先变换当前平面点TransformToStart((surfPointsFlat-points[i]), pointSel);然后从上一帧平面点云里找最近点kdtreeSurfLast-nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);找到最近点后代码会在附近扫描线里找另外两个点minPointInd2和minPointInd3。这里的策略比角点更复杂一些因为平面需要三个不共线点。代码希望这三个点来自合理的扫描线组合避免三个点几乎在一条线上导致平面不稳定。找到三个点后last_point_a last_point_b last_point_c然后创建点到面残差ceres::CostFunction *cost_function LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s); problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);点到面残差可以理解成e_plane 当前点到上一帧三个面点构成平面的距离公式大概是n ((B - A) × (C - A)) / ||(B - A) × (C - A)|| e_plane n^T × (P - A)其中A、B、C是上一帧平面点P是当前帧平面点经过位姿变换后的点n是平面法向量。优化希望P到平面的距离尽可能小。这就是 A-LOAM 面点约束的核心面点不追求和某个单点重合而是让当前面点落到上一帧对应平面上。15. Ceres 求解优化当前帧相对于上一帧的位姿第 488 到 499 行是求解部分if ((corner_correspondence plane_correspondence) 10) { printf(less correspondence! *************************************************\n); } ceres::Solver::Options options; options.linear_solver_type ceres::DENSE_QR; options.max_num_iterations 4; options.minimizer_progress_to_stdout false; ceres::Solve(options, problem, summary);如果角点约束和平面点约束总数小于 10说明当前匹配很少位姿估计可能不可靠。代码只是打印 warning并没有直接退出。DENSE_QR是 Ceres 的线性求解器类型。因为 laserOdometry 每次只优化 7 个参数也就是四元数 4 个和平移 3 个问题规模很小所以用 dense 求解器没有问题。max_num_iterations 4表示每次 Ceres 最多迭代 4 次。外层又有 2 次数据关联所以整体是第一次根据初值找匹配 → Ceres 优化最多 4 次 第二次根据更新后的位姿重新找匹配 → Ceres 再优化最多 4 次这个过程就是典型的前端 ICP / LOAM 式迭代找对应关系 → 优化位姿 → 再找对应关系 → 再优化位姿。16. 位姿累计把相邻帧相对运动累加成全局 odometry第 504 到 505 行是 laserOdometry 的关键输出逻辑t_w_curr t_w_curr q_w_curr * t_last_curr; q_w_curr q_w_curr * q_last_curr;这里的q_last_curr和t_last_curr是刚刚 Ceres 优化出来的当前帧相对于上一帧的运动增量。q_w_curr和t_w_curr在更新前表示上一帧的全局位姿更新后表示当前帧的全局位姿。公式可以写成R_w_curr_new R_w_last × R_last_curr t_w_curr_new t_w_last R_w_last × t_last_curr意思是如果上一帧在世界坐标系下的位置已经知道而当前帧相对上一帧的运动也估计出来了那么就可以把相对运动接到上一帧后面得到当前帧的世界位姿。所以laserOdometry输出的轨迹本质上是很多帧间相对位姿连续乘起来的结果。它速度快但是也因为一直累乘所以会有漂移。这就是为什么后面还需要laserMapping做 scan-to-map 修正。17. 发布 odometry 和 path第 510 到 530 行负责发布里程计和轨迹laserOdometry.header.frame_id /camera_init; laserOdometry.child_frame_id /laser_odom;/camera_init可以理解为初始世界坐标系/laser_odom是当前激光里程计坐标系。代码把q_w_curr和t_w_curr填入nav_msgs::OdometrylaserOdometry.pose.pose.orientation.x q_w_curr.x(); ... laserOdometry.pose.pose.position.x t_w_curr.x(); ...然后发布pubLaserOdometry.publish(laserOdometry);同时还把当前 pose 放进laserPath发布/laser_odom_path用于 RViz 显示前端轨迹。18. 更新上一帧点云当前 less 特征变成下一帧匹配目标第 554 到 568 行是非常关键的“帧切换”逻辑pcl::PointCloudPointType::Ptr laserCloudTemp cornerPointsLessSharp; cornerPointsLessSharp laserCloudCornerLast; laserCloudCornerLast laserCloudTemp; laserCloudTemp surfPointsLessFlat; surfPointsLessFlat laserCloudSurfLast; laserCloudSurfLast laserCloudTemp;这里不是深拷贝而是交换指针。当前帧处理完之后当前帧的cornerPointsLessSharp就变成下一帧的laserCloudCornerLast当前帧的surfPointsLessFlat就变成下一帧的laserCloudSurfLast。然后建立 KD-TreekdtreeCornerLast-setInputCloud(laserCloudCornerLast); kdtreeSurfLast-setInputCloud(laserCloudSurfLast);这一步很重要。下一帧来之后当前帧 sharp 角点就会去这个laserCloudCornerLast里找线约束当前帧 flat 面点会去laserCloudSurfLast里找平面约束。所以 laserOdometry 的 scan-to-scan 本质就是第 k 帧 sharp / flat → 匹配第 k-1 帧 less sharp / less flatsharp / flat用作当前帧优化点less sharp / less flat用作上一帧参考点。这样可以保证参考点数量更多几何结构更完整。19. 给 mapping 发布特征点和全点云第 570 到 590 行控制特征点发布if (frameCount % skipFrameNum 0) { ... pubLaserCloudCornerLast.publish(laserCloudCornerLast2); pubLaserCloudSurfLast.publish(laserCloudSurfLast2); pubLaserCloudFullRes.publish(laserCloudFullRes3); }这里的skipFrameNum来自参数nh.paramint(mapping_skip_frame, skipFrameNum, 2);也就是说不一定每一帧都把特征点发给 mapping。比如skipFrameNum 2就是每两帧给 mapping 发一次。这样可以降低后端建图压力。注意/laser_odom_to_init是每帧发布的而给 mapping 的 corner/surf/fullres 点云可以降频发布。这也体现了 A-LOAM 的前后端设计laserOdometry高频快速输出前端里程计 laserMapping低频利用局部地图做精细优化20. 这个文件的完整流程总结这个laserOdometry.cpp的完整流程可以概括为1. 订阅 feature extraction 输出的五类点云 sharp角点、less sharp角点、flat面点、less flat面点、全分辨率点云。 2. 把五类点云放进缓存队列主循环等待它们全部到齐。 3. 检查五类点云时间戳是否一致确保它们属于同一帧。 4. 第一帧只初始化不做位姿优化把当前帧 less sharp / less flat 保存为上一帧参考点云。 5. 从第二帧开始 当前帧 sharp 角点先 TransformToStart 到上一帧坐标系 然后在上一帧角点 KD-Tree 中找两个点构成线 构造点到线残差。 6. 当前帧 flat 面点同样 TransformToStart 到上一帧坐标系 然后在上一帧面点 KD-Tree 中找三个点构成平面 构造点到面残差。 7. 把点到线残差和点到面残差加入 Ceres 优化当前帧相对于上一帧的旋转 q_last_curr 和平移 t_last_curr。 8. 优化完成后把相邻帧相对位姿累加到全局位姿 T_w_curr T_w_last × T_last_curr。 9. 发布 /laser_odom_to_init 和 /laser_odom_path。 10. 把当前帧 less sharp / less flat 变成下一帧的 laserCloudCornerLast / laserCloudSurfLast 并重新建立 KD-Tree。 11. 按 skipFrameNum 降频发布角点、面点和全分辨率点云给 laserMapping。21. 最后总结laserOdometry.cpp是 A-LOAM 前端里程计模块的核心文件它位于特征提取模块之后、laserMapping.cpp后端建图模块之前。它的主要任务不是直接处理原始点云也不是维护全局地图而是接收已经提取好的角点和平面点利用当前帧特征点和上一帧特征点之间的几何约束估计当前帧 LiDAR 相对于上一帧 LiDAR 的相对位姿然后把这个相对位姿一帧一帧累加形成连续的激光里程计轨迹。也就是说它解决的是scan-to-scan 前端匹配问题输出的是一个高频但会累计漂移的前端 odometry后面还需要laserMapping.cpp做 scan-to-map 精修和建图。整个流程从输入上看laserOdometry订阅了五类点云/laser_cloud_sharp、/laser_cloud_less_sharp、/laser_cloud_flat、/laser_cloud_less_flat和/velodyne_cloud_2。其中sharp角点和flat面点数量较少、质量较高主要用于当前帧优化less sharp角点和less flat面点数量更多主要作为上一帧参考点云保存下来供下一帧做匹配全分辨率点云/velodyne_cloud_2主要用于传递给后端 mapping 和可视化。这里要注意laserOdometry并不是直接拿所有点做 ICP而是只利用 LOAM 前面筛选出的几何特征点因此计算量更小实时性更好。程序启动后五个 ROS 回调函数会分别接收这五类点云并把它们放入各自的缓存队列中。回调函数本身不做复杂计算只负责收数据这是为了避免 ROS callback 被耗时操作阻塞。真正的处理逻辑在main()的循环中进行。主循环会不断检查五个队列是否都有数据如果五类点云都到齐就取出它们的时间戳进行同步检查。因为这五类点云应该来自同一帧原始点云所以它们的 header 时间戳必须一致。如果时间戳不一致说明数据可能错帧比如拿了当前帧的角点和上一帧的面点一起优化这会导致匹配关系错误所以代码会直接报错中断。当第一帧数据进入时系统还没有上一帧点云可以匹配因此第一帧不会进行位姿优化。它主要做初始化把当前帧的cornerPointsLessSharp保存成laserCloudCornerLast把当前帧的surfPointsLessFlat保存成laserCloudSurfLast并用它们建立 KD-Tree。这样做的目的就是为第二帧提供匹配参考。也就是说第一帧只是建立“上一帧参考点云”真正的 scan-to-scan 位姿估计从第二帧开始。从第二帧开始laserOdometry的核心流程就是当前帧特征点变换到上一帧坐标系 → 在上一帧特征点中找对应几何结构 → 构造残差 → Ceres 优化当前帧相对上一帧的位姿增量。这里优化的变量是q_last_curr和t_last_curr它们表示当前帧相对于上一帧的旋转和平移。也就是说Ceres 不是直接优化全局位姿而是优化相邻两帧之间的相对运动。全局位姿q_w_curr和t_w_curr是后面通过连续累乘得到的。在构造约束之前代码会调用TransformToStart()把当前帧点变换到上一帧坐标系下。这个函数的作用非常关键因为当前帧点和上一帧点原本处在不同坐标系中不能直接比较距离。只有先用当前估计的相对位姿把当前帧点投到上一帧坐标系里才能在上一帧点云中查找最近邻并构造几何残差。代码里还预留了点云运动畸变补偿逻辑如果DISTORTION打开会根据每个点在一帧扫描周期中的相对时间进行位姿插值但当前代码中DISTORTION 0所以实际使用的是整帧统一位姿变换没有启用逐点去畸变。角点约束的构造过程是遍历当前帧的cornerPointsSharp对每个 sharp 角点先执行TransformToStart()然后用kdtreeCornerLast在上一帧laserCloudCornerLast中找最近的角点。如果最近点距离小于阈值说明这个匹配候选还比较可靠。接着代码会根据 scan line ID 在附近扫描线中继续寻找第二个角点并要求第二个点和最近点不能来自完全相同的扫描线。这样上一帧中就有了两个角点它们可以构成一条空间边缘线。然后当前帧角点和上一帧这两个角点之间构造点到线残差。优化的目标就是让当前帧角点在变换后尽量落到上一帧对应的边缘线上。这里不是简单点到点匹配而是点到线匹配因此更符合 LiDAR 角点特征的几何意义。面点约束的构造过程类似但它需要三个点构造平面。代码会遍历当前帧的surfPointsFlat先把当前平面点变换到上一帧坐标系再用kdtreeSurfLast在上一帧laserCloudSurfLast中找最近面点。找到最近点后还会在附近扫描线中继续寻找另外两个面点尽量保证这三个点能够形成一个稳定平面而不是退化成一条线。找到三个上一帧面点后就可以构造一个平面然后把当前帧面点到这个平面的距离作为残差。优化目标就是让当前帧平面点在变换后尽量贴合上一帧对应平面。这样角点约束主要提供边缘结构约束面点约束主要提供平面结构约束两者共同约束 LiDAR 的 6DoF 位姿。构造完角点残差和面点残差后代码会把这些残差块加入 Ceres 优化问题。Ceres 中的优化参数包括四元数para_q和平移para_t分别对应当前帧相对上一帧的旋转和平移。四元数使用EigenQuaternionParameterization进行参数化保证优化过程中旋转仍然是合法的单位四元数。残差项外面加了HuberLoss鲁棒核函数用来降低错误匹配点对优化结果的影响。因为实际点云匹配中难免会有动态物体、遮挡、重复结构、最近邻错误等问题如果没有鲁棒核大误差点可能会把位姿优化结果拉偏。优化过程采用两层迭代思想。外层通常循环两次每次都会重新根据当前位姿估计寻找特征对应关系内层 Ceres 每次最多迭代若干次比如代码中设置为最多 4 次。这样做的原因是一开始位姿初值可能不够准第一次找出来的对应点可能不是最优的经过一次 Ceres 优化后位姿更接近真实值再重新查找对应点得到的点线、点面约束会更可靠然后再优化一次最终得到更稳定的相邻帧位姿增量。这个过程本质上就是 LOAM 风格的迭代配准先找对应关系再优化位姿再用更新后的位姿重新找对应关系再继续优化。当 Ceres 优化完成后q_last_curr和t_last_curr就表示当前帧相对于上一帧的最优估计运动。接下来代码会把这个相对运动累加到全局位姿中。更新公式对应代码中的t_w_curr t_w_curr q_w_curr * t_last_curr; q_w_curr q_w_curr * q_last_curr;这表示用上一帧在初始坐标系下的全局位姿乘上当前帧相对于上一帧的位姿增量得到当前帧在初始坐标系下的全局位姿。这里的/camera_init可以理解为初始世界坐标系/laser_odom是当前 LiDAR 里程计坐标系。因为这个全局位姿是通过一帧一帧的相对位姿连续累乘得到的所以它会随着时间产生累计漂移。这也是为什么 A-LOAM 不能只靠laserOdometry后面还必须有laserMapping做更低频但更精细的 scan-to-map 修正。位姿更新完成后程序会发布/laser_odom_to_init也就是当前帧相对于初始坐标系的激光里程计结果。同时还会把当前 pose 加入/laser_odom_path用于 RViz 显示轨迹。这个 odometry 是前端高频输出通常比 mapping 更快主要提供实时运动估计。除了发布里程计代码还会按一定频率把角点、面点和全分辨率点云发布给后端laserMapping。这里通过mapping_skip_frame控制是否降频发送给 mapping比如不是每帧都交给后端处理从而减轻后端建图和优化压力。每一帧处理结束后还有一个非常关键的点云交换步骤。代码会把当前帧的cornerPointsLessSharp和surfPointsLessFlat交换到laserCloudCornerLast和laserCloudSurfLast中使它们成为下一帧的上一帧参考点云。随后重新建立 KD-Tree。这样下一帧到来时当前帧的 sharp 角点就会去这份laserCloudCornerLast中寻找线约束当前帧的 flat 面点会去laserCloudSurfLast中寻找面约束。整个laserOdometry就是这样不断滚动运行的当前帧处理完后变成上一帧下一帧再来和它匹配。所以从完整流程上看laserOdometry.cpp可以概括为1. 接收 feature extraction 发布的五类点云 sharp角点、less sharp角点、flat面点、less flat面点、全分辨率点云。 2. 将五类点云分别放入缓存队列主循环等待数据齐全。 3. 检查五类点云的时间戳是否一致确保它们来自同一帧。 4. 第一帧只做系统初始化 保存 less sharp 和 less flat 作为上一帧参考点云 建立 corner 和 surf 的 KD-Tree 不进行相邻帧位姿优化。 5. 从第二帧开始进入 scan-to-scan 匹配 当前帧 sharp 角点用于角点约束 当前帧 flat 面点用于面点约束 上一帧 less sharp / less flat 用作匹配目标。 6. 对当前帧角点执行 TransformToStart 将其变换到上一帧坐标系 在上一帧角点 KD-Tree 中寻找最近点和相邻扫描线上的第二个点 构造点到线残差。 7. 对当前帧面点执行 TransformToStart 将其变换到上一帧坐标系 在上一帧面点 KD-Tree 中寻找三个合适点 构造点到面残差。 8. 将点到线残差和点到面残差加入 Ceres 优化当前帧相对于上一帧的旋转 q_last_curr 和平移 t_last_curr。 9. 优化过程中使用 HuberLoss 抑制误匹配影响 使用四元数参数化保证旋转合法 并通过多轮“重新找对应点 优化”提高匹配稳定性。 10. 优化得到相邻帧位姿增量后 将其累加到全局位姿 q_w_curr、t_w_curr 得到当前帧相对于初始坐标系的 laser odometry。 11. 发布 /laser_odom_to_init 和 /laser_odom_path 用于实时里程计输出和轨迹显示。 12. 根据 mapping_skip_frame 控制频率 将 corner、surf 和 full resolution 点云发布给 laserMapping。 13. 将当前帧 less sharp / less flat 交换成下一帧的 laserCloudCornerLast / laserCloudSurfLast 重建 KD-Tree为下一帧匹配做准备。 14. 重复以上流程持续输出高频 scan-to-scan 激光里程计。从算法本质上说这个文件实现的是一个基于特征的 LiDAR 前端 ICP。普通 ICP 往往是点到点、点到面地处理大量原始点而 A-LOAM 先通过特征提取把点云分成角点和平面点再分别构造点到线、点到面残差。角点约束利用边缘结构平面点约束利用平面结构这样既减少了参与优化的点数量又增强了几何约束的稳定性。优化出来的是相邻帧之间的运动增量因此速度快、实时性强但因为只依赖上一帧不使用长期地图约束所以不可避免会累计漂移。从工程实现上说这个文件把实时性放在第一位。它用队列缓存和时间戳检查保证多话题输入同步用 KD-Tree 加速最近邻查找用 scan line ID 限制匹配点的搜索范围用距离阈值过滤明显错误匹配用 HuberLoss 降低异常点影响用 Ceres 自动求导和非线性优化求解位姿增量用指针交换而不是深拷贝更新上一帧点云用mapping_skip_frame控制后端输入频率。这些设计共同保证了laserOdometry可以比较高频地运行。最终一句话概括laserOdometry.cpp的完整作用是接收前端特征提取后的角点和平面点将当前帧特征点与上一帧特征点进行 scan-to-scan 几何匹配通过点到线和点到面残差用 Ceres 优化相邻帧位姿增量再把这个增量连续累加成/laser_odom_to_init前端激光里程计同时把当前帧特征点更新为下一帧参考并按频率把特征点云传给laserMapping做后端 scan-to-map 精修。版权声明 辛苦码字不易转载请注明原文出处和作者信息谢谢理解欢迎分享与交流但拒绝任何形式的商业转载或洗稿。