0. A-LOAM 整体运行链路A-LOAM 本质是一个纯 3D LiDAR 特征点 SLAM。它不是直接拿整帧点云硬匹配而是先提取角点和平面点再用几何误差优化位姿。完整链路如下原始点云 ↓ scanRegistration.cpp 提取角点和平面点 ↓ lidarFactor.hpp 定义点到线、点到面误差 ↓ laserOdometry.cpp 当前帧 vs 上一帧估计快速里程计 ↓ laserMapping.cpp 当前帧 vs 局部地图优化地图位姿 ↓ 发布轨迹、TF、点云地图可以这样记scanRegistration负责“选点” lidarFactor负责“误差怎么算” laserOdometry负责“短时间怎么动” laserMapping负责“在地图里怎么对齐”1. 数据输入层kittiHelper.cpp1.1 这个文件干什么kittiHelper.cpp不是核心 SLAM 算法它主要用于跑 KITTI 数据集。因为 KITTI 的点云一般是.bin文件而 A-LOAM 正常处理的是 ROS 点云话题。所以它的作用是读取 KITTI .bin 文件 ↓ 解析 x、y、z、intensity ↓ 转成 PCL 点云 ↓ 转成 ROS PointCloud2 ↓ 发布成 /velodyne_points如果你用真实雷达跑 A-LOAM这个文件可以先不重点看因为真实雷达驱动会直接发布点云。1.2 关键代码逻辑std::ifstream input(filename.c_str(), std::ios::binary); pcl::PointCloudPointType::Ptr laserCloud(new pcl::PointCloudPointType); while (!input.eof()) { float x, y, z, intensity; input.read((char*)x, sizeof(float)); input.read((char*)y, sizeof(float)); input.read((char*)z, sizeof(float)); input.read((char*)intensity, sizeof(float)); PointType point; point.x x; point.y y; point.z z; point.intensity intensity; laserCloud-push_back(point); } sensor_msgs::PointCloud2 laserCloudMsg; pcl::toROSMsg(*laserCloud, laserCloudMsg); laserCloudMsg.header.frame_id camera_init; pubLaserCloud.publish(laserCloudMsg);这段代码很好理解。input.read()每次从.bin文件里读取一个float。KITTI 点云里一个点一般由四个 float 组成x, y, z, intensity其中x、y、z 点在雷达坐标系下的三维坐标。 intensity 反射强度。读取完之后把点放进 PCL 点云laserCloud-push_back(point);然后通过pcl::toROSMsg(*laserCloud, laserCloudMsg);把 PCL 点云转成 ROS 点云消息最后发布出去。这个文件只解决“数据怎么进系统”的问题真正前端从scanRegistration.cpp开始。2. 前端一scanRegistration.cpp2.1 这个文件干什么scanRegistration.cpp是 A-LOAM 前端的第一步负责点云预处理和特征提取。它输入原始点云输出四类特征点cornerPointsSharp 最尖锐角点数量少质量高。 cornerPointsLessSharp 次尖锐角点数量更多。 surfPointsFlat 最平坦平面点数量少质量高。 surfPointsLessFlat 次平面点数量更多通常会降采样。这个文件的核心逻辑是原始点云 ↓ 去无效点、去近点 ↓ 按垂直角度分线束 ↓ 计算点在一帧中的相对时间 ↓ 按扫描线重新排列点云 ↓ 计算曲率 ↓ 根据曲率选角点和平面点 ↓ 发布特征点云2.2 去掉太近的点先看一个基础预处理去除距离太近的点。void removeClosedPointCloud( const pcl::PointCloudPointType cloud_in, pcl::PointCloudPointType cloud_out, float min_range) { cloud_out.clear(); for (int i 0; i cloud_in.points.size(); i) { PointType p cloud_in.points[i]; float dis2 p.x * p.x p.y * p.y p.z * p.z; if (dis2 min_range * min_range) continue; cloud_out.push_back(p); } }这里计算的是点到雷达原点的距离平方dis² x² y² z²如果dis² min_range²说明点太近直接丢掉。为什么不写成dis sqrt(x² y² z²)因为开根号更耗时。比较距离大小时直接比较平方就够了。太近点通常可能来自车体 雷达外壳 安装支架 近距离噪声 异常反射点这些点不代表稳定环境结构保留下来会影响后面曲率计算。2.3 根据垂直角度计算 scanID多线雷达的点来自不同线束。A-LOAM 需要知道每个点属于第几根线因为后面计算曲率时要找同一根线上的前后邻居点。关键代码float verticalAngle atan(point.z / sqrt(point.x * point.x point.y * point.y)) * 180.0 / M_PI; int scanID int((verticalAngle 15) / 2 0.5); if (scanID 0 || scanID N_SCANS) continue;公式是verticalAngle atan(z / sqrt(x² y²))其中z 点的高度。 sqrt(x² y²) 点在水平面上的距离。 z / sqrt(x² y²) 点相对于水平面的倾斜程度。 atan 把倾斜比例转成角度。然后scanID int((verticalAngle 15) / 2 0.5)这一般针对 16 线雷达的角度分布来理解15 把负角度平移到正区间。 /2 相邻线束大约间隔 2 度。 0.5 四舍五入。 int 变成整数线号。这一步非常关键。因为如果scanID算错点就会被放到错误的扫描线里后面的前后邻居也会错曲率和特征点都会错。2.4 计算点在一帧中的相对时间机械式 LiDAR 一帧点云不是瞬间采完的而是旋转一圈扫出来的。也就是说一帧点云内部的点有不同采集时间。关键代码float startOri -atan2(firstPoint.y, firstPoint.x); float endOri -atan2(lastPoint.y, lastPoint.x) 2 * M_PI; float ori -atan2(point.y, point.x); float relTime (ori - startOri) / (endOri - startOri); point.intensity scanID scanPeriod * relTime;核心公式relTime (ori - startOri) / (endOri - startOri)解释startOri 这一帧第一个点的水平角。 endOri 这一帧最后一个点的水平角。 ori 当前点的水平角。 relTime 当前点在这一帧中的相对采集时间。如果relTime ≈ 0说明这个点接近当前帧开始时刻采集。如果relTime ≈ 1说明这个点接近当前帧结束时刻采集。最后这句很关键point.intensity scanID scanPeriod * relTime;这里intensity字段被复用了整数部分scanID表示第几根线 小数部分相对采集时间后面的里程计模块会利用这个相对时间做运动补偿。2.5 计算曲率曲率是 A-LOAM 提取特征的核心。它想判断一个点局部是“平滑”还是“突变”。关键代码for (int i 5; i cloudSize - 5; i) { float diffX cloud[i - 5].x cloud[i - 4].x cloud[i - 3].x cloud[i - 2].x cloud[i - 1].x - 10 * cloud[i].x cloud[i 1].x cloud[i 2].x cloud[i 3].x cloud[i 4].x cloud[i 5].x; float diffY cloud[i - 5].y cloud[i - 4].y cloud[i - 3].y cloud[i - 2].y cloud[i - 1].y - 10 * cloud[i].y cloud[i 1].y cloud[i 2].y cloud[i 3].y cloud[i 4].y cloud[i 5].y; float diffZ cloud[i - 5].z cloud[i - 4].z cloud[i - 3].z cloud[i - 2].z cloud[i - 1].z - 10 * cloud[i].z cloud[i 1].z cloud[i 2].z cloud[i 3].z cloud[i 4].z cloud[i 5].z; cloudCurvature[i] diffX * diffX diffY * diffY diffZ * diffZ; }对应公式c_i || p_{i-5} p_{i-4} p_{i-3} p_{i-2} p_{i-1} - 10 * p_i p_{i1} p_{i2} p_{i3} p_{i4} p_{i5} ||²这里p_i 当前点。 p_{i-5} 到 p_{i-1} 当前点前面的 5 个邻居。 p_{i1} 到 p_{i5} 当前点后面的 5 个邻居。 c_i 当前点曲率。为什么是-10 * p_i因为当前点前后总共取了 10 个邻居。如果当前点处在平面或平滑区域那么周围 10 个点的位置和大约等于10 * 当前点位置也就是p_{i-5} ... p_{i-1} p_{i1} ... p_{i5} ≈ 10 * p_i所以差值接近 0曲率小。如果当前点处在墙角、柱子边缘、箱子棱边那么它和周围点差异很大曲率就大。因此曲率大角点 / 边缘点 曲率小平面点2.6 选择角点和平面点A-LOAM 不会整帧统一选最大曲率点而是每根线分成多个区域每个区域单独选点。for (int scanID 0; scanID N_SCANS; scanID) { for (int j 0; j 6; j) { int sp scanStartInd[scanID] (scanEndInd[scanID] - scanStartInd[scanID]) * j / 6; int ep scanStartInd[scanID] (scanEndInd[scanID] - scanStartInd[scanID]) * (j 1) / 6 - 1; sort(cloudSortInd sp, cloudSortInd ep 1, byCurvature); } }这样做是为了让特征点分布更均匀。否则角点可能全堆在某一侧导致位姿约束不稳定。角点选择int largestPickedNum 0; for (int k ep; k sp; --k) { int ind cloudSortInd[k]; if (cloudNeighborPicked[ind] 0 cloudCurvature[ind] 0.1) { largestPickedNum; if (largestPickedNum 2) { cloudLabel[ind] 2; cornerPointsSharp.push_back(cloud[ind]); cornerPointsLessSharp.push_back(cloud[ind]); } else if (largestPickedNum 20) { cloudLabel[ind] 1; cornerPointsLessSharp.push_back(cloud[ind]); } else { break; } cloudNeighborPicked[ind] 1; } }这里cornerPointsSharp 每个区域最尖锐的少量角点用于精确匹配。 cornerPointsLessSharp 数量更多的角点用于后续里程计和建图参考。平面点选择int smallestPickedNum 0; for (int k sp; k ep; k) { int ind cloudSortInd[k]; if (cloudNeighborPicked[ind] 0 cloudCurvature[ind] 0.1) { cloudLabel[ind] -1; surfPointsFlat.push_back(cloud[ind]); smallestPickedNum; if (smallestPickedNum 4) break; cloudNeighborPicked[ind] 1; } }这里surfPointsFlat 最平坦的少量平面点。 surfPointsLessFlat 更多平面点一般会做体素滤波减少数量。3. 优化模型lidarFactor.hpp3.1 这个文件在系统中的位置lidarFactor.hpp不直接订阅点云也不发布话题。它是给优化器用的作用是定义点到线残差 点到面残差这个文件会被laserOdometry.cpp和laserMapping.cpp使用。所以它在逻辑上属于优化模型层它回答的问题是当前帧点经过位姿变换后和参考线/参考面的误差怎么算3.2 位姿变换公式A-LOAM 优化的是当前帧位姿可以写成T [R, t]其中R旋转 t平移当前帧里的点p变换到参考坐标系p R * p t代码里通常用四元数q表示旋转Eigen::QuaternionT quat(q[3], q[0], q[1], q[2]); Eigen::MatrixT, 3, 1 trans(t[0], t[1], t[2]); Eigen::MatrixT, 3, 1 p_trans quat * p trans;这里p 当前帧点。 quat * p 把点旋转到参考坐标系方向。 trans 加上平移。 p_trans 变换后的点。3.3 点到线残差角点用点到线残差。假设p当前帧角点 a、b参考帧或地图中的两个边缘点变换后p R * p t点到线距离d_edge || (p - a) × (p - b) || / || a - b ||代码Eigen::MatrixT, 3, 1 p_trans quat * p trans; Eigen::MatrixT, 3, 1 a( T(point_a.x), T(point_a.y), T(point_a.z)); Eigen::MatrixT, 3, 1 b( T(point_b.x), T(point_b.y), T(point_b.z)); Eigen::MatrixT, 3, 1 area_vec (p_trans - a).cross(p_trans - b); Eigen::MatrixT, 3, 1 line_vec a - b; residual[0] area_vec.norm() / line_vec.norm();公式解释(p - a) × (p - b) 叉乘模长等于平行四边形面积。 ||a - b|| 线段 ab 的长度。 面积 / 底边 就是点到直线的高也就是点到线距离。所以这个残差越小说明当前角点越贴近参考线。3.4 点到面残差平面点用点到面残差。假设p当前帧平面点 a、b、c参考帧或地图中的三个平面点平面法向量n (a - b) × (a - c)点到面距离d_plane | n · (p - a) | / ||n||代码Eigen::MatrixT, 3, 1 normal (a - b).cross(a - c); normal.normalize(); residual[0] normal.dot(p_trans - a);解释a - b 和 a - c 平面内的两个方向向量。 cross 两个平面内向量叉乘得到垂直于平面的法向量。 normal.normalize 把法向量单位化。 normal.dot(p_trans - a) 当前点到平面的有符号距离。如果残差接近 0说明当前平面点基本落在参考平面上。4. 前端二laserOdometry.cpp4.1 这个文件干什么laserOdometry.cpp是前端里程计模块。它接收scanRegistration.cpp输出的角点和平面点然后做当前帧特征点 vs 上一帧特征点也就是scan-to-scan 匹配。它的目标是快速估计当前帧相对于上一帧的位姿变化输出一个高频、快速的里程计结果。4.2 输入和保存的数据输入当前帧 cornerPointsSharp 当前帧 surfPointsFlat 当前帧 cornerPointsLessSharp 当前帧 surfPointsLessFlat内部会保存上一帧laserCloudCornerLast laserCloudSurfLast当前帧来了以后就和上一帧做匹配。4.3 当前点运动补偿因为一帧点云内部点的采集时间不同所以当前点要根据相对时间做补偿。void TransformToStart(PointType const *pi, PointType *po) { double s 1.0; if (DISTORTION) s (pi-intensity - int(pi-intensity)) / SCAN_PERIOD; 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 point_trans q_point_last * point t_point_last; po-x point_trans.x(); po-y point_trans.y(); po-z point_trans.z(); po-intensity pi-intensity; }解释s (pi-intensity - int(pi-intensity)) / SCAN_PERIOD;前面scanRegistration.cpp把点的相对时间放进了intensity小数部分。这里就是把相对时间取出来。slerp(s, q_last_curr)这是四元数球面插值。因为当前点可能是在一帧中间采集的所以旋转量也要按时间比例插值。t_point_last s * t_last_curr;平移也按时间比例缩放。point_trans q_point_last * point t_point_last;把点补偿到统一时刻附近减少运动畸变。4.4 角点匹配当前角点找上一帧的线for (int i 0; i cornerPointsSharpNum; i) { PointType pointSel; TransformToStart(cornerPointsSharp-points[i], pointSel); kdtreeCornerLast-nearestKSearch( pointSel, 1, pointSearchInd, pointSearchSqDis); int closestPointInd pointSearchInd[0]; int minPointInd2 findAnotherCornerPointInDifferentScanLine(); if (minPointInd2 0) { ceres::CostFunction *cost_function LidarEdgeFactor::Create( cornerPointsSharp-points[i], laserCloudCornerLast-points[closestPointInd], laserCloudCornerLast-points[minPointInd2], s); problem.AddResidualBlock( cost_function, loss_function, para_q, para_t); } }逻辑是1. 当前角点先做运动补偿。 2. 在上一帧角点云中找最近点。 3. 再找另一个合适角点。 4. 两个参考角点组成一条线。 5. 当前角点建立点到线残差。几何意义当前帧的边缘点应该落在上一帧的某条边缘线附近。4.5 平面点匹配当前平面点找上一帧的面for (int i 0; i surfPointsFlatNum; i) { PointType pointSel; TransformToStart(surfPointsFlat-points[i], pointSel); kdtreeSurfLast-nearestKSearch( pointSel, 1, pointSearchInd, pointSearchSqDis); int closestPointInd pointSearchInd[0]; int minPointInd2 findSecondSurfPoint(); int minPointInd3 findThirdSurfPoint(); if (minPointInd2 0 minPointInd3 0) { ceres::CostFunction *cost_function LidarPlaneFactor::Create( surfPointsFlat-points[i], laserCloudSurfLast-points[closestPointInd], laserCloudSurfLast-points[minPointInd2], laserCloudSurfLast-points[minPointInd3], s); problem.AddResidualBlock( cost_function, loss_function, para_q, para_t); } }逻辑是1. 当前平面点先做运动补偿。 2. 在上一帧平面点云中找最近点。 3. 再找两个合适平面点。 4. 三个参考平面点构成一个平面。 5. 当前平面点建立点到面残差。几何意义当前帧的平面点应该落在上一帧的某个平面附近。4.6 前端里程计优化目标laserOdometry.cpp优化的是当前帧相对于上一帧的位姿T* argmin_T { Σ || e_edge(T) ||² Σ || e_plane(T) ||² }解释T 当前帧相对上一帧的位姿。 e_edge 角点到线误差。 e_plane 平面点到面误差。 Σ 所有特征点误差求和。Ceres 会优化para_q旋转四元数 para_t平移向量目标是让所有角点尽量贴近线所有平面点尽量贴近面。这个结果速度快适合实时输出但它只参考上一帧所以会累计误差。5. 后端laserMapping.cpp5.1 这个文件干什么laserMapping.cpp是后端建图优化模块。它做的是当前帧特征点 vs 局部地图特征点也就是scan-to-map 匹配。它和laserOdometry.cpp的区别是laserOdometry.cpp 当前帧 vs 上一帧速度快。 laserMapping.cpp 当前帧 vs 局部地图更稳定。局部地图包含多帧历史点云所以约束更丰富优化结果更稳定。5.2 当前点变换到地图坐标系当前帧点需要先根据初始位姿变换到地图坐标系附近。void pointAssociateToMap(PointType const *pi, PointType *po) { Eigen::Vector3d point(pi-x, pi-y, pi-z); Eigen::Vector3d point_w q_w_curr * point t_w_curr; po-x point_w.x(); po-y point_w.y(); po-z point_w.z(); po-intensity pi-intensity; }公式p_map R_w_curr * p_curr t_w_curr解释p_curr 当前帧坐标系下的点。 R_w_curr 当前帧到地图坐标系的旋转。 t_w_curr 当前帧到地图坐标系的平移。 p_map 变换到地图坐标系后的点。5.3 局部地图维护A-LOAM 不会每次都拿全局地图匹配因为地图越来越大计算量会越来越高。所以它只取当前位置附近的一片局部地图laserCloudCornerFromMap.clear(); laserCloudSurfFromMap.clear(); for (int i nearbyCubeStart; i nearbyCubeEnd; i) { *laserCloudCornerFromMap *laserCloudCornerArray[i]; *laserCloudSurfFromMap *laserCloudSurfArray[i]; } kdtreeCornerFromMap-setInputCloud(laserCloudCornerFromMap); kdtreeSurfFromMap-setInputCloud(laserCloudSurfFromMap);这里laserCloudCornerFromMap 局部地图角点集合。 laserCloudSurfFromMap 局部地图平面点集合。 KD-Tree 用于快速找最近邻点。局部地图的目的减少计算量 提高实时性 只匹配当前位置附近的有效结构5.4 后端角点匹配PCA 找线在后端当前角点不是简单找两个点而是在局部地图角点里找 5 个近邻点然后判断这 5 个点是否形成线结构。kdtreeCornerFromMap-nearestKSearch( pointSel, 5, pointSearchInd, pointSearchSqDis); if (pointSearchSqDis[4] 1.0) { Eigen::Vector3d center Eigen::Vector3d::Zero(); for (int j 0; j 5; j) { PointType p laserCloudCornerFromMap-points[pointSearchInd[j]]; center Eigen::Vector3d(p.x, p.y, p.z); } center / 5.0; Eigen::Matrix3d covMat Eigen::Matrix3d::Zero(); for (int j 0; j 5; j) { PointType p laserCloudCornerFromMap-points[pointSearchInd[j]]; Eigen::Vector3d tmp(p.x - center.x(), p.y - center.y(), p.z - center.z()); covMat tmp * tmp.transpose(); } Eigen::SelfAdjointEigenSolverEigen::Matrix3d saes(covMat); Eigen::Vector3d eigenvalues saes.eigenvalues(); Eigen::Matrix3d eigenvectors saes.eigenvectors(); if (eigenvalues[2] 3 * eigenvalues[1]) { Eigen::Vector3d unit_direction eigenvectors.col(2); Eigen::Vector3d point_on_line_a center 0.1 * unit_direction; Eigen::Vector3d point_on_line_b center - 0.1 * unit_direction; addEdgeResidual(pointSel, point_on_line_a, point_on_line_b); } }这里重点是 PCA。协方差矩阵表示这 5 个点在三维空间中的分布情况。特征值可以理解成λ0、λ1、λ2 点云沿三个主方向的分散程度。如果λ2 λ1说明这几个点主要沿一个方向分布像一条线。代码中if (eigenvalues[2] 3 * eigenvalues[1])表示最大方向明显强于第二大方向可以认为是线特征。然后unit_direction eigenvectors.col(2);取最大特征值对应的特征向量作为线方向。再构造线上的两个点point_on_line_a center 0.1 * unit_direction; point_on_line_b center - 0.1 * unit_direction;这两个点就代表局部地图中的一条边缘线。5.5 后端平面点匹配最小二乘拟合平面当前平面点会在局部地图平面点中找 5 个近邻点然后拟合平面。kdtreeSurfFromMap-nearestKSearch( pointSel, 5, pointSearchInd, pointSearchSqDis); if (pointSearchSqDis[4] 1.0) { Eigen::Matrixdouble, 5, 3 A; Eigen::Matrixdouble, 5, 1 b; for (int j 0; j 5; j) { PointType p laserCloudSurfFromMap-points[pointSearchInd[j]]; A(j, 0) p.x; A(j, 1) p.y; A(j, 2) p.z; b(j, 0) -1.0; } Eigen::Vector3d norm A.colPivHouseholderQr().solve(b); double negative_OA_dot_norm 1.0 / norm.norm(); norm.normalize(); bool planeValid true; for (int j 0; j 5; j) { PointType p laserCloudSurfFromMap-points[pointSearchInd[j]]; double distance fabs(norm.x() * p.x norm.y() * p.y norm.z() * p.z negative_OA_dot_norm); if (distance 0.2) { planeValid false; break; } } if (planeValid) { addPlaneResidual(pointSel, norm, negative_OA_dot_norm); } }平面方程是a*x b*y c*z d 0代码把它变形成a*x b*y c*z -1于是 5 个点可以构成x1*a y1*b z1*c -1 x2*a y2*b z2*c -1 x3*a y3*b z3*c -1 x4*a y4*b z4*c -1 x5*a y5*b z5*c -1矩阵形式A * n b其中n [a, b, c]^T因为 5 个方程、3 个未知数所以用最小二乘求解norm A.colPivHouseholderQr().solve(b);然后检查这 5 个点是否真的接近平面distance fabs(a*x b*y c*z d);如果某些点离平面太远说明它们不是稳定平面结构这个约束就丢掉。5.6 后端优化目标后端优化的是当前帧在地图坐标系下的位姿T_map* argmin_T { Σ || e_edge_map(T) ||² Σ || e_plane_map(T) ||² }这里e_edge_map 当前角点到局部地图线特征的误差。 e_plane_map 当前平面点到局部地图平面特征的误差。它比前端里程计更稳定因为它参考的是局部地图而不是单独上一帧。5.7 当前帧加入地图并降采样优化完成后当前帧特征点会加入地图。for (int i 0; i laserCloudCornerStackNum; i) { pointAssociateToMap(laserCloudCornerStack-points[i], pointSel); laserCloudCornerArray[cubeInd]-push_back(pointSel); } for (int i 0; i laserCloudSurfStackNum; i) { pointAssociateToMap(laserCloudSurfStack-points[i], pointSel); laserCloudSurfArray[cubeInd]-push_back(pointSel); }加入地图后还要体素滤波downSizeFilterCorner.setInputCloud(laserCloudCornerArray[i]); downSizeFilterCorner.filter(*laserCloudCornerArray[i]); downSizeFilterSurf.setInputCloud(laserCloudSurfArray[i]); downSizeFilterSurf.filter(*laserCloudSurfArray[i]);体素滤波的作用减少地图点数量 防止地图无限增长 加快 KD-Tree 搜索 保持主要几何结构6. 按“前端 / 优化 / 后端”重新总结代码顺序你可以这样记 A-LOAM 文件顺序第一层数据输入 kittiHelper.cpp 负责把数据集点云变成 ROS 点云话题。 真实雷达场景下这个不是算法重点。 第二层前端特征提取 scanRegistration.cpp 负责点云预处理、线束划分、相对时间计算、曲率计算、角点和平面点提取。 第三层优化残差模型 lidarFactor.hpp 负责定义点到线、点到面的 Ceres 残差。 这个文件被前端里程计和后端建图共同使用。 第四层前端里程计 laserOdometry.cpp 负责当前帧和上一帧匹配。 角点找线平面点找面优化帧间位姿。 第五层后端建图优化 laserMapping.cpp 负责当前帧和局部地图匹配。 局部地图中 PCA 找线、最小二乘拟合平面优化地图位姿并更新地图。7. 完整流程详细总结A-LOAM 运行时首先需要点云输入。如果是 KITTI 数据集kittiHelper.cpp会把.bin文件中的x、y、z、intensity读取出来转成 ROS 点云话题。如果是真实雷达则由雷达驱动直接发布原始点云。原始点云进入scanRegistration.cpp后系统先去掉无效点和距离太近的点。然后根据每个点的垂直角度计算scanID判断它属于第几根激光线。接着根据水平角度计算点在当前帧中的相对采集时间并把scanID和相对时间编码到intensity字段中。然后scanRegistration.cpp按照扫描线重新排列点云并对每个点计算曲率。曲率大的点代表局部变化剧烈通常是墙角、柱子边缘、箱子棱边所以选为角点曲率小的点代表局部平滑通常是地面、墙面、桌面所以选为平面点。为了让特征分布均匀它会按线束、按区域分别选点而不是整帧统一排序。特征点提取完成后进入laserOdometry.cpp。这个文件做前端帧间里程计也就是当前帧和上一帧匹配。当前角点会在上一帧角点中找两个点构成边缘线建立点到线误差当前平面点会在上一帧平面点中找三个点构成平面建立点到面误差。这里实际使用的残差计算来自lidarFactor.hpp。lidarFactor.hpp是优化模型核心。它定义了两个最重要的几何误差。第一个是点到线误差d_edge || (p - a) × (p - b) || / || a - b ||第二个是点到面误差d_plane | n · (p - a) | / ||n||其中p R * p t表示把当前帧点通过当前估计位姿变换到参考坐标系。优化器调整的就是R和t让所有点到线、点到面的误差尽量小。laserOdometry.cpp优化完成后会得到一个快速里程计结果。这个结果频率高、实时性好但它只参考上一帧所以长期运行会有累计误差。接着进入laserMapping.cpp。这个文件做后端局部地图优化。它先把当前帧特征点根据前端里程计初值变换到地图坐标系附近然后从已有地图中取出当前位置附近的局部地图。当前角点会在局部地图角点中找附近点并通过 PCA 判断这些点是否形成线结构当前平面点会在局部地图平面点中找附近点并用最小二乘拟合平面。如果线和平面都有效laserMapping.cpp会再次构建点到线、点到面的残差并用 Ceres 优化当前帧在地图坐标系下的位姿。由于这次参考的是局部地图而不是单独上一帧所以结果更稳定。最后优化后的当前帧特征点会被加入地图。为了防止地图点云越来越大系统会对地图中的角点和平面点做体素滤波。最终发布优化后的位姿、TF、轨迹和点云地图。最终主线可以压缩成数据输入 ↓ scanRegistration.cpp提取角点和平面点 ↓ lidarFactor.hpp定义点到线、点到面误差 ↓ laserOdometry.cpp当前帧和上一帧匹配得到快速里程计 ↓ laserMapping.cpp当前帧和局部地图匹配得到稳定地图位姿 ↓ 更新地图发布轨迹和点云地图一句话记忆A-LOAM 前端特征提取 优化残差建模 帧间里程计 后端局部地图优化。版权声明 辛苦码字不易转载请注明原文出处和作者信息谢谢理解欢迎分享与交流但拒绝任何形式的商业转载或洗稿。