（一次性搞定）ORB_SLAM2地圖保存與加載

本文記錄了ORB_SLAM2中地圖保存與加載的過程。
參考博客：
https://blog.csdn.net/qq_34254510/article/details/79969046
http://www.cnblogs.com/mafuqiang/p/6972841.html
https://blog.csdn.net/felaim/article/details/79667635

1、地圖保存

地圖保存過程比較簡單，只需要按照參考一步步修改源碼即可。接下來我們一起來走一遍這個過程。

1.1 地圖元素分析

所謂地圖保存，就是保存地圖“Map”中的各個元素，以及它們之間的關系，凡是跟蹤過程中需要用到的東西自然也就是需要保存的對象。地圖主要包含關鍵幀、3D地圖點、BoW向量、共視圖、生長樹等，在跟蹤過程中有三種跟蹤模型和局部地圖跟蹤等過程，局部地圖跟蹤需要用到3D地圖點、共視關系等元素，參考幀模型需要用到關鍵幀的BoW向量，重定位需要用到BoW向量、3D點等。所以基本上述元素都需要保存。

另一方面，關鍵幀也是一個抽象的概念（一個類），我們看看具體包含什么（其實都在關鍵幀類里面了），關鍵幀是從普通幀來的，所以來了視頻幀首先需要做的就是檢測特征點，計算描述符，還有當前幀的相機位姿，作為關鍵幀之后需要有對應的ID編號，以及特征點進行三角化之后的3D地圖點等。

關于3D地圖點需要保存的就只有世界坐標了，至于其它的關聯關系可以重關鍵幀獲得。需要單獨說的是在關鍵幀類中包含了特征點和描述符，所以BoW向量是不需要保存的（也沒辦法保存），只需要在加載了關鍵幀之后利用特征描述符重新計算即可。

所以現在需要保存的東西包括關鍵幀、3D地圖點、共視圖、生長樹。

1.2 源碼修改

SLAM對地圖維護的操作均在Map.cc這個函數類中,所以要保存地圖，我們需要在這個文件中添加相應代碼。

第一步：修改Map.h頭文件

在Map.h文件中的Map類中添加如下函數：

public://保存地圖信息void Save(const string &filename);
protected://保存地圖點和關鍵幀 void SaveMapPoint(ofstream &f,MapPoint* mp);void SaveKeyFrame(ofstream &f,KeyFrame* kf);

第二步：修改Map.cc文件

在Map.cc文件中添加第一步中函數的實現。

Save()函數的實現過程：

 //保存地圖信息
void Map::Save ( const string& filename )
{//Print the information of the saving mapcerr<<"Map.cc :: Map Saving to "<<filename <<endl;ofstream f;f.open(filename.c_str(), ios_base::out|ios::binary);//Number of MapPointsunsigned long int nMapPoints = mspMapPoints.size();f.write((char*)&nMapPoints, sizeof(nMapPoints) );//Save MapPoint sequentiallyfor ( auto mp: mspMapPoints ){//Save MapPointSaveMapPoint( f, mp );// cerr << "Map.cc :: Saving map point number: " << mp->mnId << endl;}//Print The number of MapPointscerr << "Map.cc :: The number of MapPoints is :"<<mspMapPoints.size()<<endl;//Grab the index of each MapPoint, count from 0, in which we initialized mmpnMapPointsIdx  GetMapPointsIdx(); //Print the number of KeyFramescerr <<"Map.cc :: The number of KeyFrames:"<<mspKeyFrames.size()<<endl;//Number of KeyFramesunsigned long int nKeyFrames = mspKeyFrames.size();f.write((char*)&nKeyFrames, sizeof(nKeyFrames));//Save KeyFrames sequentiallyfor ( auto kf: mspKeyFrames )SaveKeyFrame( f, kf );for (auto kf:mspKeyFrames ){//Get parent of current KeyFrame and save the ID of this parentKeyFrame* parent = kf->GetParent();unsigned long int parent_id = ULONG_MAX;if ( parent )parent_id = parent->mnId;f.write((char*)&parent_id, sizeof(parent_id));//Get the size of the Connected KeyFrames of the current KeyFrames//and then save the ID and weight of the Connected KeyFramesunsigned long int nb_con = kf->GetConnectedKeyFrames().size();f.write((char*)&nb_con, sizeof(nb_con));for ( auto ckf: kf->GetConnectedKeyFrames()){int weight = kf->GetWeight(ckf);f.write((char*)&ckf->mnId, sizeof(ckf->mnId));f.write((char*)&weight, sizeof(weight));}}// Save last Frame ID// SaveFrameID(f);f.close();cerr<<"Map.cc :: Map Saving Finished!"<<endl;
}

