激光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获取保存，并发送给后端来添加回环因子完成优化

…详情请参照古月居

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：/a/767161.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

Activity、Window、DecorView的关系

Activity、Window、DecorView的关系

目录 一、Activity、Window、DecorView的层级关系如下图所示&#xff1a; 1、Activity 2、Window 3、DecorView 二、DecorView初始化相关源码 三、DecorView显示时机 前言&#xff1a; 不同的Android版本有差异&#xff0c;以下基于Android 11进行讲解。 一、Activi…
阅读更多...
音乐发行平台无加密开源源码

音乐发行平台无加密开源源码

适用于唱片公司&#xff0c;用于接收物料&#xff0c;下载物料功能&#xff1a;个人或机构认证&#xff0c;上传专辑和歌曲&#xff0c;版税结算环境要求php7.4Nginx 1、导入数据库 2、/inc/conn.php里填写数据库密码等后台路径/admin&#xff08;可自行修改任意入口名称&…
阅读更多...
Meta 3D Gen：文生 3D 模型

Meta 3D Gen：文生 3D 模型

是由 Meta 公布的一个利用 Meta AssetGen&#xff08;模型生成&#xff09;和 TextureGen&#xff08;贴图材质生成&#xff09;的组合 AI 系统&#xff0c;可以在分分钟内生成高质量 3D 模型和高分辨率贴图纹理。 视频演示的效果非常好&#xff0c;目前只有论文&#xff0c;期…
阅读更多...
计算机网络--网络层

计算机网络--网络层

一、网络层的服务和功能 网络层主要为应用层提供端对端的数据传输服务 网络层接受运输层的报文段&#xff0c;添加自己的首部&#xff0c;形成网络层分组。分组是网络层的传输单元。网络层分组在各个站点的网络层之间传输&#xff0c;最终到达接收方的网络层。接收方网络层将运…
阅读更多...
PLC_博图系列☞TP：生成脉冲

PLC_博图系列☞TP：生成脉冲

PLC_博图系列☞TP&#xff1a;生成脉冲 文章目录 PLC_博图系列☞TP&#xff1a;生成脉冲背景介绍TP&#xff1a; 生成脉冲说明参数脉冲时序图示例 关键字&#xff1a; PLC、 西门子、 博图、 Siemens 、 TP 背景介绍 这是一篇关于PLC编程的文章&#xff0c;特别是关于西门…
阅读更多...
快速上手文心一言指令：解锁AI对话新纪元

快速上手文心一言指令：解锁AI对话新纪元

快速上手文心一言指令 一、引言&#xff1a;文心一言的魅力所在二、准备工作&#xff1a;了解文心一言平台2.1 轻松注册&#xff0c;开启智能对话之旅2.2 深度探索&#xff0c;掌握界面布局奥秘2.2.1 输入框&#xff1a;智慧交流的起点2.2.2 回复区&#xff1a;即时反馈的窗口2…
阅读更多...
Echarts-折线图

Echarts-折线图

