2.ORB-SLAM3中如何从二进制文件中加载多地图、关键帧、地图点等数据结构

1 为什么保存&加载(视觉)地图

1.1 加载多地图的主函数

1.2 加载各个地图 Atlas::PostLoad

1.3 加载关键帧及地图点Map::PostLoad

1.4 恢复地图点信息 MapPoint::PostLoad

1.5 恢复关键帧信息KeyFrame::PostLoad

1 为什么保存&加载(视觉)地图

        因为我们要去做导航，导航需要先验地图。因此需要保存地图供导航使用。

        上篇博客我们讲解了如何保存地图数据，这是我们已经保存下来的文件。

        下面来为大家讲解如何加载地图数据。

1.1 加载多地图的主函数

我们看System.cc文件中的代码：

    // 查看配置文件版本，不同版本有不同处理方法cv::FileNode node = fsSettings["File.version"];if(!node.empty() && node.isString() && node.string() == "1.0"){settings_ = new Settings(strSettingsFile,mSensor);// 保存及加载地图的名字mStrLoadAtlasFromFile = settings_->atlasLoadFile();mStrSaveAtlasToFile = settings_->atlasSaveFile();cout << (*settings_) << endl;}// 我们的配置文件版本为1.0版本  LoadAtlasFromFile = ‘akm’  SaveAtlasToFile = ‘akm’// mStrLoadAtlasFromFile = ‘akm’// mStrSaveAtlasToFile = ‘akm’else{settings_ = nullptr;cv::FileNode node = fsSettings["System.LoadAtlasFromFile"];if(!node.empty() && node.isString()){mStrLoadAtlasFromFile = (string)node;}node = fsSettings["System.SaveAtlasToFile"];if(!node.empty() && node.isString()){mStrSaveAtlasToFile = (string)node;}}

我们的配置文件版本时1.0版本。

mStrSaveAtlasToFile

在setting构造函数中我们读取了下面的参数：

void Settings::readLoadAndSave(cv::FileStorage &fSettings)
{bool found;sLoadFrom_ = readParameter<string>(fSettings, "System.LoadAtlasFromFile", found, false);sSaveto_ = readParameter<string>(fSettings, "System.SaveAtlasToFile", found, false);
}

因此，mStrLoadAtlasFromFile和mStrSaveAtlasToFile变量都是字符串'akm'。

接着看这段：

    // 如果没有先验地图的话 mStrLoadAtlasFromFile = ‘akm’if(mStrLoadAtlasFromFile.empty()){//Load ORB Vocabularycout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;// 建立一个新的ORB字典mpVocabulary = new ORBVocabulary();// 读取预训练好的ORB字典并返回成功/失败标志bool bVocLoad = mpVocabulary->loadFromBinaryFile(strVocFile);// 如果加载失败，就输出错误信息if(!bVocLoad){cerr << "Wrong path to vocabulary. " << endl;cerr << "Falied to open at: " << strVocFile << endl;exit(-1);}cout << "Vocabulary loaded!" << endl << endl;//Create KeyFrame Database// Step 4 创建关键帧数据库mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);//Create the Atlas// Step 5 创建多地图，参数0表示初始化关键帧id为0cout << "Initialization of Atlas from scratch " << endl;mpAtlas = new Atlas(0);}// 如果有先验地图的话else{//Load ORB Vocabularycout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;mpVocabulary = new ORBVocabulary();bool bVocLoad = mpVocabulary->loadFromBinaryFile(strVocFile);if(!bVocLoad){cerr << "Wrong path to vocabulary. " << endl;cerr << "Falied to open at: " << strVocFile << endl;exit(-1);}cout << "Vocabulary loaded!" << endl << endl;//Create KeyFrame DatabasempKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);cout << "Load File" << endl;// Load the file with an earlier session//clock_t start = clock();cout << "Initialization of Atlas from file: " << mStrLoadAtlasFromFile << endl;加载地图bool isRead = LoadAtlas(FileType::BINARY_FILE);if(!isRead){cout << "Error to load the file, please try with other session file or vocabulary file" << endl;exit(-1);}//mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);//cout << "KF in DB: " << mpKeyFrameDatabase->mnNumKFs << "; words: " << mpKeyFrameDatabase->mnNumWords << endl;loadedAtlas = true;//如果选择杶定位模式 先用我们写的方式加载地图// mode2 pure locationif(mode2){mpAtlas->LoadMap();}elsempAtlas->CreateNewMap();//clock_t timeElapsed = clock() - start;//unsigned msElapsed = timeElapsed / (CLOCKS_PER_SEC / 1000);//cout << "Binary file read in " << msElapsed << " ms" << endl;//usleep(10*1000*1000);}