存儲地圖點函數——SaveMapPoint()函數的實現：

void Map::SaveMapPoint( ofstream& f, MapPoint* mp)
{   //Save ID and the x,y,z coordinates of the current MapPointf.write((char*)&mp->mnId, sizeof(mp->mnId));cv::Mat mpWorldPos = mp->GetWorldPos();f.write((char*)& mpWorldPos.at<float>(0),sizeof(float));f.write((char*)& mpWorldPos.at<float>(1),sizeof(float));f.write((char*)& mpWorldPos.at<float>(2),sizeof(float));
}

存儲關鍵幀函數——SaveKeyFrame()函數的實現：

void Map::SaveKeyFrame( ofstream &f, KeyFrame* kf )
{//Save the ID and timesteps of current KeyFramef.write((char*)&kf->mnId, sizeof(kf->mnId));// cout << "saving kf->mnId = " << kf->mnId <<endl;f.write((char*)&kf->mTimeStamp, sizeof(kf->mTimeStamp));//Save the Pose Matrix of current KeyFramecv::Mat Tcw = kf->GetPose();Save the rotation matrix// for ( int i = 0; i < Tcw.rows; i ++ )// {//     for ( int j = 0; j < Tcw.cols; j ++ )//     {//         f.write((char*)&Tcw.at<float>(i,j), sizeof(float));//         //cerr<<"Tcw.at<float>("<<i<<","<<j<<"):"<<Tcw.at<float>(i,j)<<endl;//     }// }//Save the rotation matrix in Quaternionstd::vector<float> Quat = Converter::toQuaternion(Tcw);for ( int i = 0; i < 4; i ++ )f.write((char*)&Quat[i],sizeof(float));//Save the translation matrixfor ( int i = 0; i < 3; i ++ )f.write((char*)&Tcw.at<float>(i,3),sizeof(float));//Save the size of the ORB features current KeyFrame//cerr<<"kf->N:"<<kf->N<<endl;f.write((char*)&kf->N, sizeof(kf->N));//Save each ORB featuresfor( int i = 0; i < kf->N; i ++ ){cv::KeyPoint kp = kf->mvKeys[i];f.write((char*)&kp.pt.x, sizeof(kp.pt.x));f.write((char*)&kp.pt.y, sizeof(kp.pt.y));f.write((char*)&kp.size, sizeof(kp.size));f.write((char*)&kp.angle,sizeof(kp.angle));f.write((char*)&kp.response, sizeof(kp.response));f.write((char*)&kp.octave, sizeof(kp.octave));//Save the Descriptors of current ORB featuresf.write((char*)&kf->mDescriptors.cols, sizeof(kf->mDescriptors.cols)); //kf->mDescriptors.cols is always 32 here.for (int j = 0; j < kf->mDescriptors.cols; j ++ )f.write((char*)&kf->mDescriptors.at<unsigned char>(i,j), sizeof(char));//Save the index of MapPoints that corresponds to current ORB featuresunsigned long int mnIdx;MapPoint* mp = kf->GetMapPoint(i);if (mp == NULL  )mnIdx = ULONG_MAX;elsemnIdx = mmpnMapPointsIdx[mp];f.write((char*)&mnIdx, sizeof(mnIdx));}// Save BoW for relocalization.// f.write((char*)&kf->mBowVec, sizeof(kf->mBowVec));
}

Save()函數中的GetMapPointsIdx()函數用于獲取地圖點的ID號，所以需要在Map.h中定義如下成員變量：

std::map<MapPoint*,unsigned long int> mmpnMapPointsIdx;

GetMapPointsIdx()函數的實現過程為（在Map.cc文件中添加）：

void Map::GetMapPointsIdx()
{unique_lock<mutex> lock(mMutexMap);unsigned long int i = 0;for ( auto mp: mspMapPoints ){mmpnMapPointsIdx[mp] = i;i += 1;}
}

第三步：修改Converter相關文件

