激光SLAM--(7)A-LOAM代码逻辑梳理

本篇文章只梳理了自己角度的代码逻辑，具体代码和逻辑可以结合上一篇博客查看，至于代码具体的注释版本可以在github上自行查找（有很多）。

项目工程四个文件分别是scanRegistration.cpp、laserOdometry.cpp、laserMapping.cpp、和lidarFactor，分别对应于激光数据处理、低精度里程计和高精度建图，还有就是ceres实现的优化。

---

scanRegistration.cpp

接受lidar数据，进行数据处理和特征提取。

数据预处理

1. systemDelay负责数据的缓冲初始化，
2. pcl::removeNaNFromPointCloud对点云滤波，去除NaN的值（射向天空或很远的点）；
   removeClosedPointCloud去除靠的太近的点（这种基本就是在激光载体上），这个函数里面的resize操作在复用同一点云输出的时候都是会用的。
3. 计算起始点和结束点的水平角度（都是与x轴的夹角），用在后面的水平角确定时间上。
4. 计算俯仰角来确定处于第几个线束；计算某点水平角度处于哪个范围从而得到其时间戳；（这两个计算在如今很多激光雷达中都不需要了，因为点能自带线束和时间戳了）

提取特征

1. 计算每个点曲率：前后五个不参与，曲率计算（当前点的前5个加上后5个三坐标值减去当前点的10倍，再得到三坐标的平方和）
2. 对每个can进行6等分操作，每个等分对点云按照曲率进行排序，从而得到特征点，最后对该scan点云中提取的所有surf_less_flat特征点进行降采样（体素滤波），因为点太多了。
3. 发布四种特征点

异常点剔除（对应于论文里的不可靠点，上一篇博客里也有）

A-LOAM里面没有实现，在LIO-SAM里

void markOccludedPoints()
    {
        int cloudSize = extractedCloud->points.size();
        // 标记遮挡点和平行光束点
        for (int i = 5; i < cloudSize - 6; ++i)
        {
            // 获取相邻两个点距离信息
            float depth1 = cloudInfo.pointRange[i];
            float depth2 = cloudInfo.pointRange[i + 1];
            // 获取相邻两个有效点之间的列索引差
            int columnDiff = std::abs(int(cloudInfo.pointColInd[i + 1] - cloudInfo.pointColInd[i]));

            // 如果列索引差较小，即两点在扫描角度上靠的很近，这样才有意义
            if (columnDiff < 10)
            {
                // 如果深度值差的较大，则将相邻的前后5个点认为是遮挡点，并标记为已选择过，后面不会再对这些点进行特征提取
                if (depth1 - depth2 > 0.3)
                {
                    cloudNeighborPicked[i - 5] = 1;
                    cloudNeighborPicked[i - 4] = 1;
                    cloudNeighborPicked[i - 3] = 1;
                    cloudNeighborPicked[i - 2] = 1;
                    cloudNeighborPicked[i - 1] = 1;
                    cloudNeighborPicked[i] = 1;
                }
                else if (depth2 - depth1 > 0.3)
                {
                    cloudNeighborPicked[i + 1] = 1;
                    cloudNeighborPicked[i + 2] = 1;
                    cloudNeighborPicked[i + 3] = 1;
                    cloudNeighborPicked[i + 4] = 1;
                    cloudNeighborPicked[i + 5] = 1;
                    cloudNeighborPicked[i + 6] = 1;
                }
            }
            // 获取当前点与左右相邻点之间的深度差
            float diff1 = std::abs(float(cloudInfo.pointRange[i - 1] - cloudInfo.pointRange[i]));
            float diff2 = std::abs(float(cloudInfo.pointRange[i + 1] - cloudInfo.pointRange[i]));
            // 如果相邻深度差都较大，则认为当前点为平行光束点(即激光近似水平射向反射平面)，并标记为已选择过，后面不会对这些点进行特征提取
            if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])
                cloudNeighborPicked[i] = 1;
        }
    }

---

laserOdometry.cpp

这个文件主要就是实现特征的匹配，然后调用ceres库实现转换位姿的求解运算。

• TransformToStart：点云运动补偿到此帧开始时刻
• TransformToEnd：补偿到结束时刻

每次迭代具体流程：

