目录
1.4 恢复地图点信息 MapPoint::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 Vocabulary cout << 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为0 cout << "Initialization of Atlas from scratch " << endl; mpAtlas = new Atlas(0); } // 如果有先验地图的话 else { //Load ORB Vocabulary cout << 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 Database mpKeyFrameDatabase = 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 location if(mode2) { mpAtlas->LoadMap(); } else mpAtlas->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.osa string 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; } // strVocabularyName boost::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 file KeyFrameDatabase* mpKeyFrameDB; ORBVocabulary* mpORBVocabulary; // Mutex std::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 instances 3. 使用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. 恢复ID if (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 variables 1.恢复关键帧位姿 SetPose(mTcw); mTrl = mTlr.inverse(); // Reference reconstruction 2.恢复关键帧地图点 mvpMapPoints.clear(); mvpMapPoints.resize(N); for (int i = 0; i < N; ++i) { if (mvBackupMapPointsId[i] != -1) mvpMapPoints[i] = mpMPid[mvBackupMapPointsId[i]]; else mvpMapPoints[i] = static_cast<MapPoint *>(NULL); } // Conected KeyFrames with him weight mConnectedKeyFrameWeights.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 KeyFrame if (mBackupParentId >= 0) mpParent = mpKFid[mBackupParentId]; // KeyFrame childrens mspChildrens.clear(); for (vector<long unsigned int>::const_iterator it = mvBackupChildrensId.begin(), end = mvBackupChildrensId.end(); it != end; ++it) { mspChildrens.insert(mpKFid[*it]); } // Loop edge KeyFrame mspLoopEdges.clear(); for (vector<long unsigned int>::const_iterator it = mvBackupLoopEdgesId.begin(), end = mvBackupLoopEdgesId.end(); it != end; ++it) { mspLoopEdges.insert(mpKFid[*it]); } // Merge edge KeyFrame mspMergeEdges.clear(); for (vector<long unsigned int>::const_iterator it = mvBackupMergeEdgesId.begin(), end = mvBackupMergeEdgesId.end(); it != end; ++it) { mspMergeEdges.insert(mpKFid[*it]); } // Camera data if (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 data if (mBackupPrevKFId != -1) { mPrevKF = mpKFid[mBackupPrevKFId]; } if (mBackupNextKFId != -1) { mNextKF = mpKFid[mBackupNextKFId]; } mpImuPreintegrated = &mBackupImuPreintegrated; // Remove all backup container mvBackupMapPointsId.clear(); mBackupConnectedKeyFrameIdWeights.clear(); mvBackupChildrensId.clear(); mvBackupLoopEdgesId.clear(); UpdateBestCovisibles(); }
771
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