我们有先验地图（mStrLoadAtlasFromFile不为空），因此走else语句。

1.我们首先读取ORB词典

2.利用ORB词典创建关键帧数据库

3.加载地图

这是我们这次的重头戏，看看如何加载地图：

/*** @brief 加载地图* @param type 保存类型*/
bool System::LoadAtlas(int type)
{1. 加载地图文件 这里的pathLoadFileName就是 ./ + akm + .osa// /home/liuhongwei/Desktop/final/catkin_nav/src/akm_nav/thirdparty/ORB_SLAM3_relocation_nav/akm.osastring strFileVoc, strVocChecksum;bool isRead = false;string pathLoadFileName = "./";pathLoadFileName = pathLoadFileName.append(mStrLoadAtlasFromFile);pathLoadFileName = pathLoadFileName.append(".osa");if(type == TEXT_FILE) // File text{cout << "Starting to read the save text file " << endl;std::ifstream ifs(pathLoadFileName, std::ios::binary);if(!ifs.good()){cout << "Load file not found" << endl;return false;}// strVocabularyNameboost::archive::text_iarchive ia(ifs);ia >> strFileVoc;ia >> strVocChecksum;ia >> mpAtlas;cout << "End to load the save text file " << endl;isRead = true;}// 这里我们是二值地图else if(type == BINARY_FILE) // File binary{cout << "Starting to read the save binary file"  << endl;// 这段代码是用 C++ 语言中的 ifstream 类来创建一个文件输入流对象。// 它打开一个文件以供读取，并使用 `std::ios::binary` 标志表示以二进制模式打开文件，而不是文本模式。// 在这个例子中，`pathLoadFileName` 是文件的路径和名称。std::ifstream ifs(pathLoadFileName, std::ios::binary);//这段代码首先检查文件输入流 ifs 是否成功打开并准备好进行读取。// 如果文件未能成功打开（即 !ifs.good()），它会输出错误信息 "Load file not found"，然后返回 false，表示加载文件失败。if(!ifs.good()){cout << "Load file not found" << endl;return false;}// 如果文件成功打开，它使用 Boost 库中的 boost::archive::binary_iarchive 对象 ia 对打开的文件进行反序列化操作。boost::archive::binary_iarchive ia(ifs);// 通过 ia 从文件中读取了 strFileVoc、strVocChecksum 和 mpAtlas。ia >> strFileVoc;ia >> strVocChecksum;ia >> mpAtlas;cout << "End to load the save binary file" << endl;isRead = true;}2. 如果加载成功if(isRead){//Check if the vocabulary is the same// 校验词典是否一样string strInputVocabularyChecksum = CalculateCheckSum(mStrVocabularyFilePath,TEXT_FILE);if(strInputVocabularyChecksum.compare(strVocChecksum) != 0){cout << "The vocabulary load isn't the same which the load session was created " << endl;cout << "-Vocabulary name: " << strFileVoc << endl;return false; // Both are differents}// 加载对应数据mpAtlas->SetKeyFrameDababase(mpKeyFrameDatabase);mpAtlas->SetORBVocabulary(mpVocabulary);mpAtlas->PostLoad();return true;}return false;
}

1.首先我们读取我们保存的二进制文件：

pathLoadFileName = ./akm.osa

2.从二进制文件中读取地图：

我们回忆一下存储地图的过程：

我们存储了ORB词典的位置、checksum校验和以及多地图数据库。

因此我们在读取时候也是按这三个顺序读取。

这个数据结构包含以下内容：

它就是一个指针保存了所有地图和关键帧（Map structure that stores the pointers to all KeyFrames and MapPoints.）

