激光SLAM如何动态管理关键帧和地图

Tip: 如果你在进行深度学习、自动驾驶、模型推理、微调或AI绘画出图等任务，并且需要GPU资源，可以考虑使用UCloud云计算旗下的Compshare的GPU算力云平台。他们提供高性价比的4090 GPU，按时收费每卡2.6元，月卡只需要1.7元每小时，并附带200G的免费磁盘空间。通过链接注册并联系客服，可以获得20元代金券（相当于6-7H的免费GPU资源）。欢迎大家体验一下~

0. 简介

个人在想在长期执行的SLAM程序时，当场景发生替换时，激光SLAM如何有效的更新或者替换地图是非常关键的。在看了很多Life-Long的文章后，个人觉得可以按照以下思路去做。这里可以给大家分享一下

1. 初始化保存关键帧

首先对应的应该是初始化设置，初始化设置当中会保存关键帧数据，这里的对应的关键帧点云数据会被存放在history_kf_lidar当中，这个数据是和关键帧状态一一对应的。

/**
 * @brief 初始化当前第一个关键帧
 */
void dlio::OdomNode::initializeInputTarget() {
  // 构建第一个关键帧
  // 更新prev_scan_stamp
  this->prev_scan_stamp = this->scan_stamp;
  // 保存关键帧的姿态 以及去畸变降采样后的点云
  this->keyframes.push_back(std::make_pair(std::make_pair(this->lidarPose.p, this->lidarPose.q), this->current_scan));
  // 保存关键帧消息时间戳
  this->keyframe_timestamps.push_back(this->scan_header_stamp);
  // 保存关键帧点云协方差
  this->keyframe_normals.push_back(this->gicp.getSourceCovariances());
  // 初始T-corr为单位的 T = T_corr * T_prior
  this->keyframe_transformations.push_back(this->T_corr);
  this->keyframe_transformations_prior.push_back(this->T_prior);
  this->keyframe_stateT.push_back(this->T);

  // 保存历史的lidar系点云
  std::unique_lock<decltype(this->history_kf_lidar_mutex)> lock_his_lidar(this->history_kf_lidar_mutex);
  pcl::PointCloud<PointType>::Ptr temp(new pcl::PointCloud<PointType>);
  pcl::copyPointCloud(*this->current_scan_lidar, *temp);
  this->history_kf_lidar.push_back(temp);
  lock_his_lidar.unlock();

  // 保存当前的关键帧信息 等待后端处理
  // tempKeyframe的姿态这里有两种处理方法
  // 1. 使用lidarPose 2. 使用state
  // 根据DLIO作者的解释 使用lidarPose更加稳定
  // (https://github.com/vectr-ucla/direct_lidar_inertial_odometry/issues/13#issuecomment-1638876779)
  this->currentFusionState = this->state;
  this->tempKeyframe.pos = this->lidarPose.p;
  this->tempKeyframe.rot = this->currentFusionState.q;
  this->tempKeyframe.vSim = {1};
  this->tempKeyframe.submap_kf_idx = {0};
  this->tempKeyframe.time = this->scan_stamp;
  this->v_kf_time.push_back(this->scan_stamp);
  pcl::copyPointCloud(*this->current_scan, *this->tempKeyframe.pCloud);
  this->KeyframesInfo.push_back(this->tempKeyframe);

  this->saveFirstKeyframeAndUpdateFactor();

}();

在保存第一针状态并更新因子的时候，会同时缓存一个update_map_info。这个缓存了回环数据（依赖addLoopFactor函数）以及基于Gtsam计算出来的(iSAMCurrentEstimate)函数。

添加里程计因子 - 通过调用this->addOdomFactor()，函数向因子图中添加了一个里程计因子。里程计因子可能包含了从传感器得到的运动信息，这些信息用于估算机器人或设备的位置。

更新ISAM（增量平滑和映射） - 使用this->isam->update(this->gtSAMgraph, this->initialEstimate)来更新ISAM的状态，这个步骤通常包括增量优化，以便在获得新数据时快速更新地图或路径。然后再次调用this->isam->update()进行进一步的更新。
清空因子图和初始估计 - 将因子图和初始状态估计清空，这通常发生在更新完成之后，为下一轮更新做准备。
**计算当前估计 **- 使用this->isam->calculateEstimate()来计算当前状态的最佳估计。
获取临时关键帧信息 - 在互斥锁的保护下，从this->tempKeyframe获取临时关键帧的信息，并释放锁。
校正位置 - 通过调用this->correctPoses()函数来校正或调整已经估计的位置。
更新地图信息 - 处理是否发生了回环检测（由this->isLoop变量表示），并将当前估计状态和回环检测的结果推入更新队列this->update_map_info中，用于后续的地图更新或路径规划。