關于旋轉矩陣的存儲可以通過四元數或矩陣的形式存儲，如果使用四元數需要自定義一個矩陣和四元數相互轉換的函數，在Converter.cc類里面添加如下函數：

cv::Mat Converter::toCvMat(const std::vector<float>& v)
{Eigen::Quaterniond q;q.x() = v[0];q.y() = v[1];q.z() = v[2];q.w() = v[3];Eigen::Matrix<double,3,3> eigMat(q);cv::Mat M = toCvMat(eigMat);return M;
}

第四步：System文件修改

上述修改完成之后，還需要對system.h和system.cc文件進行修改，分別添加聲明和定義。
system.h文件：

void SaveMap(const string &filename); 

system.cc文件

//地圖保存
void System::SaveMap(const string &filename)
{mpMap->Save(filename);
}

1.3 測試

做完這些修改之后，在Examples文件中對應的示例程序中加入地圖存儲代碼即可實現地圖存儲功能。
如在stereo_kitti.cc文件中加入如下語句：

//SLAM.SaveMap("/home/ORB_SLAM2/Examples/Stereo/map.bin");

2、地圖加載

地圖加載相當于地圖保存的逆過程，但是實際操作要相對麻煩一點。

2.1 源碼修改

地圖加載部分需要修改的較多，所以按所需修改的文件來進行說明。

第一步：Map相關文件修改

在Map.h文件中聲明地圖加載函數、地圖點加載函數和關鍵幀加載函數：

//加載地圖信息void Load(const string &filename,SystemSetting* mySystemSetting);MapPoint* LoadMapPoint(ifstream &f);KeyFrame* LoadKeyFrame(ifstream &f,SystemSetting* mySystemSetting);

在Map.cc文件中進行相應實現：

地圖加載函數

//地圖加載函數
void Map::Load ( const string &filename, SystemSetting* mySystemSetting)
{cerr << "Map.cc :: Map reading from:"<<filename<<endl;ifstream f;f.open( filename.c_str(),ios::binary );// Same as the sequence that we save the file, we first read the number of MapPoints.unsigned long int nMapPoints;f.read((char*)&nMapPoints, sizeof(nMapPoints));// Then read MapPoints one after another, and add them into the mapcerr<<"Map.cc :: The number of MapPoints:"<<nMapPoints<<endl;for ( unsigned int i = 0; i < nMapPoints; i ++ ){MapPoint* mp = LoadMapPoint(f);AddMapPoint(mp);}// Get all MapPointsstd::vector<MapPoint*> vmp = GetAllMapPoints();// Read the number of KeyFramesunsigned long int nKeyFrames;f.read((char*)&nKeyFrames, sizeof(nKeyFrames));cerr<<"Map.cc :: The number of KeyFrames:"<<nKeyFrames<<endl;// Then read KeyFrames one after another, and add them into the mapvector<KeyFrame*>kf_by_order;for( unsigned int i = 0; i < nKeyFrames; i ++ ){KeyFrame* kf = LoadKeyFrame(f, mySystemSetting);AddKeyFrame(kf);kf_by_order.push_back(kf);}cerr<<"Map.cc :: Max KeyFrame ID is: " << mnMaxKFid << ", and I set mnId to this number" <<endl;cerr<<"Map.cc :: KeyFrame Load OVER!"<<endl;// Read Spanning Tree(open loop trajectory)map<unsigned long int, KeyFrame*> kf_by_id;for ( auto kf: mspKeyFrames )kf_by_id[kf->mnId] = kf;cerr<<"Map.cc :: Start Load The Parent!"<<endl;for( auto kf: kf_by_order ){// Read parent_id of current KeyFrame.unsigned long int parent_id;f.read((char*)&parent_id, sizeof(parent_id));// Add parent KeyFrame to current KeyFrame.// cout<<"Map::Load : Add parent KeyFrame to current KeyFrame"<<endl;if ( parent_id != ULONG_MAX )kf->ChangeParent(kf_by_id[parent_id]);// Read covisibility graphs.// Read the number of Connected KeyFrames of current KeyFrame.unsigned long int nb_con;f.read((char*)&nb_con, sizeof(nb_con));// Read id and weight of Connected KeyFrames of current KeyFrame, // and add Connected KeyFrames into covisibility graph.// cout<<"Map::Load : Read id and weight of Connected KeyFrames"<<endl;for ( unsigned long int i = 0; i < nb_con; i ++ ){unsigned long int id;int weight;f.read((char*)&id, sizeof(id));f.read((char*)&weight, sizeof(weight));kf->AddConnection(kf_by_id[id],weight);}}cerr<<"Map.cc :: Parent Load OVER!"<<endl;for ( auto mp: vmp ){// cout << "Now mp = "<< mp << endl;if(mp){// cout << "compute for mp = "<< mp << endl;mp->ComputeDistinctiveDescriptors();// cout << "Computed Distinctive Descriptors." << endl;mp->UpdateNormalAndDepth();// cout << "Updated Normal And Depth." << endl;}}f.close();cerr<<"Map.cc :: Load IS OVER!"<<endl;return;
}