3.如果读取成功，我们设置关键帧的数据库为System.cc中初始化的关键帧数据库，ORB词典为System.cc初始化的ORB词典。

执行加载数据函数mpAtlas->PostLoad(); 1.2-1.5节说明

如果执行成功，那么我们已经恢复了所有的地图了，开始下一步的工作

1.2 加载各个地图 Atlas::PostLoad

/*** @brief 后加载，意思是读取地图文件后加载各个信息*/
void Atlas::PostLoad()
{1. 读取当前所有相机map<unsigned int, GeometricCamera *> mpCams;for (GeometricCamera *pCam : mvpCameras){mpCams[pCam->GetId()] = pCam;}mspMaps.clear();unsigned long int numKF = 0, numMP = 0;2. 加载各个地图for (Map *pMi : mvpBackupMaps){mspMaps.insert(pMi);pMi->PostLoad(mpKeyFrameDB, mpORBVocabulary, mpCams);numKF += pMi->GetAllKeyFrames().size();numMP += pMi->GetAllMapPoints().size();}//mvpBackupMaps.clear();
}

我们先来看读取到Atlas类中的变量：

protected:// 地图集合std::set<Map*> mspMaps;// 坏的地图集合std::set<Map*> mspBadMaps;// Its necessary change the container from set to vector because libboost 1.58 and Ubuntu 16.04 have an error with this cointainer// 备份地图集合std::vector<Map*> mvpBackupMaps;// 当前地图Map* mpCurrentMap;// 相机模型std::vector<GeometricCamera*> mvpCameras;unsigned long int mnLastInitKFidMap;Viewer* mpViewer;bool mHasViewer;// Class references for the map reconstruction from the save fileKeyFrameDatabase* mpKeyFrameDB;ORBVocabulary* mpORBVocabulary;// Mutexstd::mutex mMutexAtlas;

1.首先把所有的相机模型读取到变量map<unsigned int, GeometricCamera *> mpCams里面。（第几个相机，对应的相机抽象模型）

2.从备份地图mvpBackupMaps中加载地图：

将地图存储进地图数据结构mspMaps（保存所有地图的数据结构），用numKF和numMP获取每个地图的关键帧和地图点数量。（dubug信息貌似，这两个临时变量没什么用呀）。

1.3 加载关键帧及地图点Map::PostLoad

我们这个函数是恢复每个地图都被调用一次，因此是恢复一张地图的关键帧及地图点。

    for (Map *pMi : mvpBackupMaps){mspMaps.insert(pMi);pMi->PostLoad(mpKeyFrameDB, mpORBVocabulary, mpCams);numKF += pMi->GetAllKeyFrames().size();numMP += pMi->GetAllMapPoints().size();}

我们来看代码：

/** 后加载，也就是把备份的变量恢复到正常变量中* @param spCams 相机*/
void Map::PostLoad(KeyFrameDatabase *pKFDB, ORBVocabulary *pORBVoc /*, map<long unsigned int, KeyFrame*>& mpKeyFrameId*/, map<unsigned int, GeometricCamera *> &mpCams)
{std::copy(mvpBackupMapPoints.begin(), mvpBackupMapPoints.end(), std::inserter(mspMapPoints, mspMapPoints.begin()));std::copy(mvpBackupKeyFrames.begin(), mvpBackupKeyFrames.end(), std::inserter(mspKeyFrames, mspKeyFrames.begin()));1. 恢复map中的地图点，注意此时mp中只恢复了保存的量map<long unsigned int, MapPoint *> mpMapPointId;for (MapPoint *pMPi : mspMapPoints){if (!pMPi || pMPi->isBad())continue;pMPi->UpdateMap(this);mpMapPointId[pMPi->mnId] = pMPi;}2. 恢复map中的kf，注意此时kf中只恢复了保存的量map<long unsigned int, KeyFrame *> mpKeyFrameId;for (KeyFrame *pKFi : mspKeyFrames){if (!pKFi || pKFi->isBad())continue;pKFi->UpdateMap(this);pKFi->SetORBVocabulary(pORBVoc);pKFi->SetKeyFrameDatabase(pKFDB);mpKeyFrameId[pKFi->mnId] = pKFi;}// References reconstruction between different instances3. 使用mp中的备份变量恢复正常变量for (MapPoint *pMPi : mspMapPoints){if (!pMPi || pMPi->isBad())continue;pMPi->PostLoad(mpKeyFrameId, mpMapPointId);}4. 使用kf中的备份变量恢复正常变量for (KeyFrame *pKFi : mspKeyFrames){if (!pKFi || pKFi->isBad())continue;pKFi->PostLoad(mpKeyFrameId, mpMapPointId, mpCams);pKFDB->add(pKFi);}5. 恢复IDif (mnBackupKFinitialID != -1){mpKFinitial = mpKeyFrameId[mnBackupKFinitialID];}if (mnBackupKFlowerID != -1){mpKFlowerID = mpKeyFrameId[mnBackupKFlowerID];}mvpKeyFrameOrigins.clear();mvpKeyFrameOrigins.reserve(mvBackupKeyFrameOriginsId.size());for (int i = 0; i < mvBackupKeyFrameOriginsId.size(); ++i){mvpKeyFrameOrigins.push_back(mpKeyFrameId[mvBackupKeyFrameOriginsId[i]]);}mvpBackupMapPoints.clear();
}

