1.ORB-SLAM3中如何保存多地图、关键帧、地图点到二进制文件中

1 保存多地图

1.1 为什么保存(视觉)地图

        因为我们要去做导航,导航需要先验地图。因此需要保存地图供导航使用,下面来为大家讲解如何保存多地图。

 1.2 保存多地图的主函数SaveAtlas

2051

        mStrSaveAtlasToFile是配置文件中传递的参数:

        这里我们赋值成了akm(string mStrSaveAtlasToFile;)。

        即我们如果不传入mStrSaveAtlasToFile这个参数的话,就不会保存地图。

        我们执行预保存的代码 1.2.1 - 1.2.4节有详细的推导。

        我们确定要保存的文件名字:mStrSaveAtlasToFile(akm) + .osa即akm.osa

        保存词典的校验结果及名字

 3. 保存词典的校验结果及名字
// 计算给定文件路径 mStrVocabularyFilePath 对应文件的校验和或者哈希值,并将结果保存在 strVocabularyChecksum 变量中。
string strVocabularyChecksum = CalculateCheckSum(mStrVocabularyFilePath,TEXT_FILE);
// 找到了 mStrVocabularyFilePath 中最后一个目录分隔符(/ 或者 \)的位置,并将其索引保存在 found 变量中。这个操作通常用于从文件路径中提取文件名。
std::size_t found = mStrVocabularyFilePath.find_last_of("/\\");
// 这行代码利用之前找到的目录分隔符的位置 found,从 mStrVocabularyFilePath 中提取文件名部分,并将提取的文件名存储在 strVocabularyName 变量中。
string strVocabularyName = mStrVocabularyFilePath.substr(found+1);

        方便后续的读取。

        我们用二进制保存地图:

         4.用txt保存 / 用二进制保存if(type == TEXT_FILE){cout << "Starting to write the save text file " << endl;std::remove(pathSaveFileName.c_str());std::ofstream ofs(pathSaveFileName, std::ios::binary);boost::archive::text_oarchive oa(ofs);oa << strVocabularyName;oa << strVocabularyChecksum;oa << mpAtlas;cout << "End to write the save text file" << endl;}else if(type == BINARY_FILE) // File binary{cout << "Starting to write the save binary file" << endl;std::remove(pathSaveFileName.c_str());std::ofstream ofs(pathSaveFileName, std::ios::binary);boost::archive::binary_oarchive oa(ofs);oa << strVocabularyName;oa << strVocabularyChecksum;oa << mpAtlas;cout << "End to write save binary file" << endl;}}

        在1.2.1-1.2.4中我们已经准备好了所有的保存变量(含Backup的变量)

        至此地图被保存:

1.2.1 预保存想要保存的数据Atlas::PreSave