地圖點加載函數：

MapPoint* Map::LoadMapPoint( ifstream &f )
{// Position and Orientation of the MapPoints.cv::Mat Position(3,1,CV_32F);long unsigned int id;f.read((char*)&id, sizeof(id));f.read((char*)&Position.at<float>(0), sizeof(float));f.read((char*)&Position.at<float>(1), sizeof(float));f.read((char*)&Position.at<float>(2), sizeof(float));// Initialize a MapPoint, and set its id and Position.MapPoint* mp = new MapPoint(Position, this );mp->mnId = id;mp->SetWorldPos( Position );return mp;
}

關鍵幀加載函數：

KeyFrame* Map::LoadKeyFrame( ifstream &f, SystemSetting* mySystemSetting )
{InitKeyFrame initkf(*mySystemSetting);// Read ID and TimeStamp of each KeyFrame.f.read((char*)&initkf.nId, sizeof(initkf.nId));f.read((char*)&initkf.TimeStamp, sizeof(double));// Read position and quaternioncv::Mat T = cv::Mat::zeros(4,4,CV_32F);std::vector<float> Quat(4);//Quat.reserve(4);for ( int i = 0; i < 4; i ++ )f.read((char*)&Quat[i],sizeof(float));cv::Mat R = Converter::toCvMat(Quat);for ( int i = 0; i < 3; i ++ )f.read((char*)&T.at<float>(i,3),sizeof(float));for ( int i = 0; i < 3; i ++ )for ( int j = 0; j < 3; j ++ )T.at<float>(i,j) = R.at<float>(i,j);T.at<float>(3,3) = 1;// Read feature point number of current Key Framef.read((char*)&initkf.N, sizeof(initkf.N));initkf.vKps.reserve(initkf.N);initkf.Descriptors.create(initkf.N, 32, CV_8UC1);vector<float>KeypointDepth;std::vector<MapPoint*> vpMapPoints;vpMapPoints = vector<MapPoint*>(initkf.N,static_cast<MapPoint*>(NULL));// Read Keypoints and descriptors of current KeyFramestd::vector<MapPoint*> vmp = GetAllMapPoints();for(int i = 0; i < initkf.N; i ++ ){cv::KeyPoint kp;f.read((char*)&kp.pt.x, sizeof(kp.pt.x));f.read((char*)&kp.pt.y, sizeof(kp.pt.y));f.read((char*)&kp.size, sizeof(kp.size));f.read((char*)&kp.angle,sizeof(kp.angle));f.read((char*)&kp.response, sizeof(kp.response));f.read((char*)&kp.octave, sizeof(kp.octave));initkf.vKps.push_back(kp);// Read descriptors of keypointsf.read((char*)&initkf.Descriptors.cols, sizeof(initkf.Descriptors.cols));// for ( int j = 0; j < 32; j ++ ) // Since initkf.Descriptors.cols is always 32, for loop may also write like this.for ( int j = 0; j < initkf.Descriptors.cols; j ++ )f.read((char*)&initkf.Descriptors.at<unsigned char>(i,j),sizeof(char));// Read the mapping from keypoints to MapPoints.unsigned long int mpidx;f.read((char*)&mpidx, sizeof(mpidx));// Look up from vmp, which contains all MapPoints, MapPoint of current KeyFrame, and then insert in vpMapPoints.if( mpidx == ULONG_MAX )vpMapPoints[i] = NULL;elsevpMapPoints[i] = vmp[mpidx];}initkf.vRight = vector<float>(initkf.N,-1);initkf.vDepth = vector<float>(initkf.N,-1);//initkf.vDepth = KeypointDepth;initkf.UndistortKeyPoints();initkf.AssignFeaturesToGrid();// Use initkf to initialize a KeyFrame and set parametersKeyFrame* kf = new KeyFrame( initkf, this, NULL, vpMapPoints );kf->mnId = initkf.nId;kf->SetPose(T);kf->ComputeBoW();for ( int i = 0; i < initkf.N; i ++ ){if ( vpMapPoints[i] ){vpMapPoints[i]->AddObservation(kf,i);if( !vpMapPoints[i]->GetReferenceKeyFrame())vpMapPoints[i]->SetReferenceKeyFrame(kf);}}return kf;
}

