GWH Blog

LeGo-LOAM中的mapOptmization节点

发表于 分类于 ， ，
本文字数： 11k 阅读时长 ≈ 10 分钟

LeGo-LOAM中的mapOptmization节点

LeGo-LOAM是在LOAM基础上改进的激光雷达定位与建图方法，它对地面点进行了优化、提出了两步LM优化算法，加入了闭环检测和全局因子图优化等。
本系列博客只说明了关键代码的功能实现，以及一些关键地方的数学推导，详细的解释都写在了代码注释中，注释后的代码在融合相机后会传到github。同时也列出来一些问题提醒自己有条件时实际去验证。

这个模块接收5种消息

1
2
3
4
5
6
7
8
9
10
// 从featureAssociation节点中接收的投影到结束点时刻Lidar坐标系下的去畸变次极大边线点云
subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2, &mapOptimization::laserCloudCornerLastHandler, this);
// 从featureAssociation节点中接收的投影到结束点时刻Lidar坐标系下的去畸变次极小平面点云
subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2, &mapOptimization::laserCloudSurfLastHandler, this);
// 从featureAssociation节点中接收的外点点云——非地面无效分割点云(5倍降采样后)
subOutlierCloudLast = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2, &mapOptimization::laserCloudOutlierLastHandler, this);
// 从featureAssociation节点中接收的当前帧位姿，是当前帧点云结束点时刻Lidar坐标系到world系的位姿变换
subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 5, &mapOptimization::laserOdometryHandler, this);
// 接收原始的IMU消息
subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 50, &mapOptimization::imuHandler, this);

这里的消息接收函数都比较简单，都是将接收到的消息存入成员变量。

这个模块有三个线程

  • 主线程：sweep到local_map的优化，ISAM2增量式更新（这个线程添加连续两点云帧之间的约束）；
  • 闭环线程：检测闭环帧，ICP配准当前帧与历史帧的local_map，ISAM2增量式更新（这个线程添加回环约束）；
  • 可视化线程：publish全局地图、保存地图到硬盘；

1、主线程

主线程的主要过程如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
void run(){
    // 判断如果3个点云消息和1个里程计消息接收成功，且时间戳一致
    // 这里就将频率不一致的里程计消息和点云消息时间戳对齐了
      ...
    std::lock_guard<std::mutex> lock(mtx);
    // 判断如果当前接收里程计消息时间戳相比上一Mapping的时间戳的间隔时间大于等于设置的建图时间间隔mappingProcessInterval=0.3s，则进行本次mapping
    if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {
      timeLastProcessing = timeLaserOdometry;
      // 基于匀速模型，为待优化变量transformTobeMapped赋一个初始值
      transformAssociateToMap();
      // 构造当前帧对应的由关键帧构成的local_map
      extractSurroundingKeyFrames();
      // 将接收到的lessSharp点集、lessFlat点集和outlierCloudl点集降采样
      downsampleCurrentScan();
      // 当前帧与local_map进行匹配、优化
      scan2MapOptimization();
      // 全局位姿优化，添加关键帧、增量式更新ISAM2，求解最新关键帧的位姿
      saveKeyFramesAndFactor(); 
      // 找到闭环并进行闭环帧优化后，在这里更新所有关键帧点云的位姿
      correctPoses();
      // 将当前帧Mapping修正后的全局位姿transformAftMapped发布出去
      publishTF();
      // 将存储关键帧位姿的点云cloudKeyPoses3D发布出去，将点云laserCloudSurfFromMapDS发布出去
      publishKeyPosesAndFrames();
      // 清空近邻地图特征点云
      clearCloud();
    }
    else return;
  }
};

mapOptmization节点的作用就是将一帧点云(sweep)与local_map匹配，得到更加准确的全局位姿，并建立全局一致的点云地图。

1.1、transformAssociateToMap()

基于匀速模型，为待优化变量transformTobeMapped赋一个初始值，这里先说明一下这三个变量的含义：

  • transformSum[]是Odometry模块计算出来的当前帧结束点时刻Lidar坐标系到世界坐标系的变换 $T_{cur-end}^{world}$;
  • transformBefMapped[]是mapping之前的Odometry计算的世界坐标系下的转换矩阵$T_{bef-map}^{world}$；
  • transformAftMapped[]是mapping微调之后的转换矩阵$T_{aft-map}^{world}$；

这里要给优化变量transformTobeMapped[]赋一个初始值：$T_{tobe}^{world} = T_{cur-end}^{world} (T_{bef-map}^{world})^{-1}T_{aft-map}^{world}$

