沉默杀手HomeArchivesTagsCategories
ALOAM的流程
|LOAM系列|
Word count: 1.6k|Reading time: 5 min

LOAM是一个激光里程计算法，没有闭环检测，也就没有加入图优化框架，该算法把SLAM问题分为两个算法并行运行：一个odometry算法，10Hz；另一个mapping算法，1Hz，最终将两者输出的pose做整合，实现10Hz的位姿实时输出。

LOAM源码主要由四个节点构成，分别完成特征点提取，高频低精度odom， 低频高精度odom， 双频odom融合的功能。实际上， 四个节点的执行顺序完全是串行的，很容易改成单进程的版本。

scanRegistration 流程

  1. 原始点云去除无效点和很近的点，成为无序点云

  2. 通过Lidar坐标系下点的仰角以及水平夹角计算点云的scanID和fireID(点在每个scan的ID为fireID)

  3. 以上是每根线作为独立的子点云，将子点云合并为一帧有序点云

  4. 根据曲率计算4种特征点，角点曲率大，面点曲率小。计算曲率，除了开始5个点和结束的5个点，以当前点为中心，前5个和后5个点都参与计算。然后针对每条scan，提取特征点：

    曲率最大的前2个点认为是sharp点，最大的前20个点认为是less_sharp点
    曲率最小的前4个点认为是flat点，将除了角点之外的所有点(包括flat)都加入 less_flat

  5. 发布4种特征点与滤波后的当前帧点云

为什么选择线特征和面特征？

  1. 在三维空间中或人造的建筑物中，常见的特征是线、面
  2. 由于LOAM没有提取特征点，直接使用传统icp的点到点约束，会由于数据关联错误造成较大的估计误差

laserOdometry 流程

1
2
3
4
5
6
7
8
// 后面可以看到，这是点云特征匹配时的优化变量
double para_q[4] = {0, 0, 0, 1};
double para_t[3] = {0, 0, 0};

// 下面的2个分别是优化变量 para_q 和 para_t 的映射：表示的是
// world坐标系下的两个位姿之间的增量，例如 △P = P0.inverse() * P1
Eigen::Map<Eigen::Quaterniond>  q_last_curr(para_q);
Eigen::Map<Eigen::Vector3d>  t_last_curr(para_t);
  1. 订阅4种特征点的消息，各自加入队列，每次取一帧进行匹配
  2. 用当前帧的角点和面点，与上一帧的角点和面点建立约束。点到线以及点到面的ICP，迭代2次

2.1 sharp点： 将当前帧的sharp点 , 从当前帧的lidar坐标系转换到上一帧的Lidar坐标系，转换后的点云记做 , 同时将所有点补偿到起始时刻。
每个点的转换公式：

使用KD树，寻找上一帧点云 和 中的每个点 最相近的那个，记做 ，最小距离记做min_square_dis。这是找到了第1个点，还要找第2个。

第2个点和第1个不在同一条scan，在上下的线上寻找。找到比min_square_dis更小，而且是最小的距离min_square_dis2，对应点的索引为 。必须有两个点，否则不加入ceres优化。

1
2
3
4
// 残差 = 点O到直线AB的距离
ceres::CostFunction *cost_function = LidarEdgeFactor::Create( $$ p_{cur} $$, $$ P_{closest_\ i} $$, $$ P_{closest_\ j} $$, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
// 统计约束的数量

2.2 flat点： 找平面点O最近的3个点A B C，第1个A点也是用KD树找到，一般情况下，B和A在同一条scan，C在不同的scan。必须有三个点，否则不加入ceres优化。

1
2
3
4
// 残差 = 点O到平面ABC的距离
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);
// 统计约束的数量

两种约束的数量不要太少

  1. 对于sharp点，将当前帧Lidar坐标系下的点变换到上一帧的Lidar坐标系下，然后在上一帧的Lidar坐标系计算点到线(找到的原上一帧点云的2个点)的残差。 对于flat点，将当前帧Lidar坐标系下的点变换到上一帧的Lidar坐标系下，然后在上一帧的Lidar坐标系计算点到平面(找到的原上一帧点云的3个点组成的平面)的残差。

  2. 经过2次点到线以及点到面的ICP点云配准之后，得到最优的位姿增量 para_q 和 para_t，更新上一帧的位姿，得到当前帧的位姿，这里说的位姿都指世界坐标系下的位姿，世界坐标系本质就是第1帧的lidar坐标系。

  1. 发布当前帧在世界坐标系在的位姿，也就是 和 , corner_less_sharp特征点、surf_less_flat特征点、滤波后的点云

laserMapping 流程

地图的移动机制的目的：尽量将雷达保持在特征地图中心处，以保证在做点云配准时，可以保证在雷达附近可以找到特征点云地图中的特征。

  1. 接收laserOdometry发布的特征点，submap中的corner特征和surf特征在匹配中作为target；而当前帧点云中的两种特征在匹配中作为source。

  2. 基本流程与laserOdometry相同，同样地也分为点到线和点到面的匹配。submap中的点云都是world坐标系，而当前帧的点云都是Lidar坐标系，所以在搜寻最近邻点时，先用预测的Mapping位姿w_curr，将Lidar坐标系下的特征点变换到world坐标系下

  3. 在submap的corner特征点（target）中，寻找距离当前帧corner特征点（source）最近的5个点，求出这5个点的中心。判断这5个点是不是呈线状分布，如果是线状，以中心点构建两个虚拟点。和laserOdometry处理sharp点一样，加入ceres优化，这样比laserOdometry中的ICP过程更鲁棒

  4. 对点到面的情况，也是找5个最近点，使用最小二乘拟合出平面(不是PCA了)，如果平面够平，把当前帧的点和平面法向量加入ceres优化，其实也是把点到平面的距离作为残差。

  5. 完成ICP（迭代2次）的特征匹配后，用最后匹配计算出的优化变量w_curr，更新增量wmap_wodom，为下一次Mapping做准备

Mapping线程耗时长，容易丢帧。LOAM最终输出的实时位姿是Mapping线程计算的低频位姿和Odometry线程计算的高频位姿做了整合。