1.案例1 1.1代码 option {"tooltip": {"trigger": "axis","backgroundColor": "rgba(32, 33, 36,.7)","borderColor": "rgba(32, 33, 36,0.20)","borderWidth": 10,"textStyle"…
阅读更多...
星辰资讯 | TiUP v1.16 发版，支持 PD 微服务

星辰资讯 | TiUP v1.16 发版，支持 PD 微服务

如果你对 TiDB 还不太了解&#xff0c;或者你对数据库安装部署的认知仍然停留在手动和脚本时代&#xff0c;那么&#xff0c;请先戳这里了解一下 TiUP 神器&#xff1a; 震惊&#xff01;数据库小白装国产数据库只需10分钟&#xff01; TiDB 7.x 源码编译之 TiUP 篇 TiUP&#…
阅读更多...
基于改进高斯-拉普拉斯滤波器的一维时间序列平滑与降噪（MATLAB）

基于改进高斯-拉普拉斯滤波器的一维时间序列平滑与降噪（MATLAB）

以图像处理为例&#xff0c;拉普拉斯算子是基于图像的二阶导数来找到边缘并搜索过零点&#xff0c;传统的拉普拉斯算子常产生双像素宽的边缘&#xff0c;对于较暗区域中的亮斑进行边缘检测时&#xff0c;拉普拉斯运算就会使其变得更亮。因此&#xff0c;与梯度算子一样&#xf…
阅读更多...
基于tensorflow2的目标检测完整实现过程

基于tensorflow2的目标检测完整实现过程

序言 虽然tf1仍然在维护&#xff0c;但tf2毕竟是主流&#xff0c;如果不是项目有明确要求&#xff0c;建议直接选择tf2。本文以tf2为例展开&#xff0c;总结从环境准备到使用自己的数据和tensorflow预训练模型进行快速训练和调用。对tensorflow和目标检测算法有深入了解的&…
阅读更多...
Seal^_^【送书活动第8期】——《ChatGLM3大模型本地化部署、应用开发与微调》

Seal^_^【送书活动第8期】——《ChatGLM3大模型本地化部署、应用开发与微调》

Seal^_^【送书活动第8期】——《ChatGLM3大模型本地化部署、应用开发与微调》 一、参与方式二、本期推荐图书2.1 作者建语2.2 编辑推建2.3 图书简介2.4 前 言2.5 目 录 三、正版购买 大模型领域 既是繁星点点的未知宇宙&#xff0c;也是蕴含无数可能的广阔天地&#xff0c; 正…
阅读更多...
“不喝鸡汤 不诉离殇”华火电燃灶用实力引领烹饪灶具发展

“不喝鸡汤 不诉离殇”华火电燃灶用实力引领烹饪灶具发展

在这个快节奏的时代&#xff0c;我们常常被各种厨房电器的鸡汤所包围&#xff0c;并悄悄的告诉我们厨房生活是美好与温暖的&#xff0c;但面对现实中的挑战与困难时&#xff0c;常常表现出选择性失明&#xff1b;那些隐藏在传统厨房烹饪环境下的危机&#xff0c;就像是慢性的毒…
阅读更多...
参数污染漏洞（HPP）挖掘技巧及实战案例全汇总

参数污染漏洞（HPP）挖掘技巧及实战案例全汇总

目录 概念： 漏洞原理： 实战案例总结： 1. 逻辑漏洞（IDOR） 2. 绕过检测（WAF） 挖掘技巧： 修复方案： 概念： HTTP参数污染，也叫HPP（HTTP Parameter Pollution）。简单地讲就是给一个参数赋上两个或两个以上的值，由于现行的HTTP标准没有提及在遇到多个输入值给相…
阅读更多...
设计IC行业SAP软件如何处理芯片成本计算

设计IC行业SAP软件如何处理芯片成本计算

在集成电路(IC)设计与制造行业中&#xff0c;精确的成本计算对于维持健康的财务状况、优化生产流程以及保持市场竞争力至关重要。SAP软件&#xff0c;作为一种全面的企业资源规划(ERP)解决方案&#xff0c;为IC行业提供了强大且灵活的成本计算工具。以下是SAP软件如何处理芯片成…
阅读更多...
【Linux】应用层创建XXX文件，文件系统调用可以查看到文件名

【Linux】应用层创建XXX文件，文件系统调用可以查看到文件名

搞了台电脑&#xff0c;昨天把系统装了下&#xff0c;继续搞事&#xff1a; 上次基于内核代码openat的系统打印被操作的文件名&#xff0c;发现不成功&#xff0c;很奇怪&#xff0c;这种问题内核不可能会犯这种低级别的问题吧&#xff1f; 反过来想&#xff0c;那不是内核的问…
阅读更多...
Vscode快捷键崩溃

Vscode快捷键崩溃

Vscode快捷键崩溃 Linux虚拟机下使用vscode写代码【ctrlA&#xff0c;CtrlC&#xff0c;CtrlV】等快捷键都不能使用&#xff0c;还会出现“NO text insert“等抽象的指令&#xff0c;问题就是不知道什么时候装了一个VIM插件&#xff0c;让他滚出电脑》》》
阅读更多...
[vue3+js]实现3d旋转效果

[vue3+js]实现3d旋转效果

1. 实现效果图&#xff1a; 2.实现代码&#xff1a; css: <style lang"scss" scoped>.bottomContainer{width: 1200px;height: 400px;display: flex;justify-content: center;position: relative;margin:200px auto;align-items: center;// background-image…
阅读更多...
迈向智慧水利新时代：聚焦智慧水利解决方案的核心技术与发展方向，展望其在推动水利行业可持续发展中的重要作用

迈向智慧水利新时代：聚焦智慧水利解决方案的核心技术与发展方向，展望其在推动水利行业可持续发展中的重要作用

目录 一、引言 二、智慧水利解决方案的核心技术 1. 物联网技术 2. 大数据与云计算 3. 人工智能与机器学习 4. 区块链技术 三、智慧水利的发展方向 1. 深化技术融合与创新 2. 强化系统集成与协同 3. 提升公众参与与互动 4. 注重生态友好与可持续发展 四、智慧水利在…
阅读更多...
5.3.3、二维数组案例-考试成绩统计

5.3.3、二维数组案例-考试成绩统计

代码 #include <iostream> using namespace std; #include <string>int main() {//二维数组案例-考试成绩统计//1、创建二维数组int scores[3][3] {{100,100,100},{90,50,100},{60,70,80},};string names[3] { "张三","李四","王五&quo…
阅读更多...
Qt——升级系列（Level Seven）：事件、文件

Qt——升级系列（Level Seven）：事件、文件

目录 Qt事件 事件介绍 事件的处理 按键事件 鼠标事件 定时器 事件分发器 事件过滤器 Qt文件 Qt文件概述 输入输出设备类 文件读写类 文件和目录信息类 Qt事件 事件介绍 事件是应⽤程序内部或者外部产⽣的事情或者动作的统称。在 Qt 中使⽤⼀个对象来表⽰⼀个事件。所有的 Qt …
阅读更多...
最新文章

Copyright @ 2022~2023