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.cpp
1.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::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>); 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.cpp
2.1 这个文件干什么？
scanRegistration.cpp是 A-LOAM 前端的第一步，负责点云预处理和特征提取。
它输入原始点云，输出四类特征点：
cornerPointsSharp： 最尖锐角点，数量少，质量高。 cornerPointsLessSharp： 次尖锐角点，数量更多。 surfPointsFlat： 最平坦平面点，数量少，质量高。 surfPointsLessFlat： 次平面点，数量更多，通常会降采样。这个文件的核心逻辑是：
原始点云 ↓ 去无效点、去近点 ↓ 按垂直角度分线束 ↓ 计算点在一帧中的相对时间 ↓ 按扫描线重新排列点云 ↓ 计算曲率 ↓ 根据曲率选角点和平面点 ↓ 发布特征点云2.2 去掉太近的点
先看一个基础预处理：去除距离太近的点。
void removeClosedPointCloud( const pcl::PointCloud<PointType>& cloud_in, pcl::PointCloud<PointType>& 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_{i+1} + p_{i+2} + p_{i+3} + p_{i+4} + p_{i+5} ||²这里：
p_i： 当前点。 p_{i-5} 到 p_{i-1}： 当前点前面的 5 个邻居。 p_{i+1} 到 p_{i+5}： 当前点后面的 5 个邻居。 c_i： 当前点曲率。为什么是-10 * p_i？
因为当前点前后总共取了 10 个邻居。如果当前点处在平面或平滑区域，那么周围 10 个点的位置和大约等于：
10 * 当前点位置也就是：
p_{i-5} + ... + p_{i-1} + p_{i+1} + ... + p_{i+5} ≈ 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.hpp
3.1 这个文件在系统中的位置
lidarFactor.hpp不直接订阅点云，也不发布话题。它是给优化器用的，作用是定义：
点到线残差 点到面残差这个文件会被laserOdometry.cpp和laserMapping.cpp使用。
所以它在逻辑上属于：
优化模型层它回答的问题是：
当前帧点经过位姿变换后，和参考线/参考面的误差怎么算？3.2 位姿变换公式
A-LOAM 优化的是当前帧位姿，可以写成：
T = [R, t]其中：
R：旋转 t：平移当前帧里的点p变换到参考坐标系：
p' = R * p + t代码里通常用四元数q表示旋转：
Eigen::Quaternion<T> quat(q[3], q[0], q[1], q[2]); Eigen::Matrix<T, 3, 1> trans(t[0], t[1], t[2]); Eigen::Matrix<T, 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::Matrix<T, 3, 1> p_trans = quat * p + trans; Eigen::Matrix<T, 3, 1> a( T(point_a.x), T(point_a.y), T(point_a.z)); Eigen::Matrix<T, 3, 1> b( T(point_b.x), T(point_b.y), T(point_b.z)); Eigen::Matrix<T, 3, 1> area_vec = (p_trans - a).cross(p_trans - b); Eigen::Matrix<T, 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::Matrix<T, 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.cpp
4.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.cpp
5.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::SelfAdjointEigenSolver<Eigen::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::Matrix<double, 5, 3> A; Eigen::Matrix<double, 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 = 前端特征提取 + 优化残差建模 + 帧间里程计 + 后端局部地图优化。版权声明： 辛苦码字不易，转载请注明原文出处和作者信息，谢谢理解！
欢迎分享与交流，但拒绝任何形式的商业转载或洗稿。