第二步：MapPoint相關文件修改

由于在加載地圖時我們只有Position以及當前的Map，所以需要重新定義一種MapPoint類的構造函數以滿足要求。

在MapPoint.h文件中添加如下構造函數：

MapPoint(const cv::Mat &Pos,Map* pMap);

在MapPoint.cc文件中實現該構造函數：

MapPoint::MapPoint(const cv::Mat &Pos,Map* pMap):mnFirstKFid(0), mnFirstFrame(0), nObs(0), mnTrackReferenceForFrame(0), mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0),mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(static_cast<KeyFrame*>(NULL)), mnVisible(1), mnFound(1), mbBad(false),mpReplaced(static_cast<MapPoint*>(NULL)), mfMinDistance(0), mfMaxDistance(0), mpMap(pMap)
{Pos.copyTo(mWorldPos);mNormalVector = cv::Mat::zeros(3,1,CV_32F);unique_lock<mutex> lock(mpMap->mMutexPointCreation);mnId = nNextId++;
}

此外，還需要添加如下函數：

KeyFrame* SetReferenceKeyFrame(KeyFrame* RFKF);

KeyFrame* MapPoint::SetReferenceKeyFrame(KeyFrame* RFKF)
{return mpRefKF = RFKF;
}

第三步：KeyFrame相關文件修改

與MapPoint文件相同，KeyFrame文件也要做相關修改。

在KeyFrame.h文件中添加如下構造函數：

KeyFrame(InitKeyFrame &initkf,Map* pMap,KeyFrameDatabase* pKFDB,vector<MapPoint*>& vpMapPoints);

在KeyFrame.cc文件中實現該構造函數：

KeyFrame::KeyFrame(InitKeyFrame &initkf, Map *pMap, KeyFrameDatabase *pKFDB, vector<MapPoint*> &vpMapPoints):mnFrameId(0), mTimeStamp(initkf.TimeStamp), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS),mfGridElementWidthInv(initkf.fGridElementWidthInv), mfGridElementHeightInv(initkf.fGridElementHeightInv),mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0),mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnBAGlobalForKF(0),fx(initkf.fx), fy(initkf.fy), cx(initkf.cx), cy(initkf.cy), invfx(initkf.invfx),invfy(initkf.invfy), mbf(initkf.bf), mb(initkf.b), mThDepth(initkf.ThDepth), N(initkf.N),mvKeys(initkf.vKps), mvKeysUn(initkf.vKpsUn), mvuRight(initkf.vRight), mvDepth(initkf.vDepth),mDescriptors(initkf.Descriptors.clone()), mBowVec(initkf.BowVec), mFeatVec(initkf.FeatVec),mnScaleLevels(initkf.nScaleLevels), mfScaleFactor(initkf.fScaleFactor), mfLogScaleFactor(initkf.fLogScaleFactor),mvScaleFactors(initkf.vScaleFactors), mvLevelSigma2(initkf.vLevelSigma2),mvInvLevelSigma2(initkf.vInvLevelSigma2),mnMinX(initkf.nMinX), mnMinY(initkf.nMinY), mnMaxX(initkf.nMaxX), mnMaxY(initkf.nMaxY), mK(initkf.K),mvpMapPoints(vpMapPoints), mpKeyFrameDB(pKFDB), mpORBvocabulary(initkf.pVocabulary),mbFirstConnection(true), mpParent(NULL), mbNotErase(false), mbToBeErased(false), mbBad(false),mHalfBaseline(initkf.b/2), mpMap(pMap){mnId = nNextId ++;}