/**
 * @brief 对第一帧的处理
 */
void dlio::OdomNode::saveFirstKeyframeAndUpdateFactor()
{
    // 添加里程计因子
    this->addOdomFactor();

    this->isam->update(this->gtSAMgraph, this->initialEstimate);
    this->isam->update();

    this->gtSAMgraph.resize(0);
    this->initialEstimate.clear();
    this->iSAMCurrentEstimate = this->isam->calculateEstimate();

    std::unique_lock<decltype(this->tempKeyframe_mutex)> lock_temp(this->tempKeyframe_mutex);
    KeyframeInfo his_info = this->tempKeyframe;
    lock_temp.unlock();

    this->correctPoses();
    auto loop_copy = this->isLoop;
    auto iSAMCurrentEstimate_copy  = this->iSAMCurrentEstimate;
    std::unique_lock<decltype(this->update_map_info_mutex)> lock_update_map(this->update_map_info_mutex);
    this->update_map_info.push(std::make_pair(loop_copy, iSAMCurrentEstimate_copy));
    lock_update_map.unlock();
}

在初始状态下，第一帧只用去考虑this->addOdomFactor();这个函数，用于添加odom的因子，里程计因子包含了从传感器得到的运动信息，这些信息用于估算机器人或设备的位置。

静态计数器- static int num_factor用作计数器，记录添加到因子图中的里程计因子数量。

获取当前关键帧信息 - 函数首先通过互斥锁保护的方式获取临时关键帧（this->tempKeyframe）的信息。

初始帧处理 - 如果是第一帧（num_factor为0），函数会设置一个非常小的噪声模型作为先验，表明对初始关键帧的位置非常有信心，并将其添加到因子图和初始估计中。

添加里程计因子 - 对于非第一帧的情况，函数会遍历当前关键帧匹配的子地图关键帧，并对每一个匹配计算一个从匹配关键帧到当前关键帧的位姿变换。这涉及到获取匹配关键帧的位姿并计算它们之间的相对变换（poseFrom.between(poseTo)）。

计算噪声权重 - 函数会根据匹配的相似度计算噪声权重，相似度越高，噪声权重越小，表示对这个匹配的信心越大。这里依赖的是buildSubmapViaJaccard函数

添加因子到因子图 - 将计算得到的位姿变换和相应的噪声模型作为里程计因子添加到因子图中。

更新初始估计- 将当前关键帧的位姿添加到初始估计中。

因子计数器递增 - 最后，num_factor递增，为下一次调用准备

/**
 * @brief 添加里程计因子 包含连续里程计和非连续里程计
 */