从这个变换矩阵中，能推导出的旋转与平移为：
$$
\begin{aligned}
R_{tobe}^{world} &= R_{cur-end}^{world}(R_{bef-map}^{world})^{-1}R_{aft-map}^{world} \\
t_{tobe}^{world} &= t_{cur-end}^{world}-R_{cur-end}^{world}(R_{bef-map}^{world})^{-1}(t_{bef-map}^{world} - t_{aft-map}^{world})
\end{aligned}
$$
这里的旋转项能和代码完全对应，但是平移项对应不上！原始代码中的平移项这样计算：
$$
t_{tobe}^{world} = t_{aft-map}^{world} - R_{tobe}^{world}(R_{cur-end}^{world})^{-1}(t_{bef-map}^{world} - t_{cur-end}^{world})
$$
从推导中看，原始代码中没理解出来是什么含义。

1.2、extractSurroundingKeyFrames()

构造当前帧对应的由关键帧构成的local_map，这个local_map就是用来做配准的。这里当前帧与关键帧不是一个概念，只有满足一定条件的当前帧才能成为关键帧，关键帧的添加在saveKeyFramesAndFactor()函数中。

LeGo-LOAM提供了一个比较粗糙的闭环功能，若开启闭环功能，local_map是由一组时间近邻关键帧构成的，即由距离当前帧时间最近的50帧关键帧构成。若关闭闭环功能，local_map是由一组空间近邻关键帧构成的，即由距离当前帧半径50m内的关键帧构成的。不同于LOAM中维护空间范围划分的滑动窗口，LeGo-LOAM这里就比较直接干脆。

满足条件的关键帧的次极大边线点集拼凑起来，构成了local_map中的次极大边线点集（laserCloudCornerFromMapDS）；满足条件的关键帧的次极小平面点集与外点点集拼凑在一块，构成了local_map中的次极小平面点集（laserCloudSurfFromMapDS）。这里是有点问题的！mapOptmization节点接收到的次极大边线点集与次极小平面点集都是经过去畸变处理的，但是外点点集并没有去畸变，所以次极小平面点集与外点点集的参考坐标不一致，代码中将这俩点云直接拼凑起来。

1.3、scan2MapOptimization()

当前帧与local_map进行匹配、优化，这是一个主力函数，对应LOAM论文中Lidaring Mapping那段。首先只有local_map中的lessSharp以及lessFlat点集够大时（分别大于10,100）才进行匹配优化。

cornerOptimization()函数从局部地图lessSharp点集中查找当前帧LessSharp特征点的匹配线（通过PCA的方法），并计算匹配距离。LOAM原论文中说，Mapping模块提取二倍Odometry模块提取的边线特征点，Lego-LOAM这里对降采样后的当前帧的每一个次极大边线点都去寻求匹配线。这里寻找匹配线也是LOAM中的方法，不同于featureAssociation节点，此节点采用PCA的方法，对当前帧中的一个次极大边线点$p_a$，在local_map的次极大边线点云中寻找5个距离$p_a$最近的点，计算这5个点的均值和协方差矩阵，代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
float cx = 0, cy = 0, cz = 0;
for (int j = 0; j < 5; j++){
     cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
     cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
     cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
}
cx /= 5; cy /= 5; cz /= 5;
float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
for (int j = 0; j < 5; j++){
    float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
    float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
    float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;
    a11 += ax * ax; a12 += ax * ay; a13 += ax * az;
    a22 += ay * ay; a23 += ay * az; a33 += az * az;
}
a11 /= 5; a12 /= 5; a13 /= 5;
a22 /= 5; a23 /= 5; a33 /= 5;
matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12; matA1.at<float>(0, 2) = a13;
matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22; matA1.at<float>(1, 2) = a23;
matA1.at<float>(2, 0) = a13; matA1.at<float>(2, 1) = a23; matA1.at<float>(2, 2) = a33;
// 求协防差矩阵的特征值与特征向量
cv::eigen(matA1, matD1, matV1);

A-LOAM的代码中使用Eigen来帮助计算，代码会简洁许多：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
std::vector<Eigen::Vector3d> nearCorners;
Eigen::Vector3d center(0, 0, 0);
for (int j = 0; j < 5; j++) {
    Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
                        laserCloudCornerFromMap->points[pointSearchInd[j]].y,
                        laserCloudCornerFromMap->points[pointSearchInd[j]].z);
    center = center + tmp;
    nearCorners.push_back(tmp);
}
center = center / 5.0;
// 计算这个5个最近邻点的协方差矩阵
Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
for (int j = 0; j < 5; j++) {
    Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;
    covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
}
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);