1.首先将备份的所有关键帧mvpBackupKeyFrames

信息和所有的地图点mvpBackupMapPoints提取出来，mspMapPoints、mspKeyFrames存储了运行状态的所有地图点和关键帧。

2.缓存所有地图点mspMapPoints：用mpMapPointId数据结构保存（地图点ID、地图点的指针）并更新这个地图点对本地图可视。

     1. 恢复map中的地图点，注意此时mp中只恢复了保存的量map<long unsigned int, MapPoint *> mpMapPointId;for (MapPoint *pMPi : mspMapPoints){if (!pMPi || pMPi->isBad())continue;pMPi->UpdateMap(this);mpMapPointId[pMPi->mnId] = pMPi;}

void MapPoint::UpdateMap(Map* pMap)
{unique_lock<mutex> lock(mMutexMap);mpMap = pMap;
}

3.缓存所有关键帧：用mpKeyFrameId数据结构保存（帧ID、帧指针）。

     2. 恢复map中的kf，注意此时kf中只恢复了保存的量map<long unsigned int, KeyFrame *> mpKeyFrameId;for (KeyFrame *pKFi : mspKeyFrames){if (!pKFi || pKFi->isBad())continue;pKFi->UpdateMap(this);pKFi->SetORBVocabulary(pORBVoc);pKFi->SetKeyFrameDatabase(pKFDB);mpKeyFrameId[pKFi->mnId] = pKFi;}

恢复地图对该关键帧的观测：

void KeyFrame::UpdateMap(Map *pMap)
{unique_lock<mutex> lock(mMutexMap);mpMap = pMap;
}

设置该帧的ORB词典（必须设置的，每一帧的构造函数中有）

void KeyFrame::SetORBVocabulary(ORBVocabulary *pORBVoc)
{mpORBvocabulary = pORBVoc;
}

设置该帧的关键帧数据库：

void KeyFrame::SetKeyFrameDatabase(KeyFrameDatabase *pKFDB)
{mpKeyFrameDB = pKFDB;
}

4.恢复本地图地图点信息：1.4节有详细介绍

     3. 使用mp中的备份变量恢复正常变量for (MapPoint *pMPi : mspMapPoints){if (!pMPi || pMPi->isBad())continue;pMPi->PostLoad(mpKeyFrameId, mpMapPointId);}

5.恢复本地图关键帧信息：1.5节有详细介绍

     4. 使用kf中的备份变量恢复正常变量for (KeyFrame *pKFi : mspKeyFrames){if (!pKFi || pKFi->isBad())continue;pKFi->PostLoad(mpKeyFrameId, mpMapPointId, mpCams);pKFDB->add(pKFi);}

1.4 恢复地图点信息 MapPoint::PostLoad