void dlio::OdomNode::addOdomFactor()
{
    static int num_factor = 0;

    std::unique_lock<decltype(this->tempKeyframe_mutex)> lock_temp(this->tempKeyframe_mutex);
    KeyframeInfo current_kf_info = this->tempKeyframe;
    lock_temp.unlock();

    // 初始第一帧
    if (num_factor == 0)
    {
        gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(
                (gtsam::Vector(6) << 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12).finished());
        this->gtSAMgraph.addPrior(0, state2Pose3(current_kf_info.rot, current_kf_info.pos), priorNoise);
        this->initialEstimate.insert(0, state2Pose3(current_kf_info.rot, current_kf_info.pos));
    }
    else
    {
        // 当前帧匹配的子地图关键帧索引
        std::vector<int> curr_submap_id = current_kf_info.submap_kf_idx;
        // 当前帧匹配的子地图关键帧相似度
        std::vector<float> curr_sim = current_kf_info.vSim;
        // 当前帧的位姿
        gtsam::Pose3 poseTo = state2Pose3(current_kf_info.rot, current_kf_info.pos);
        // 遍历与当前帧匹配的子地图关键帧
        for (int i = 0; i < curr_submap_id.size(); i++)
        {
            // 子地图关键帧的位姿
            std::unique_lock<decltype(this->keyframes_mutex)> lock_kf(this->keyframes_mutex);
            gtsam::Pose3 poseFrom = state2Pose3(this->KeyframesInfo[curr_submap_id[i]].rot,
                                                this->KeyframesInfo[curr_submap_id[i]].pos);
            lock_kf.unlock();
            // 噪声权重
            double weight = 1.0;

            if (curr_sim.size() > 0)
            {
                // 对相似度进行归一化
                auto max_sim = std::max_element(curr_sim.begin(), curr_sim.end());
                for (auto &it: curr_sim)
                    it = it / *max_sim;
                double sim = curr_sim[i] >= 1 ? 0.99 : curr_sim[i] < 0 ? 0 : curr_sim[i];

                // 相似度越大 噪声权重越小
                weight = (1 - sim) * this->icpScore;
            }
            // 添加相邻/不相邻里程计因子
            gtsam::noiseModel::Diagonal::shared_ptr odometryNoise = gtsam::noiseModel::Diagonal::Variances(
                    (gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished() * weight);
            this->gtSAMgraph.emplace_shared<gtsam::BetweenFactor<gtsam::Pose3>>(curr_submap_id[i],
                                                                          num_factor,poseFrom.between(poseTo), odometryNoise);
        }
        this->initialEstimate.insert(num_factor, poseTo);
    }
    num_factor++;

}

2. 在过程当中保存关键帧

然后下面就是保存关键帧，这个主要会根据欧式距离这些来判断是否是属于关键帧。如果符合条件就会将关键帧keyframes以及历史的激光点云history_kf_lidar保存（updateCurrentInfo）。并将对应的因子加入到Gtsam因子图优化中（addOdomFactor、addGPSFactor、addLoopFactor）。

/**
 * @brief 判断关键帧 添加因子 后端优化
 */
void dlio::OdomNode::saveKeyframeAndUpdateFactor()
{
    if (this->isKeyframe())
    {
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
        // 更新关键帧信息
        this->updateCurrentInfo();

        // 添加里程计因子
        this->addOdomFactor();

        // 添加GPS因子
        this->addGPSFactor();

        // 添加闭环因子
        this->addLoopFactor();

        this->isam->update(this->gtSAMgraph, this->initialEstimate);
        this->isam->update();

        if (this->isLoop)
        {
            this->isam->update();
            this->isam->update();
            this->isam->update();
        }

        this->gtSAMgraph.resize(0);
        this->initialEstimate.clear();
        this->iSAMCurrentEstimate = this->isam->calculateEstimate();

        // 用本次后端处理的结果更新位姿
        this->correctPoses();

        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
        double time = std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();

        auto loop_copy = this->isLoop;
        auto iSAMCurrentEstimate_copy  = this->iSAMCurrentEstimate;

        // 将本次后端的处理结果发送给updateMap线程 用于更新可视化地图
        std::unique_lock<decltype(this->update_map_info_mutex)> lock_update_map(this->update_map_info_mutex);
        this->update_map_info.push(std::make_pair(loop_copy, iSAMCurrentEstimate_copy));
        lock_update_map.unlock();

    }
    else
    {
        return;
    }

}

3. 关键帧整合以及地图点替换

到这一步，我们其实可以不停地获取关键帧了，同时这些关键帧会被赋予是否是在地图中可以被找到的。然后我们就可以根据当前的位置。这里采用了并集点来判断关键帧的相似度，如果相似度大于一定的值它可以被加入，同时在定位模式下，如果前一帧和后一帧都满足条件，而当前帧小于一定的值这个方法也可以去替换对应的关键帧，从而进一步确保地图可以有效的被替换，从而维护一个长期的地图

初始化变量 - 初始化用于跟踪子地图构建次数、总时间和最后一个回环检测ID的静态变量。

清理当前子地图关键帧索引 - 清空存储当前构建子地图的关键帧索引的容器。

筛选候选关键帧- 遍历所有处理过的关键帧，并筛选出与当前车辆状态距离较近的关键帧。

排序和筛选 - 根据距离对筛选出的关键帧进行排序，保留距离当前帧最近的一定数量的关键帧。

计算交并比 - 对每个候选关键帧，使用八叉树搜索算法对其点云进行处理，计算与当前帧点云的交集和并集，进而得到Jaccard相似度。

确定最终子地图关键帧 - 如果计算得到的相似度大于设定的阈值，则将该候选关键帧加入最终子地图关键帧集。

回环检测 - 对于每个候选关键帧，如果其与当前帧的索引相差较大，并且距离上一次回环检测有一定帧数的间隔，并且距离小于一定值，则认为发现了回环。然后会被performLoop获取保存，并发送给后端来添加回环因子完成优化

…详情请参照古月居