边线点找寻匹配的线，若这5个点能构成边线，协方差矩阵的特征值只有一个比较大（代码里会验证最大的特征值是否大于3倍的次极大的特征值），则这个最大的特征值对应的特征向量就是匹配线的方向、这5个点的均值就作为直线上的点，如此边线找到了，点到直线的距离约束就可建立了。

surfOptimization()函数从局部地图lessFlat点集中查找当前帧lessFloat特征点的匹配平面（通过拟合平面的方法）。假设平面不通过原点，则平面的一般方程为Ax + By + Cz + 1 = 0，用这个假设可以少算一个参数，提高效率。对当前帧中的一个次极小平面点$p_b$，在local_map的次极小平面点云中寻找5个距离$p_b$最近的点，使用这5个点拟合出一个平面出来。若拟合的平面效果较好（将这5个点分别代入平面方程，看误差是否超过0.2），点到平面的距离约束就可建立了。

调用LMOptimization()函数，Gauss-Newton优化算法求解建立的约束的增量方程。具体的推导新开一节，统一整理。

1.4、saveKeyFramesAndFactor()

此函数添加关键帧，并进行全局的因子图优化。若当前帧距离上一关键帧的位移量超过0.3m，当前帧被定义为关键帧，首先将当前帧的位姿图约束添加到因子图中，增量式更新iSAM2对象，将更新后的当前帧位姿保存到关键帧队列，当前帧对应的点云也保存到关键帧队列，供给回环检测线程使用。这里向因子图添加约束时，Lego-LOAM又将坐标系转换到原来的x轴向前，y轴向左，z轴向上的坐标下：

1
2
3
4
5
6
7
8
// 因子图中增加一个先验因子，将其设为原点 TODO::这里为什么要做这么个变换，后来还费事变换回去？
// 传感器本身的坐标系是x轴向前，y轴向左，z轴向上的右手坐标系，lego-loam将其转换为z轴向前，x轴向左，y轴向上的右手坐标系
// 整个处理都是在z轴向前，x轴向左，y轴向上的右手坐标系下，在这个坐标系下：
// roll = transformTobeMapped[0]; pitch = transformTobeMapped[1]; yaw = transformTobeMapped[2];  R_cur^{world} = Ry(pitch) * Rx(roll) * Rz(yaw)
// 这里的旋转Pose3,平移Point3又变换到原来的坐标系下了，Pose3 = Rz(pitch) * Ry(roll) * Rx(yaw)
gtSAMgraph.add(PriorFactor<Pose3>(0,Pose3(Rot3::RzRyRx(transformTobeMapped[2],transformTobeMapped[0], transformTobeMapped[1]),Point3(transformTobeMapped[5],transformTobeMapped[3],transformTobeMapped[4])), priorNoise));
// 给添加进去的因子一个初始值，initialEstimate的数据类型是Values,其实就是一个map，这里在0对应的值下面保存了一个Pose3
initialEstimate.insert(0,Pose3(Rot3::RzRyRx(transformTobeMapped[2],transformTobeMapped[0],transformTobeMapped[1]),Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])));

1.5、correctPoses()

回环检测线程找到闭环并进行闭环帧优化后，在这个线程里更新所有关键帧点云的位姿。这里先将当前帧近邻特征点云队列清空，因为所有关键帧的位姿都会更新，再搜寻当前帧50m半径范围内的关键帧时会发生点变化，所以全部清空，完全从新构造local_map。

1.6、发布全局位姿、相关点云

publishTf()把当前帧Mapping修正后的全局位姿transformAftMapped发布出去；publishKeyPosesAndFrames()将存储关键帧位姿的点云cloudKeyPoses3D发布出，将点云laserCloudSurfFromMapDS(当前帧的近邻关键帧集合)发布出去。

2、回环检测线程

回环检测线程以1Hz的频率去检测闭环，Lego-LOAM中的闭环检测是基于里程计的几何关系的：

1
2
3
4
5
6
7
8
9
10
11
12
void loopClosureThread()
{
  if (loopClosureEnableFlag == false)
    return;
  // 以1Hz的频率去检测回环
  ros::Rate rate(1);
  while (ros::ok())
  {
    rate.sleep();
    performLoopClosure();
  }
}

performLoopClosure()函数先调用detectLoopClosure()函数从历史关键帧轨迹中找寻回环帧，流程如下：

  1. 使用历史关键帧位置构建kdtree，从kdtree中搜素距离当前帧位置7.0m范围内的历史帧;
  2. 将当前帧的lessSharp与lessFlat点合并构成latestSurfKeyFrameCloud;
  3. 在满足条件的历史回环帧周围(前后25帧)构建局部地图nearHistorySurfKeyFrameCloudDS，局部地图是由lessSharp与lessFlat点集构成的;
  4. 将这个局部地图Publish出去用于可视化。

