A-LOAM完整详细解读

chase_my_dream

554人浏览 · 2026-06-09 18:48:36

chase_my_dream · 2026-06-09 18:48:36 发布

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，表示第几根线
小数部分：相对采集时间

后面的里程计模块会利用这个相对时间做运动补偿。A-LOAM 里基本就是用上一帧和当前帧之间估计出来的相对位姿 q_last_curr、t_last_curr，按点的相对时间 s 做插值，近似当作当前帧内部的运动补偿量。

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;
    }
}

这段代码是在 当前扫描线的某个分段 [sp, ep] 中选择角点特征：由于 cloudSortInd 已经按曲率从小到大排好序，所以这里从 ep 往 sp 反向遍历，相当于优先检查曲率最大的点。largestPickedNum 表示当前分段已经选出的角点数量；ind = cloudSortInd[k] 表示排序后第 k 个点在原始点云 cloud 里的真实索引；cloudNeighborPicked[ind] == 0 表示该点没有被选过，也没有被邻域抑制；cloudCurvature[ind] > 0.1 表示该点曲率足够大，局部几何变化明显，适合作为边缘/角点。如果满足条件，就先让 largestPickedNum++。当 largestPickedNum <= 2 时，说明这是当前分段里最尖锐的前 2 个点，会被标记为强角点：cloudLabel[ind] = 2，并同时加入 cornerPointsSharp 和 cornerPointsLessSharp；当 largestPickedNum <= 20 时，说明它不是最尖锐的前 2 个，但仍属于曲率较大的前 20 个点，会被标记为次角点：cloudLabel[ind] = 1，只加入 cornerPointsLessSharp；如果超过 20 个，就 break，停止当前分段的角点选择。最后 cloudNeighborPicked[ind] = 1 表示这个点已经被选中，后续不能重复选。整体逻辑就是：每个分段内按曲率从大到小选角点，最多选 2 个 sharp 角点、20 个 less sharp 角点，用于后续 LiDAR 匹配中的边缘特征约束。

这里：

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;
    }
}

这段代码是在 LOAM / A-LOAM 特征提取阶段，从当前扫描线某个分段 [sp, ep] 中挑选平面点 surfPointsFlat。前面代码通常已经把当前分段内的点按照曲率 cloudCurvature 从小到大排序，并把排序后的原始点索引存到 cloudSortInd 中，所以这里 for (int k = sp; k <= ep; ++k) 是从曲率最小的点开始往后遍历。ind = cloudSortInd[k] 表示当前候选点在原始点云 cloud 中的真实索引；cloudNeighborPicked[ind] == 0 表示该点没有被选中过，也没有被邻域抑制；cloudCurvature[ind] < 0.1 表示该点曲率足够小，局部表面比较平滑，适合作为平面特征点。满足条件后，代码把 cloudLabel[ind] = -1，表示将该点标记为平面点，然后通过 surfPointsFlat.push_back(cloud[ind]) 把它加入强平面点集合。smallestPickedNum 用来统计当前分段已经选了多少个平面点，每选中一个就加 1；当数量达到 4 个时执行 break，说明当前分段最多只选 4 个最平坦的点，避免平面点过多集中在某一局部区域。最后 cloudNeighborPicked[ind] = 1 表示该点已经被使用，后续不再重复选择。整体逻辑就是：在每个分段里优先选择曲率最小、未被选过的点作为平面特征点，每段最多选 4 个，用于后续 LiDAR 匹配中的点到平面约束。

这里：

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 = 前端特征提取 + 优化残差建模 + 帧间里程计 + 后端局部地图优化。

欢迎分享与交流，但拒绝任何形式的商业转载或洗稿。

AtomGit开源社区

AtomGit 是由开放原子开源基金会联合 CSDN 等生态伙伴共同推出的新一代开源与人工智能协作平台。平台坚持“开放、中立、公益”的理念，把代码托管、模型共享、数据集托管、智能体开发体验和算力服务整合在一起，为开发者提供从开发、训练到部署的一站式体验。

更多推荐

Grok实时热点分析实战：用AI抓取行业动态，辅助技术决策

AtomGit开源社区

【顶刊复现】配电网两阶段鲁棒故障恢复研究（Matlab代码实现)

面向高比例分布式电源深度渗透背景下主动配电网故障恢复的强不确定性、弱可观性、高风险性难题，本文提出一种保守性可调、求解高效、工程适用的两阶段鲁棒故障恢复优化方法。该方法以区间型多面体不确定集统一刻画分布式电源出力间歇性、负荷需求时变性以及负荷估计误差带来的多重不确定性，完全摆脱对概率分布、模糊隶属函数等难以获取的先验信息依赖。模型采用两阶段嵌套决策架构：第一阶段以最大化停电区域负荷恢复总量为目标，