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。

相关文章

二进制文件转化成文本文件

二进制文件转化成文本文件

文章中如果有写错、表述不明、有疑问或者需要扩展的知识，欢迎留言或者私信~   1.区别 如果一个文件说是文本文件，使用任何一种文本编辑器打开可以展现出人类可读信息字符，因为编码都符合某种编码方式，如ASCII、UTF8、GB2312等等（在文件头可以读出来是什么编码方式，然后文本编辑器再按照规则去读取翻译成对应的字符，展示给我们的就是可读的了）。（关于编码方式不了解可以看这一篇） 如果一
阅读更多...
JAVA读取MongoDB中的二进制图片并显示在页面上

JAVA读取MongoDB中的二进制图片并显示在页面上

1：Jsp页面： <td><img src="${ctx}/mongoImg/show"></td> 2：xml配置： <?xml version="1.0" encoding="UTF-8"?><beans xmlns="http://www.springframework.org/schema/beans"xmlns:xsi="http://www.w3.org/2001
阅读更多...
uniapp H5打开地图

uniapp H5打开地图

manifest.json文件，源码视图找到H5添加下面内容 "h5" : {"sdkConfigs" : {"maps" : {"amap" : {"key" : "**********************","securityJsCode" : "****************************","serviceHost" : ""}}}} 高德开放平台 申请时选择（W
阅读更多...
把Tiled中做出的地图弄到项目中~~就是懒，为了以后直接复制写过来

把Tiled中做出的地图弄到项目中~~就是懒，为了以后直接复制写过来

1.现在.h中声明private: cocos2d::CCSprite* ninja; cocos2d::CCTMXTiledMap*  tileMap; 然后.cpp中加入tileMap = CCTMXTiledMap::create("MyTileMap.tmx"); CCTMXLayer* backLayer = tileMap->layerNamed("Tile L
阅读更多...
设置android返回键，保存和取得最高分

设置android返回键，保存和取得最高分

1.在.h中声明一些方法 virtual void keyBackClicked();           //Android返回键 bool isHaveSaveFile(); void getHighestHistoryScore(); 在.cpp中实现这个几个方法 void WelcomeLayer::keyBackClicked(
阅读更多...
剑指Offer—编程题10（二进制中1 的个数）

剑指Offer—编程题10（二进制中1 的个数）

代码如下，请在JDK7及以上版本运行： public class Test10 {/*** 请实现一个函数， 输入一个整数，输出该数二进制表示中1的个数。* 例如把9表示成二进制是1001 ，有2位是1. 因此如果输入9，该出2。** @param n 待的数字* @return 数字中二进制表表的1的数目*/public static int numberOfOne(int
阅读更多...
Android天地图常用工具

Android天地图常用工具

天地图依赖资源包，直接把地址复制到浏览器下载： http://download.tianditu.com/download/mobile/Tiandituapi3.0.2%2820190103-02%29.zip /*** 天地图工具*/public class SkyLandMap {/*** 设置中心点** @param context 上下文对象* @param mapView 地图对象
阅读更多...
将知乎专栏文章转换为 Markdown 文件保存到本地

将知乎专栏文章转换为 Markdown 文件保存到本地

一、参考内容 参考知乎文章`代码 | 将知乎专栏文章转换为 Markdown 文件保存到本地，利用代码为GitHub：https://github.com/chenluda/zhihu-download。 二、步骤 1.首先安装包flask、flask-cors、markdownify 2. 运行app.py 3.在浏览器中打开链接，并填写URL和Cookies 获取Cookies的步
阅读更多...
用Ps将PSD切片并将切片保存为透明背景的图片

用Ps将PSD切片并将切片保存为透明背景的图片

第一步：选择放大镜工具或者Ctrl++将要切片的部分放大。 第二步：选择移动工具单击要切片的部分，在右边的图层栏找到要切片的图层在文字上右键选择转换为智能对象，再右键该图层的文字选择栅格化图层。 第三步：单击选中所要切片的部分，然后Ctrl+A、Ctrl+C、Ctrl+N（背景内容选择透明）、Ctrl+V、Ctrl+S（将文件保存为PNG格式），这样就可以得到透明背景的图片了！
阅读更多...
Orcad打开原理图，将原理图中的元器件保存到自己的器件库中

Orcad打开原理图，将原理图中的元器件保存到自己的器件库中

阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2