经过这个函数，可以找到当前帧对应的历史回环帧，performLoopClosure()函数之后对latestSurfKeyFrameCloud点云向nearHistorySurfKeyFrameCloudDS点云进行ICP配准。如果ICP配准收敛并且ICP配准得分大于最低阈值（0.3），则认为回环成功，将回环约束添加到因子图中，增量式更新ISAM2。

1
2
3
// 如果ICP配准没有收敛或ICP配准得分大于最低阈值（0.3）
if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
   return;

这里闭环约束的添加感觉有点问题!

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
float x, y, z, roll, pitch, yaw;
Eigen::Affine3f correctionCameraFrame = icp.getFinalTransformation();
// roll-->transformTobeMapped[0]; pitch --> transformTobeMapped[1]; yaw --> transformTobeMapped[2]
pcl::getTranslationAndEulerAngles(correctionCameraFrame, x, y, z, roll, pitch, yaw);
// 这里变换为saveKeyFramesAndFactor()函数中ISAM2对应的Pose3格式，旋转是R = Rz(pitch) * Ry(roll) * Rx(yaw)
Eigen::Affine3f correctionLidarFrame = pcl::getTransformation(z, x, y, yaw, roll, pitch); 
Eigen::Affine3f tWrong = pclPointToAffine3fCameraToLidar(cloudKeyPoses6D->points[latestFrameIDLoopCloure]);    
// 这个的误差构造感觉有点问题，tCorrect = T_{cur}^{loop} * T_{cur}^{world}
Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;
pcl::getTranslationAndEulerAngles(tCorrect, x, y, z, roll, pitch, yaw);
// poseFrom = T_{cur}^{loop} * T_{cur}^{world}
gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
// poseTo = T_{loop}^{world}
gtsam::Pose3 poseTo = pclPointTogtsamPose3(cloudKeyPoses6D->points[closestHistoryFrameID]);
gtsam::Vector Vector6(6);
float noiseScore = icp.getFitnessScore();
Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
constraintNoise = noiseModel::Diagonal::Variances(Vector6);                                // 噪声标准差    
// poseFrom.between(poseTo) = poseFrom^{-1} poseTo
std::lock_guard<std::mutex> lock(mtx);                                                     // poseFrom^{-1}*poseTo
gtSAMgraph.add(BetweenFactor<Pose3>(latestFrameIDLoopCloure, closestHistoryFrameID, poseFrom.between(poseTo), constraintNoise));

3、可视化线程

可视化线程5s调用一次publishGlobalMap()函数，此函数发布距离当前位置currentRobotPosPoint半径globalMapVisualizationSearchRadius内的所有关键帧位姿点云和地图点云，globalMapVisualizationSearchRadius足够大，可以发布整张地图。并且可视化线程还将这个地图保存到硬盘中，也保存所有关键帧的LessSharp点云，LessFlat点云+outlier点云到硬盘。

想要获取比较稠密的点云地图怎么去改

  1. 在featureAssociation节点的publishCloudsLast()函数中取消点云发布频率的限制；
  2. 在mapOptmization节点将接收到的消息缓存在queue中，这样不会出现丢帧，进而可以选择是否处理每一帧；
  3. 在mapOptmization节点的saveKeyFramesAndFactor()函数中将关键帧距离阈值调小；

可以优化的点

  1. 次极小平面点集与外点点集的参考坐标不一致，可以在featureAssociation节点加入对outlier点云的去畸变+投影的过程；
  2. 将历史关键帧缓存到硬盘而不是一直缓存在内存。saveKeyFramesAndFactor()保存了所有关键帧的信息以用来回环检测，尽管这里只是把智能指针push_back进去vector容器，但是指针所指之内存不会被释放。小场景下不会出问题，大场景下会占用过多的内存。（PS：尽管程序自己会用SWap，但是就是感觉不爽。）
  3. Lego-LOAM的回环检测还是比较粗糙It often fails when the odometry drift is too large.参考这个SC-LeGO-LOAM]

潜在的问题

这一部分是我推导出来的结果，后续我会一一验证，暂时罗列出来给自己做一个标记！

  1. transformAssociateToMap()函数中对平移量初始值的赋予；
  2. performLoopClosure()函数中添加的回环约束项；
听说打赏我的人，最后都找到了真爱。