/*** @brief 后加载*/
void MapPoint::PostLoad(map<long unsigned int, KeyFrame*>& mpKFid, map<long unsigned int, MapPoint*>& mpMPid)
{// 1. 根据保存的ID加载参考关键帧与替代点mpRefKF = mpKFid[mBackupRefKFId];if(!mpRefKF){cout << "ERROR: MP without KF reference " << mBackupRefKFId << "; Num obs: " << nObs << endl;}mpReplaced = static_cast<MapPoint*>(NULL);if(mBackupReplacedId>=0){map<long unsigned int, MapPoint*>::iterator it = mpMPid.find(mBackupReplacedId);if (it != mpMPid.end())mpReplaced = it->second;}// 2. 加载观测mObservations.clear();for(map<long unsigned int, int>::const_iterator it = mBackupObservationsId1.begin(), end = mBackupObservationsId1.end(); it != end; ++it){KeyFrame* pKFi = mpKFid[it->first];map<long unsigned int, int>::const_iterator it2 = mBackupObservationsId2.find(it->first);std::tuple<int, int> indexes = tuple<int,int>(it->second,it2->second);if(pKFi){mObservations[pKFi] = indexes;}}mBackupObservationsId1.clear();mBackupObservationsId2.clear();
}

1.首先恢复地图点的参考关键帧与替代点：回忆ORB-SLAM2/3算法，一个地图的参考关键帧是建立它的关键帧。

2.加载观测：mObservations存储地图点可以被哪一帧观测，在该帧的序号是多少。

1.5 恢复关键帧信息KeyFrame::PostLoad

主要是恢复关键帧位姿、地图点、图连接，这里在上篇博客说明了，不再阐述：

void KeyFrame::PostLoad(map<long unsigned int, KeyFrame *> &mpKFid, map<long unsigned int, MapPoint *> &mpMPid, map<unsigned int, GeometricCamera *> &mpCamId)
{// Rebuild the empty variables1.恢复关键帧位姿SetPose(mTcw);mTrl = mTlr.inverse();// Reference reconstruction2.恢复关键帧地图点mvpMapPoints.clear();mvpMapPoints.resize(N);for (int i = 0; i < N; ++i){if (mvBackupMapPointsId[i] != -1)mvpMapPoints[i] = mpMPid[mvBackupMapPointsId[i]];elsemvpMapPoints[i] = static_cast<MapPoint *>(NULL);}// Conected KeyFrames with him weightmConnectedKeyFrameWeights.clear();for (map<long unsigned int, int>::const_iterator it = mBackupConnectedKeyFrameIdWeights.begin(), end = mBackupConnectedKeyFrameIdWeights.end();it != end; ++it){KeyFrame *pKFi = mpKFid[it->first];mConnectedKeyFrameWeights[pKFi] = it->second;}// Restore parent KeyFrameif (mBackupParentId >= 0)mpParent = mpKFid[mBackupParentId];// KeyFrame childrensmspChildrens.clear();for (vector<long unsigned int>::const_iterator it = mvBackupChildrensId.begin(), end = mvBackupChildrensId.end(); it != end; ++it){mspChildrens.insert(mpKFid[*it]);}// Loop edge KeyFramemspLoopEdges.clear();for (vector<long unsigned int>::const_iterator it = mvBackupLoopEdgesId.begin(), end = mvBackupLoopEdgesId.end(); it != end; ++it){mspLoopEdges.insert(mpKFid[*it]);}// Merge edge KeyFramemspMergeEdges.clear();for (vector<long unsigned int>::const_iterator it = mvBackupMergeEdgesId.begin(), end = mvBackupMergeEdgesId.end(); it != end; ++it){mspMergeEdges.insert(mpKFid[*it]);}// Camera dataif (mnBackupIdCamera >= 0){mpCamera = mpCamId[mnBackupIdCamera];}else{cout << "ERROR: There is not a main camera in KF " << mnId << endl;}if (mnBackupIdCamera2 >= 0){mpCamera2 = mpCamId[mnBackupIdCamera2];}// Inertial dataif (mBackupPrevKFId != -1){mPrevKF = mpKFid[mBackupPrevKFId];}if (mBackupNextKFId != -1){mNextKF = mpKFid[mBackupNextKFId];}mpImuPreintegrated = &mBackupImuPreintegrated;// Remove all backup containermvBackupMapPointsId.clear();mBackupConnectedKeyFrameIdWeights.clear();mvBackupChildrensId.clear();mvBackupLoopEdgesId.clear();UpdateBestCovisibles();
}