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

2023-11-29 09:04
文章标签 保存 二进制 地图 orb slam3 点到 关键帧

本文主要是介绍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);
}

这篇关于1.ORB-SLAM3中如何保存多地图、关键帧、地图点到二进制文件中的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/432234。

相关文章

无人叉车3d激光slam多房间建图定位异常处理方案-墙体画线地图切分方案

无人叉车3d激光slam多房间建图定位异常处理方案-墙体画线地图切分方案

墙体画线地图切分方案 针对问题：墙体两侧特征混淆误匹配，导致建图和定位偏差，表现为过门跳变、外月台走歪等 ·解决思路：预期的根治方案IGICP需要较长时间完成上线，先使用切分地图的工程化方案，即墙体两侧切分为不同地图，在某一侧只使用该侧地图进行定位 方案思路 切分原理：切分地图基于关键帧位置，而非点云。 理论基础：光照是直线的，一帧点云必定只能照射到墙的一侧，无法同时照到两侧实践考虑：关
阅读更多...
全英文地图/天地图和谷歌瓦片地图杂交/设备分布和轨迹回放/无需翻墙离线使用

全英文地图/天地图和谷歌瓦片地图杂交/设备分布和轨迹回放/无需翻墙离线使用

一、前言说明 随着风云局势的剧烈变化，对我们搞软件开发的人员来说，影响也是越发明显，比如之前对美对欧的软件居多，现在慢慢的变成了对大鹅和中东以及非洲的居多，这两年明显问有没有俄语或者阿拉伯语的输入法的增多，这要是放在2019年以前，一年也遇不到一个人问这种需求场景的。 地图应用这块也是，之前的应用主要在国内，现在慢慢的多了一些外国的应用场景，这就遇到一个大问题，我们平时主要开发用的都是国内的地
阅读更多...
通信工程学习：什么是2ASK/BASK二进制振幅键控

通信工程学习：什么是2ASK/BASK二进制振幅键控

2ASK/BASK：二进制振幅键控         2ASK/BASK二进制振幅键控是一种数字调制技术，其全称是二进制振幅键控（Binary Amplitude Shift Keying）。该技术通过改变载波的振幅来传递二进制数字信息，而载波的频率和相位则保持不变。以下是关于2ASK/BASK二进制振幅键控的详细解释： 一、2ASK/BASK二进制振幅键控的基本原理 1、振幅键控：
阅读更多...
下载/保存/读取 文件，并转成流输出

下载/保存/读取 文件，并转成流输出

最近对文件的操作又熟悉了下；现在记载下来：学习在于 坚持！！！不以细小而不为。 实现的是：文件的下载、文件的保存到SD卡、文件的读取输出String 类型、最后是文件转换成流输出；一整套够用了； 重点： 1:   操作网络要记得开线程； 2：更新网络获取的数据 切记用Handler机制； 3：注意代码的可读性(这里面只是保存到SD卡，在项目中切记要对SD卡的有无做判断，然后再获取路径！)
阅读更多...
FFmpeg系列-视频解码后保存帧图片为ppm

FFmpeg系列-视频解码后保存帧图片为ppm

在正常开发中遇到花屏时怎么处理呢？可以把解码后的数据直接保存成帧图片保存起来，然后直接看图片有没有花屏来排除是否是显示的问题，如果花屏，则代表显示无问题，如果图片中没有花屏，则可以往显示的方向去排查了。 void saveFrame(AVFrame* pFrame, int width, int height, int iFrame){FILE *pFile;char szFilename[
阅读更多...
1 模拟——67. 二进制求和

1 模拟——67. 二进制求和

1 模拟 67. 二进制求和 给你两个二进制字符串 a 和 b ，以二进制字符串的形式返回它们的和。 示例 1：输入:a = "11", b = "1"输出："100"示例 2：输入：a = "1010", b = "1011"输出："10101" 算法设计 可以从低位到高位（从后向前）计算，用一个变量carry记录进位，如果有字符没处理完或者有进位，则循环处理。两个字符串对
阅读更多...
Imageview在百度地图中实现点击事件

Imageview在百度地图中实现点击事件

1.首先第一步，需要声明的全局有关类的引用 private BMapManager mBMapMan; private MapView mMapView; private MapController mMapController; private RadioGroup radiogroup; private RadioButton normalview; private RadioBu
阅读更多...
MMO地图传送

MMO地图传送

本篇由以下四个点讲解： 创建传送点 传送点配置 编辑器扩展：传送点数据生成 传送协议与实现 创建传送点 建碰撞器触发 //位置归零 建一个传送门cube放到要传送的位置（这个teleporter1是传出的区域 这是从另一张地图传入时的传送门 创建一个脚本TeleporterObject给每个传送cube都绑上脚本 通过脚本，让传送门在编辑器下面还能绘制出来
阅读更多...
html记账本改写:数据重新布局，更好用了，没有localStorage保存版本

html记账本改写:数据重新布局，更好用了，没有localStorage保存版本

<!DOCTYPE html><html lang="zh-CN"><head><meta charset="UTF-8"><title>htm记账本</title><style>table {user-select: none;/* width: 100%; */border-collapse: collapse;}table,th,td {border: 1px solid bla
阅读更多...
Leetcode67---二进制求和

Leetcode67---二进制求和

https://leetcode.cn/problems/add-binary/description/ 给出的两个二进制，我们可以从最后开始往前运算。 给当前短的一位前面补充0即可。 class Solution {public String addBinary(String a, String b) {//给的就是二进制字符串 最后一位开始遍历 如果没有就补充0？StringBuil
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2