不要忘記在KeyFrame.h中添加相應頭文件和命名空間中的類聲明

第四步：SystemSetting和InitKeyFrame相關文件

在上面的函數中我們用到了SystemSetting類和InitKeyFrame類。其中SystemSetting類用于讀取參數文件中的相關參數，InitKeyFrame類用于進行關鍵幀初始化。其實現過程如下：

SystemSetting.h

#ifndef SYSTEMSETTING_H
#define SYSTEMSETTING_H#include<string>
#include"ORBVocabulary.h"
#include<opencv2/opencv.hpp>namespace ORB_SLAM2 {class SystemSetting{public:SystemSetting(ORBVocabulary* pVoc);bool LoadSystemSetting(const std::string strSettingPath);public:ORBVocabulary* pVocavulary;//相機參數float width;float height;float fx;float fy;float cx;float cy;float invfx;float invfy;float bf;float b;float fps;cv::Mat K;cv::Mat DistCoef;bool initialized;//相機 RGB 參數int nRGB;//ORB特征參數int nFeatures;float fScaleFactor;int nLevels;float fIniThFAST;float fMinThFAST;//其他參數float ThDepth = -1;float DepthMapFactor = -1;};}//namespace ORB_SLAM2#endif //SystemSetting

SystemSetting.cc的函數具體實現：

#include<iostream>
#include"SystemSetting.h"using namespace std;namespace ORB_SLAM2 {SystemSetting::SystemSetting(ORBVocabulary* pVoc):pVocavulary(pVoc){}bool SystemSetting::LoadSystemSetting(const std::string strSettingPath){cout<<endl<<"Loading System Parameters form:"<<strSettingPath<<endl;cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);width  = fSettings["Camera.width"];height = fSettings["Camera.height"];fx     = fSettings["Camera.fx"];fy     = fSettings["Camera.fy"];cx     = fSettings["Camera.cx"];cy     = fSettings["Camera.cy"];cv::Mat tmpK = cv::Mat::eye(3,3,CV_32F);tmpK.at<float>(0,0) = fx;tmpK.at<float>(1,1) = fy;tmpK.at<float>(0,2) = cx;tmpK.at<float>(1,2) = cy;tmpK.copyTo(K);cv::Mat tmpDistCoef(4,1,CV_32F);tmpDistCoef.at<float>(0) = fSettings["Camera.k1"];tmpDistCoef.at<float>(1) = fSettings["Camera.k2"];tmpDistCoef.at<float>(2) = fSettings["Camera.p1"];tmpDistCoef.at<float>(3) = fSettings["Camera.p2"];const float k3 = fSettings["Camera.k3"];if( k3!=0 ){tmpDistCoef.resize(5);tmpDistCoef.at<float>(4) = k3;}tmpDistCoef.copyTo( DistCoef );bf = fSettings["Camera.bf"];fps= fSettings["Camera.fps"];invfx = 1.0f/fx;invfy = 1.0f/fy;b     = bf  /fx;initialized = true;cout<<"- size:"<<width<<"x"<<height<<endl;cout<<"- fx:"  <<fx<<endl;cout << "- fy: " << fy << endl;cout << "- cx: " << cx << endl;cout << "- cy: " << cy << endl;cout << "- k1: " << DistCoef.at<float>(0) << endl;cout << "- k2: " << DistCoef.at<float>(1) << endl;if(DistCoef.rows==5)cout << "- k3: " << DistCoef.at<float>(4) << endl;cout << "- p1: " << DistCoef.at<float>(2) << endl;cout << "- p2: " << DistCoef.at<float>(3) << endl;cout << "- bf: " << bf << endl;//Load RGB parameternRGB = fSettings["Camera.RGB"];//Load ORB feature parametersnFeatures = fSettings["ORBextractor.nFeatures"];fScaleFactor = fSettings["ORBextractor.scaleFactor"];nLevels = fSettings["ORBextractor.nLevels"];fIniThFAST = fSettings["ORBextractor.iniThFAST"];fMinThFAST = fSettings["ORBextractor.minThFAST"];cout << endl  << "ORB Extractor Parameters: " << endl;cout << "- Number of Features: " << nFeatures << endl;cout << "- Scale Levels: " << nLevels << endl;cout << "- Scale Factor: " << fScaleFactor << endl;cout << "- Initial Fast Threshold: " << fIniThFAST << endl;cout << "- Minimum Fast Threshold: " << fMinThFAST << endl;//Load others parameters, if the sensor is MONOCULAR, the parameters is zero;//ThDepth = fSettings["ThDepth"];//DepthMapFactor = fSettings["DepthMapFactor"];fSettings.release();return true;}
}