1. 初始化后，取出比较突出的特征点。
2. 对每个coner或者flat点找匹配点，进行距离的运算。（这里关于匹配点等选择参考上一帧博客）
3. ceres库求解得到转换位姿，然后更新到世界坐标系位姿上。
4. 非降频向后端发送位姿

---

laserMapping.cpp

1.基于栅格的局部地图构造

• 概念介绍：不同于前端的scan to scan过程，后端是scan to map，具体来说就是把当前帧和地图进行匹配得到更准的位姿同时也可以构建更好的地图。
• 地图的构成：将整个地图分成21×21×11个珊格，每个珊格是⼀个边⻓50m的正⽅体，当地图逐渐累加时，珊格之外的部分就被舍弃，这样可以保证内存空间不会随着程序的运⾏⽽爆炸。
• 地图调整：当当前位姿要靠近地图边界时进行移动处理，使得地图安全。

实现过程

1. 以Corner为主体得到同时刻的各类数据，并且能保证实时性；利用原本地图中的上一帧位姿加上前端送过来的位姿转换，为mapping提供一个初始当前帧位姿。
2. 以上一步得到的初始位姿来寻找当前帧在栅格地图中的栅格索引。
3. 当当前帧靠近栅格地图边界的时候，对其进行调整，要保证当前帧所处栅格索引满足（3 < centerCubeI < 18， 3 < centerCubeJ < 8, 3 < centerCubeK < 18）。这里起始已经维护了一个局部地图，只是比较大。
4. 接下来构造scan to map中真正使用到的局部地图，向IJ坐标轴的正负方向各拓展2个cube，K坐标轴的正负方向各拓展1个cube，并且把这些cube的index记录下来，把这几个的点云叠加到一起构建submap。
5. 为了减少运算量，这里也会对当前帧的coner和surf点云各有一次体素滤波下采样。

2.地图中线面特征的提取

在前端，是通过当前帧的线面特征分别和上一帧的线和面特征进行匹配，构建约束从而进行优化求解。由于机械式激光雷达的性质，这里面存在线束的约束。
而在后端的当前帧与地图匹配的时候，需要从地图中寻找线特征和面特征的约束时，就没有了线束信息，所以需要额外的操作来判断其是否符合线和面特征的给定约束。

具体流程：

1. 首先还是把上面得到的submap中的点云放入kd-tree，便于最近邻搜索
2. 在进行匹配算最近邻距离的时候，要先把当前帧lidar坐标系下的点云通过前面得到的初始位姿转换到world坐标系下才能跟submap中的点云匹配。
3. 在submap的corner特征点（target）中，寻找距离当前帧corner特征点（source）最近的5个点，计算这五个点中心（并且也是方向向量），由此得到协方差矩阵并进行PCA分解，
4. 如果满足线特征要求（即最大特征值大于三倍次特征值），则从中心点沿着方向向量向两端移动0.1m，构造线上的两个点，由此两点确定一条线，算点到直线距离。

而对于面特征来说，原则上也可以通过PCA分解方式得到特征，但代码里面采用的是平面拟合。先由找到的最近五个面点拟合出一个平面，然后看这五个点到这个平面的距离远不远，如果超过一定距离的话，就不行。最后算点到面距离构建优化。
但这里需要注意的就是，线特征优化中使用的因子优化和前端一致，但面特征则已经不是采用的向量方法了，而是直接使用点到面的距离公式进行计算:

$点(x0,y0,z0)到平面Ax+By+Cz+D=0的距离=fabs(A\ast x_0+B\ast y_0+C\ast z_0+D)/sqrt(A^2+B^2+C^2)，因为法向量（A,B,C）已经归一化了，所以距离公式可以简写为：距离=fabs(A\ast x_0+B\ast y_0+C\ast z_0+D)，即：$

	residual[0] = norm.dot(point_w) + T(negative_OA_dot_norm);

---

3.地图位姿更新

void transformUpdate()
{
	q_wmap_wodom = q_w_curr * q_wodom_curr.inverse();
	t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;
}

把map坐标系和odom坐标系之间的T更新后，并且把本帧位姿叠加保存，形成轨迹。

4.地图更新

把当前帧对应角点点云和面点点云全部投到对应的cube里面，然后对所有的cube进行一次下采样（如果没有新增地图点就不用）
最后可以按一定的频率对外发布