/*** @brief 预保存,意思是在保存成地图文件之前,要保存到对应变量里面*/
void Atlas::PreSave()
{1. 更新mnLastInitKFidMap// mpCurrentMap是当前的地图 Map *Atlas::mpCurrentMapif (mpCurrentMap){// mnLastInitKFidMap为当前地图创建时第1个关键帧的id,它是在上一个地图最大关键帧id的基础上增加1// mspMaps保存了每一个地图// 如果地图集不为空且 前地图创建时第1个关键帧的id小于当前地图的最大关键帧// 言外之意就是当前地图的关键帧的数量大于1 更新mnLastInitKFidMap为下一个关键帧的索引if (!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid())mnLastInitKFidMap = mpCurrentMap->GetMaxKFid() + 1; }// 如果 elem1 所指向的 Map 对象的 Id 小于 elem2 所指向的 Map 对象的 Id,则返回 true,// 表示 elem1 应该排在 elem2 前面;否则返回 false,表示 elem2 应该排在 elem1 前面或它们相等。struct compFunctor{inline bool operator()(Map *elem1, Map *elem2){return elem1->GetId() < elem2->GetId();}};// vector<Map *> Atlas::mvpBackupMaps 拷贝地图std::copy(mspMaps.begin(), mspMaps.end(), std::back_inserter(mvpBackupMaps));2. 按照id从小到大排列sort(mvpBackupMaps.begin(), mvpBackupMaps.end(), compFunctor());std::set<GeometricCamera *> spCams(mvpCameras.begin(), mvpCameras.end());3. 遍历所有地图,执行每个地图的预保存for (Map *pMi : mvpBackupMaps){if (!pMi || pMi->IsBad())continue;// 如果地图为空,则跳过if (pMi->GetAllKeyFrames().size() == 0){// Empty map, erase before of save it.SetMapBad(pMi);continue;}pMi->PreSave(spCams);}4. 删除坏地图RemoveBadMaps();
}

        我们要先明白几个变量的含义:

        1.mpCurrentMap是当前的地图 Map *Atlas::mpCurrentMap

        2.mnLastInitKFidMap为当前地图创建时第1个关键帧的id,它是在上一个地图最大关键帧id的基础上增加1

        3.mspMaps保存了所有地图:set<Map *> Atlas::mspMaps

        if (!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid())mnLastInitKFidMap = mpCurrentMap->GetMaxKFid() + 1;

        即如果地图集不为空 且 前地图创建时第1个关键帧的id小于当前地图的最大关键帧则更新mnLastInitKFidMap为下一个关键帧的索引。那么也就是说如果多地图系统中有地图且当前地图的关键帧的数量大于1 更新mnLastInitKFidMap为下一个关键帧的索引。

    struct compFunctor{inline bool operator()(Map *elem1, Map *elem2){return elem1->GetId() < elem2->GetId();}};

        如果 elem1 所指向的 Map 对象的 Id 小于 elem2 所指向的 Map 对象的 Id,则返回 true,表示 elem1 应该排在 elem2 前面;否则返回 false,表示 elem2 应该排在 elem1 前面或它们相等。

    std::copy(mspMaps.begin(), mspMaps.end(), std::back_inserter(mvpBackupMaps));sort(mvpBackupMaps.begin(), mvpBackupMaps.end(), compFunctor());

        因此这两行的代码的含义就是将mspMaps(set<Map *> Atlas::mspMaps)存放的所有地图存放到mvpBackupMaps(vector<Map *> Atlas::mvpBackupMaps以便于后续保存,在vector容器中的地图是按照从前到后的顺序排列。

        随后我们遍历每一个地图,执行每张地图的预保存:

     3. 遍历所有地图,执行每个地图的预保存for (Map *pMi : mvpBackupMaps){if (!pMi || pMi->IsBad())continue;// 如果地图为空,则跳过if (pMi->GetAllKeyFrames().size() == 0){// Empty map, erase before of save it.SetMapBad(pMi);continue;}pMi->PreSave(spCams);}

        如果地图是不好的我们处理下一张地图。

        如果地图关键帧为空的话,我们去将这个地图设置为不好的。       

void Atlas::SetMapBad(Map *pMap)
{mspMaps.erase(pMap);pMap->SetBad();mspBadMaps.insert(pMap);
}

        我们在地图集删去这张地图并且在坏地图中加入这张地图。

        如果这张地图有关键帧存在,那么我们继续执行预保存代码。

1.2.2 预保存地图 Map::PreSave

/** 预保存,也就是把想保存的信息保存到备份的变量中* @param spCams 相机*/
void Map::PreSave(std::set<GeometricCamera *> &spCams)
{int nMPWithoutObs = 0;  // 统计用1. 剔除一下无效观测// 遍历每一个地图点for (MapPoint *pMPi : mspMapPoints){// 地图点是坏的(优化线程)if (!pMPi || pMPi->isBad())continue;// 如果这个地图点没有被观测到if (pMPi->GetObservations().size() == 0){nMPWithoutObs++;}// 能够观测到当前地图点的所有关键帧及该地图点在KF中的索引// it->first 观测到地图点的关键帧// 观测到地图点的关键帧不存在 或 观测到地图点的地图不是当前地图 或 观测到地图点的关键帧是被优化掉的关键帧// 也就是说 这里去除这个地图点在别的地图的观测以及在坏掉的关键帧的观测map<KeyFrame *, std::tuple<int, int>> mpObs = pMPi->GetObservations();for (map<KeyFrame *, std::tuple<int, int>>::iterator it = mpObs.begin(), end = mpObs.end(); it != end; ++it){if (!it->first || it->first->GetMap() != this || it->first->isBad()){// 删除某个关键帧对当前地图点的观测pMPi->EraseObservation(it->first, false);}}}// Saves the id of KF origins2. 保存最开始的帧的id,貌似一个map的mvpKeyFrameOrigins里面只有一个,可以验证一下// vector<unsigned long> Map::mvBackupKeyFrameOriginsIdmvBackupKeyFrameOriginsId.clear();mvBackupKeyFrameOriginsId.reserve(mvpKeyFrameOrigins.size());for (int i = 0, numEl = mvpKeyFrameOrigins.size(); i < numEl; ++i){mvBackupKeyFrameOriginsId.push_back(mvpKeyFrameOrigins[i]->mnId);}// Backup of MapPoints3. 保存一下对应的mpmvpBackupMapPoints.clear();for (MapPoint *pMPi : mspMapPoints){if (!pMPi || pMPi->isBad())continue;mvpBackupMapPoints.push_back(pMPi);pMPi->PreSave(mspKeyFrames, mspMapPoints);}// Backup of KeyFrames4. 保存一下对应的KFmvpBackupKeyFrames.clear();for (KeyFrame *pKFi : mspKeyFrames){if (!pKFi || pKFi->isBad())continue;mvpBackupKeyFrames.push_back(pKFi);pKFi->PreSave(mspKeyFrames, mspMapPoints, spCams);}保存一些idmnBackupKFinitialID = -1;if (mpKFinitial){mnBackupKFinitialID = mpKFinitial->mnId;}mnBackupKFlowerID = -1;if (mpKFlowerID){mnBackupKFlowerID = mpKFlowerID->mnId;}
}

        在这里我们预保存每张地图。

        1.遍历此张地图的所有地图点,先剔除一下无效观测:当观测到地图点的关键帧不存在 或 观测到地图点的地图不是当前地图 或 观测到地图点的关键帧是被优化掉的关键帧时,我们将这个地图点在相应帧的观测删除。也就是说这里去除这个地图点在别的地图的观测以及在坏掉的关键帧的观测

        2.第二步保存这张最开始的帧的id 保存到变量mvBackupKeyFrameOriginsId中。

        3.第三步再次遍历这个地图的所有地图点,将所有地图点保存到mvpBackupMapPoints中。        

        4.第四步预保存地图点。

        5.第五步预保存关键帧。

1.2.3 预保存地图点 MapPoint::PreSave

/*** @brief 预保存* @param spKF 地图中所有关键帧* @param spMP 地图中所有三维点*/
void MapPoint::PreSave(set<KeyFrame*>& spKF,set<MapPoint*>& spMP)
{// 1. 备份替代的MP idmBackupReplacedId = -1;if(mpReplaced && spMP.find(mpReplaced) != spMP.end())mBackupReplacedId = mpReplaced->mnId;// 2. 备份观测mBackupObservationsId1.clear();mBackupObservationsId2.clear();// Save the id and position in each KF who view itstd::vector<KeyFrame*> erase_kfs;for(std::map<KeyFrame*,std::tuple<int,int> >::const_iterator it = mObservations.begin(), end = mObservations.end(); it != end; ++it){KeyFrame* pKFi = it->first;if(spKF.find(pKFi) != spKF.end()){mBackupObservationsId1[it->first->mnId] = get<0>(it->second);mBackupObservationsId2[it->first->mnId] = get<1>(it->second);}else{erase_kfs.push_back(pKFi); }}for (auto pKFi : erase_kfs)EraseObservation(pKFi, false);// Save the id of the reference KF// 3. 备份参考关键帧IDif(spKF.find(mpRefKF) != spKF.end()){mBackupRefKFId = mpRefKF->mnId;}
}

        1.备份替换的地图点:如果这个地图点被替换了,我们在地图点集合spMP中寻找这个地图点所对应的ID赋值给mBackupReplacedId

        2.备份预测:mObservations存放所有地图点的预测(被哪一个关键帧观测到 + 在该帧特征点的索引),因此遍历该地图点可以看到的所有关键帧。

        如果这个关键帧存在(没有在优化节点被优化):备份地图点的观测到mBackupObservationsId1、mBackupObservationsId2变量中。(map<unsigned long, int> MapPoint::mBackupObservationsId1)

        如果这个关键帧不存在,删除掉关键帧对地图点的观测。

        3.备份第一次观测到该地图点的关键帧的ID:mBackupRefKFId

1.2.4 预保存关键帧KeyFrame::PreSave

void KeyFrame::PreSave(set<KeyFrame *> &spKF, set<MapPoint *> &spMP, set<GeometricCamera *> &spCam)
{1.预保存地图点的IDmvBackupMapPointsId.clear();mvBackupMapPointsId.reserve(N);for (int i = 0; i < N; ++i){if (mvpMapPoints[i] && spMP.find(mvpMapPoints[i]) != spMP.end()) // Checks if the element is not nullmvBackupMapPointsId.push_back(mvpMapPoints[i]->mnId);else // If the element is null his value is -1 because all the id are positivesmvBackupMapPointsId.push_back(-1);}2.预保存本质图mBackupConnectedKeyFrameIdWeights.clear();for (std::map<KeyFrame *, int>::const_iterator it = mConnectedKeyFrameWeights.begin(), end = mConnectedKeyFrameWeights.end(); it != end; ++it){if (spKF.find(it->first) != spKF.end())mBackupConnectedKeyFrameIdWeights[it->first->mnId] = it->second;}3.预保存父母节点的IDmBackupParentId = -1;if (mpParent && spKF.find(mpParent) != spKF.end())mBackupParentId = mpParent->mnId;4.预保存孩子节点的IDmvBackupChildrensId.clear();mvBackupChildrensId.reserve(mspChildrens.size());for (KeyFrame *pKFi : mspChildrens){if (spKF.find(pKFi) != spKF.end())mvBackupChildrensId.push_back(pKFi->mnId);}5.预保存回环节点的IDmvBackupLoopEdgesId.clear();mvBackupLoopEdgesId.reserve(mspLoopEdges.size());for (KeyFrame *pKFi : mspLoopEdges){if (spKF.find(pKFi) != spKF.end())mvBackupLoopEdgesId.push_back(pKFi->mnId);}6.mspMergeEdgesmvBackupMergeEdgesId.clear();mvBackupMergeEdgesId.reserve(mspMergeEdges.size());for (KeyFrame *pKFi : mspMergeEdges){if (spKF.find(pKFi) != spKF.end())mvBackupMergeEdgesId.push_back(pKFi->mnId);}7.预保存相机信息mnBackupIdCamera = -1;if (mpCamera && spCam.find(mpCamera) != spCam.end())mnBackupIdCamera = mpCamera->GetId();mnBackupIdCamera2 = -1;if (mpCamera2 && spCam.find(mpCamera2) != spCam.end())mnBackupIdCamera2 = mpCamera2->GetId();8.预保存IMU信息mBackupPrevKFId = -1;if (mPrevKF && spKF.find(mPrevKF) != spKF.end())mBackupPrevKFId = mPrevKF->mnId;mBackupNextKFId = -1;if (mNextKF && spKF.find(mNextKF) != spKF.end())mBackupNextKFId = mNextKF->mnId;if (mpImuPreintegrated)mBackupImuPreintegrated.CopyFrom(mpImuPreintegrated);
}

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

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

ssh远程连接阿里云CentOS:修改为密码登录

文章目录 控制台添加密钥下载Xshell修改密码连接重启服务&#xff1a;重复Xshell使用密码登录 控制台添加密钥 会下载一个pem文件 下载Xshell 新建 通过public key登录 修改密码连接 passwd root然后输入你想要设置的密码两遍 cd /etc/ssh/ vi sshd_config将PasswordAuth…

[论文精读]利用大语言模型对扩散模型进行自我修正

本博客是一篇最新论文的精读&#xff0c;论文为UC伯克利大学相关研究者新近(2023.11.27)在arxiv上上传的《Self-correcting LLM-controlled Diffusion Models》 。 内容提要: 现有的基于扩散的文本到图像生成模型在生成与复杂提示精确对齐的图像时仍然存在困难,尤其是需要数值和…

生成对抗网络(DCGAN)手写数字生成

文章目录 一、前言二、前期工作1. 设置GPU&#xff08;如果使用的是CPU可以忽略这步&#xff09; 二、什么是生成对抗网络1. 简单介绍2. 应用领域 三、创建模型1. 生成器2. 判别器 四、定义损失函数和优化器1. 判别器损失2. 生成器损失 五、定义训练循环六、训练模型七、创建 G…

“前端八股文背诵版“,终于整理完了,堪称最强!

随着互联网的快速发展&#xff0c;前端开发领域成为了IT行业中的热门领域之一。很多求职者都希望能够进入这个领域&#xff0c;但是面对着如此激烈的竞争&#xff0c;很多人都感到无从下手。为了帮助大家更好地掌握前端开发的相关知识&#xff0c;小编整理了一份前端面试题合集…

单片机怎么实现真正的多线程?

单片机怎么实现真正的多线程? 不考虑多核情况时&#xff0c;CPU在一个时间点只能做一件事&#xff0c;因为切换的速度快所以看起来好像是同时执行多个线程而已。 实际上就是用定时器来做时基&#xff0c;以时间片的方式分别执行来实现的&#xff0c;只不过实现起来细节比较复…

网络安全应急响应-Server2228(环境+解析)

网络安全应急响应 任务环境说明: 服务器场景:Server2228(开放链接)用户名:root,密码:p@ssw0rd123

【代码】考虑差异性充电模式的电动汽车充放电优化调度matlab-yalmip-cplex/gurobi

程序名称&#xff1a;考虑差异性充电模式的电动汽车充放电优化调度 实现平台&#xff1a;matlab-yalmip-cplex/gurobi 代码简介&#xff1a;提出了一种微电网中电动汽车的协调充电调度方法&#xff0c;以将负荷需求从高峰期转移到低谷期。在所提出的方法中&#xff0c;基于充…

RHEL8.9 静默安装Oracle19C

RHEL8.9 静默安装Oracle19C 甘肃圆角网络科技开发有限公司 说明(GUI)&#xff1a;  1.实际业务场景中&#xff0c;Linux环境一般情况下是没有GUI的。没有GUI并不意味着没有安装图形界面。可能在部署Linux操作系统环境的时候安装了桌面环境&#xff0c;只是启动的时候设置了启动…

英国人工智能初创公司Stability AI面临卖身压力;深度学习中的检索增强生成简介

&#x1f989; AI新闻 &#x1f680; 英国人工智能初创公司Stability AI面临卖身压力 摘要&#xff1a;多位知情人士透露&#xff0c;英国人工智能初创公司Stability AI正寻求出售公司&#xff0c;因为投资者对其财务状况的压力越来越大。管理层最近几周一直将自己标榜为收购…

MapInfo Pro错误提示:This operation requires elevated privileges……

尝试删除MapInfo Pro时出现错误“此操作需要提升权限。将产品DVD/CD插入媒体播放器并双击setup.exe文件重新启动。此安装程序现在将中止。”。 原因&#xff1a; 这可能是由于权限问题。 解决方式&#xff1a; 1.如果MapInfo Pro setup.exe可用&#xff0c;请执行以下步骤&…

笔记64:Bahdanau 注意力

本地笔记地址&#xff1a;D:\work_file\&#xff08;4&#xff09;DeepLearning_Learning\03_个人笔记\3.循环神经网络\第10章&#xff1a;动手学深度学习~注意力机制 a a a a a a a a a a a

FIORI /N/UI2/FLP 始终在IE浏览器中打开 无法在缺省浏览器中打开

在使用/N/UI2/FLP 打开fiori 启动面板的时候&#xff0c;总是会在IE浏览器中打开&#xff0c;无法在缺省浏览器打开 并且URL中包含myssocntl 无法正常打开 启动面板 这种情况可以取消激活ICF节点/sap/public/myssocntl

spring boot 3.2.0 idea从零开始

spring boot 3.2.0 idea从零开始 最新的spring initilizer 不再支持低版本java&#xff0c;只能选择17、21 。 我也被迫尝试下最新版本的java。 jdk下载地址 自定义好artifact和group之后点击下一步。 在这里选择需要的组件&#xff0c;我准备做web项目所以只选择spring web …

word模板导出word文件

前期准备工作word模板 右键字段如果无编辑域 ctrlF9 一下&#xff0c;然后再右键 wps 直接 ctrlF9 会变成编辑域 pom.xml所需依赖 <dependencies> <!--word 依赖--> <dependency><groupId>fr.opensagres.xdocreport</groupId><artifactId…

vim工具以及如何给用户加上sudo的权限

Linux开发工具之vim以及如何给用户配置sudo的权限文件的操作 1.vim概念的介绍 2.vim的多模式的介绍 3.vim的命令模式与低行模式的相关指令操作 4.vim如何配置 5.如何给普通用户配置sudo的权限 本文开始~~~~ 1. vim概念的介绍 vim是一款多模式的文本编辑器&#xff0c;简单…

C语言-指针讲解(4)

在上一篇博客中&#xff1a; C语言-指针讲解(3) 我们给大家介绍了指针进阶的用法 让下面我们来回顾一下讲了什么吧&#xff1a; 1.字符指针变量类型以及用法 2.数组指针本质上是一个指针&#xff0c;里面存放数组的地址。而指针数组本质上是个数组&#xff0c;里面存放的是指针…

iOS系统上待办事项提醒软件哪个好

在这个快节奏的生活中&#xff0c;各种待办事项充斥了我们的日常工作和生活&#xff0c;尤其对于像我这样的iPhone用户而言&#xff0c;一款能够在iOS系统上快速和准确记录和提醒待办事项的软件&#xff0c;显得至关重要。 正如前几天&#xff0c;我正沉浸在工作中的时突然被领…

【算法】Rabin-Karp 算法

目录 1.概述2.代码实现3.应用 更多数据结构与算法的相关知识可以查看数据结构与算法这一专栏。 有关字符串模式匹配的其它算法&#xff1a; 【算法】Brute-Force 算法 【算法】KMP 算法 1.概述 &#xff08;1&#xff09;Rabin-Karp 算法是由 Richard M. Karp 和 Michael O. R…

时间序列预测 — LSTM实现多变量多步负荷预测(Keras)

目录 1 数据处理 1.1 数据集简介 1.2 数据集处理 2 模型训练与预测 2.1 模型训练 2.2 模型多步预测 2.3 结果可视化 1 数据处理 1.1 数据集简介 实验数据集采用数据集6&#xff1a;澳大利亚电力负荷与价格预测数据&#xff08;下载链接&#xff09;&#xff0c;包括数…

2023年中国金融租赁行业研究报告

第一章 行业概况 1.1 定义 金融租赁是一种融资方式&#xff0c;其中租赁公司&#xff08;出租人&#xff09;为企业&#xff08;承租人&#xff09;购买所需设备&#xff0c;并在租赁期内由承租人使用。承租人负责支付租金&#xff0c;租赁期满后有权选择退租、续租或购买设备…