InitKeyFrame.h文件

#ifndef INITKEYFRAME_H
#define INITKEYFRAME_H#include "Thirdparty/DBoW2/DBoW2/BowVector.h"
#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
#include "SystemSetting.h"
#include <opencv2/opencv.hpp>
#include "ORBVocabulary.h"
#include "KeyFrameDatabase.h"
//#include "MapPoints.h"namespace ORB_SLAM2
{#define FRAME_GRID_ROWS 48
#define FRAME_GRID_COLS 64class SystemSetting;
class KeyFrameDatabase;
//class ORBVocabulary;class InitKeyFrame
{
public:    InitKeyFrame(SystemSetting &SS);void UndistortKeyPoints();bool PosInGrid(const cv::KeyPoint& kp, int &posX, int &posY);void AssignFeaturesToGrid();public:ORBVocabulary* pVocabulary;//KeyFrameDatabase* pKeyFrameDatabase;long unsigned int nId;double TimeStamp;float fGridElementWidthInv;float fGridElementHeightInv;std::vector<std::size_t> vGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS];float fx;float fy;float cx;float cy;float invfx;float invfy;float bf;float b;float ThDepth;int N;std::vector<cv::KeyPoint> vKps;std::vector<cv::KeyPoint> vKpsUn;cv::Mat Descriptors;//it's zero for monostd::vector<float> vRight;std::vector<float> vDepth;DBoW2::BowVector BowVec;DBoW2::FeatureVector FeatVec;int nScaleLevels;float fScaleFactor;float fLogScaleFactor;std::vector<float> vScaleFactors;std::vector<float> vLevelSigma2;std::vector<float> vInvLevelSigma2;std::vector<float> vInvScaleFactors;int nMinX;int nMinY;int nMaxX;int nMaxY;cv::Mat K;cv::Mat DistCoef;    };} //namespace ORB_SLAM2
#endif //INITKEYFRAME_H

InitKeyFrame.cc的函數具體實現：

#include "InitKeyFrame.h"
#include <opencv2/opencv.hpp>
#include "SystemSetting.h"namespace ORB_SLAM2
{InitKeyFrame::InitKeyFrame(SystemSetting &SS):pVocabulary(SS.pVocavulary)//, pKeyFrameDatabase(SS.pKeyFrameDatabase)
{fx = SS.fx;fy = SS.fy;cx = SS.cx;cy = SS.cy;invfx = SS.invfx;invfy = SS.invfy;bf = SS.bf;b  = SS.b;ThDepth = SS.ThDepth;nScaleLevels = SS.nLevels;fScaleFactor = SS.fScaleFactor;fLogScaleFactor = log(SS.fScaleFactor);vScaleFactors.resize(nScaleLevels);vLevelSigma2.resize(nScaleLevels);vScaleFactors[0] = 1.0f;vLevelSigma2[0]  = 1.0f;for ( int i = 1; i < nScaleLevels; i ++ ){vScaleFactors[i] = vScaleFactors[i-1]*fScaleFactor;vLevelSigma2[i]  = vScaleFactors[i]*vScaleFactors[i];}vInvScaleFactors.resize(nScaleLevels);vInvLevelSigma2.resize(nScaleLevels);for ( int i = 0; i < nScaleLevels; i ++ ){vInvScaleFactors[i] = 1.0f/vScaleFactors[i];vInvLevelSigma2[i]  = 1.0f/vLevelSigma2[i];}K = SS.K;DistCoef = SS.DistCoef;if( SS.DistCoef.at<float>(0)!=0.0){cv::Mat mat(4,2,CV_32F);mat.at<float>(0,0) = 0.0;mat.at<float>(0,1) = 0.0;mat.at<float>(1,0) = SS.width;mat.at<float>(1,1) = 0.0;mat.at<float>(2,0) = 0.0;mat.at<float>(2,1) = SS.height;mat.at<float>(3,0) = SS.width;mat.at<float>(3,1) = SS.height;mat = mat.reshape(2);cv::undistortPoints(mat, mat, SS.K, SS.DistCoef, cv::Mat(), SS.K);mat = mat.reshape(1);nMinX = min(mat.at<float>(0,0), mat.at<float>(2,0));nMaxX = max(mat.at<float>(1,0), mat.at<float>(3,0));nMinY = min(mat.at<float>(0,1), mat.at<float>(1,1));nMaxY = max(mat.at<float>(2,1), mat.at<float>(3,1));}else{nMinX = 0.0f;nMaxX = SS.width;nMinY = 0.0f;nMaxY = SS.height;}fGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/(nMaxX-nMinX);fGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/(nMaxY-nMinY);}void InitKeyFrame::UndistortKeyPoints()
{if( DistCoef.at<float>(0) == 0.0){vKpsUn = vKps;return;}cv::Mat mat(N,2,CV_32F);for ( int i = 0; i < N; i ++ ){mat.at<float>(i,0) = vKps[i].pt.x;mat.at<float>(i,1) = vKps[i].pt.y;}mat = mat.reshape(2);cv::undistortPoints(mat, mat, K, DistCoef, cv::Mat(), K );mat = mat.reshape(1);vKpsUn.resize(N);for( int i = 0; i < N; i ++ ){cv::KeyPoint kp = vKps[i];kp.pt.x = mat.at<float>(i,0);kp.pt.y = mat.at<float>(i,1);vKpsUn[i] = kp;}
}void InitKeyFrame::AssignFeaturesToGrid()
{int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);for ( unsigned int i = 0; i < FRAME_GRID_COLS; i ++ ){for ( unsigned int j = 0; j < FRAME_GRID_ROWS; j ++)vGrid[i][j].reserve(nReserve);}for ( int i = 0; i < N; i ++ ){const cv::KeyPoint& kp = vKpsUn[i];int nGridPosX, nGridPosY;if( PosInGrid(kp, nGridPosX, nGridPosY))vGrid[nGridPosX][nGridPosY].push_back(i);}
}bool InitKeyFrame::PosInGrid(const cv::KeyPoint &kp, int &posX,  int &posY)
{posX = round((kp.pt.x-nMinX)*fGridElementWidthInv);posY = round((kp.pt.y-nMinY)*fGridElementHeightInv);if(posX<0 || posX>=FRAME_GRID_COLS ||posY<0 || posY>=FRAME_GRID_ROWS)return false;return true;
}}

第五步：System相關文件的修改

在System.h中添加函數定義：

void LoadMap(const string &filename);

并在對應的System.cc中添加了定義

//地圖加載
void System::LoadMap(const string &filename)
{SystemSetting* mySystemSetting = new SystemSetting(mpVocabulary);mySystemSetting->LoadSystemSetting(mySettingFile);mpMap->Load(filename,mySystemSetting);
}

還需要在System.h中添加聲明

 std::string mySettingFile;

同時在構造函數函數中對mySettingFile成員變量賦值

 mySettingFile = strSettingsFile;

2.2 測試

做完這些修改之后 ，不要忘記修改CMakeLists.txt 文件。

#在add_library 中加入 src/InitkeyFrame.cc src/SystemSetting.cc
add_library(${PROJECT_NAME} SHARED
src/System.cc
src/Tracking.cc
src/LocalMapping.cc
src/LoopClosing.cc
src/ORBextractor.cc
src/ORBmatcher.cc
src/FrameDrawer.cc
src/Converter.cc
src/MapPoint.cc
src/KeyFrame.cc
src/Map.cc
src/MapDrawer.cc
src/Optimizer.cc
src/PnPsolver.cc
src/Frame.cc
src/KeyFrameDatabase.cc
src/Sim3Solver.cc
src/Initializer.cc
src/Viewer.cc
src/InitkeyFrame.cc
src/SystemSetting.cc
)

重新編譯程序，然后在Examples文件中對應的示例程序中加入地圖加載代碼即可實現地圖加載功能。
如在stereo_kitti.cc文件中加入如下語句：

SLAM.LoadMap("/home/ORB_SLAM2/Examples/Stereo/map.bin");

效果如下圖：

謝謝！如有錯誤歡迎指正，感謝其他大神的參考博客！

總結

以上是生活随笔為你收集整理的（一次性搞定）ORB_SLAM2地图保存与加载的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。