[学习SLAM]ORB-SLAM2 地图保存与加载 基础构建

一、简介

　　在ORB-SLAM2的System.h文件中，有这样一句话：// TODO: Save/Load functions，让读者自己实现地图的保存与加载功能。其实在应用过程中很多场合同样需要先保存当前场景的地图，然后下次启动时直接进行跟踪，这样避免了初始化和建图，减小相机跟踪过程中计算机负载，还有就是进行全场的定位。其实网上已经有地图保存的代码了https://github.com/xiaopengsu/ORB_SLAM2_map

二、地图元素分析

　　所谓地图保存，就是保存地图“Map”中的各个元素，以及它们之间的关系，凡是跟踪过程中需要用到的东西自然也就是需要保存的对象，上一节曾经说过地图主要包含关键帧、3D地图点、BoW向量、共视图、生长树等，在跟踪过程中有三种跟踪模型和局部地图跟踪等过程，局部地图跟踪需要用到3D地图点、共视关系等元素，参考帧模型需要用到关键帧的BoW向量，重定位需要用到BoW向量、3D点等（具体哪里用到了需要翻看代码），所以基本上述元素都需要保存。

　　另一方面，关键帧也是一个抽象的概念（一个类），我们看看具体包含什么（其实都在关键帧类里面了），关键帧是从普通帧来的，所以来了视频帧首先需要做的就是检测特征点，计算描述符，还有当前帧的相机位姿，作为关键帧之后需要有对应的ID编号，以及特征点进行三角化之后的3D地图点等。

　　关于3D地图点需要保存的就只有世界坐标了，至于其它的关联关系可以重关键帧获得。需要单独说的是在关键帧类中包含了特征点和描述符，所以BoW向量是不需要保存的（也没办法保存），只需要在加载了关键帧之后利用特征描述符重新计算即可。

　　所以现在需要保存的东西包括关键帧、3D地图点、共视图、生长树。

 地图保存

三、地图保存代码实例

保存的代码主要是在Map.h和Map.cc敲，根据代码里的保存格式，应该是保存成二进制文件，对应的代码敲进对应的位置后，我在system.cc中定义了一个save函数去调用map中的save函数，然后在ros_mono.cc中调用了system中的save函数，可能我这样的做法有点太绕了，或许可以直接在ros_mono中调用map函数：

• 在system.h和system.cc中分别添加声明和定义：

     void SaveMap(const string &filename);  
      
    void System::SaveMap(const string &filename)  
    {  
       mpMap->Save(filename);   
    }  

• 然后在ros_mono.cc中添加

    char IsSaveMap;  
        cout << "Do you want to save the map?(Y/N)" << endl;  
        cin >> IsSaveMap;  
        if(IsSaveMap == 'Y' || IsSaveMap == 'y')  
            SLAM.SaveMap("MapPointandKeyFrame.bin");  

IsSaveMap是一个flag，让你确定是否保存地图的，当然这里输入Y或者y就是保存，输入其他的字符都是不保存啦，写得不是很规范。这样MapPoint 和KeyFrame都可以保存在了执行目录下的MapPointandKeyFrame.bin文件中了(SLAM.SaveMap("Slam_latest_Map.bin");        // MapPointandKeyFrame.bin --Slam_latest_Map.bin)。

[参考https://www.cnblogs.com/mafuqiang/p/6972342.html ]

• 在Map.h中声明,在Map.cc定义

　　需要明确的是一般SLAM系统对地图的维护均在Map.cc这个函数类中，最终把地图保存成二进制文件，所以现在Map.h中声明几个函数吧：

public:
    void Save( const string &filename );
protected:
    void SaveMapPoint( ofstream &f, MapPoint* mp );
    void SaveKeyFrame( ofstream &f, KeyFrame* kf );

 　　下面关于Save函数的构成：

void Map::Save ( const string& filename )
 {
     cerr<<"Map Saving to "<GetParent();
         unsigned long int parent_id = ULONG_MAX;
         if ( parent )
             parent_id = parent->mnId;
         f.write((char*)&parent_id, sizeof(parent_id));
         //获得当前关键帧的关联关键帧的大小，并依次保存每一个关联关键帧的ID和weight；//共视图生长树，吗
         unsigned 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));
         }
     }
 
     f.close();
     cerr<<"Map Saving Finished!"<

　　可以看到，Save函数依次保存了地图点的数目、所有的地图点、关键帧的数目、所有关键帧、关键帧的生长树节点和关联关系；

　　下面是SaveMapPoint函数的构成：

void Map::SaveMapPoint( ofstream& f, MapPoint* mp)
{   
     //保存当前MapPoint的ID和世界坐标值
     f.write((char*)&mp->mnId, sizeof(mp->mnId));
     cv::Mat mpWorldPos = mp->GetWorldPos();
     f.write((char*)& mpWorldPos.at(0),sizeof(float));
     f.write((char*)& mpWorldPos.at(1),sizeof(float));
     f.write((char*)& mpWorldPos.at(2),sizeof(float));
}

 

　　其实主要就是通过MapPoint类的GetWorldPos（）函数获取了地图点的坐标值并保存下来；

　　下面是SaveKeyFrame函数的构成：

void Map::SaveKeyFrame( ofstream &f, KeyFrame* kf )
 {
//保存当前关键帧的ID和时间戳
     f.write((char*)&kf->mnId, sizeof(kf->mnId));
     f.write((char*)&kf->mTimeStamp, sizeof(kf->mTimeStamp));
     //保存当前关键帧的位姿矩阵
     cv::Mat Tcw = kf->GetPose();
     //通过四元数保存旋转矩阵
     std::vector Quat = Converter::toQuaternion(Tcw);
     for ( int i = 0; i < 4; i ++ )
         f.write((char*)&Quat[i],sizeof(float));
     //保存平移矩阵
     for ( int i = 0; i < 3; i ++ )
         f.write((char*)&Tcw.at(i,3),sizeof(float));
 
 
     //直接保存旋转矩阵
 //  for ( int i = 0; i < Tcw.rows; i ++ )
 //  {
 //      for ( int j = 0; j < Tcw.cols; j ++ )
 //      {
 //              f.write((char*)&Tcw.at(i,j), sizeof(float));
 //              //cerr<<"Tcw.at("<(i,j)<N:"<N<N, sizeof(kf->N));
     //保存每一个ORB特征点
     for( 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));
 
         //保存当前特征点的描述符
         for (int j = 0; j < kf->mDescriptors.cols; j ++ )
                 f.write((char*)&kf->mDescriptors.at(i,j), sizeof(char));
 
         //保存当前ORB特征对应的MapPoints的索引值
         unsigned long int mnIdx;
         MapPoint* mp = kf->GetMapPoint(i);
         if (mp == NULL  )
                 mnIdx = ULONG_MAX;
         else
                 mnIdx = mmpnMapPointsIdx[mp];
 
         f.write((char*)&mnIdx, sizeof(mnIdx));
     }
 }

　　保存关键帧的函数稍微复杂一点，首先需要明白一幅关键帧包含特征点，描述符，以及哪些特征点通过三角化成为了地图点。

　　其中在Save函数中的GetMapPointsIdx函数的构成为，它的作用是初始化成员变量：

std::map mmpnMapPointsIdx;

这个成员变量存储的是特征点对应的地图点的索引值。

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

 另外，关于旋转矩阵的存储可以通过四元数或矩阵的形式存储，如果使用四元数需要自定义一个矩阵和四元数相互转换的函数，在Converter.cc类里.

三、总结

　　上面就是地图保存部分的代码，经过测试针对TUM的视频是有效的。但是需要在System中设置保存函数并在主函数中调用，尤其是针对无ROS依赖并从摄像头读取图像的时候，这个最后再说。后面继续分享地图的加载部分。

___________________________________

地图的加载

一、前面说了ORB-SLAM地图的保存部分，继续说地图如何加载，因为加载部分相比保存要稍微复杂一些，所以要多说一点。

二、ORB-SLAM2地图加载构成

　　首先同样是在在Map.h加入函数的定义---头文件中声明加载函数，包含地图点和关键帧类的加载。　　

void Load( const string &filename, SystemSetting* mySystemSetting );
MapPoint* LoadMapPoint( ifstream &f );
KeyFrame* LoadKeyFrame( ifstream &f, SystemSetting* mySystemSetting );

　　下面先是Map.cc中加载主函数Load的构成，关于SystemSetting类后面再说：

void Map::Load ( const string &filename, SystemSetting* mySystemSetting )
 {
     cerr << "Map reading from:"< vmp = GetAllMapPoints();
 
     //读取关键帧的数目；
     unsigned long int nKeyFrames;
     f.read((char*)&nKeyFrames, sizeof(nKeyFrames));
     cerr<<"The number of KeyFrames:"<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<<"KeyFrame Load OVER!"< kf_by_id;
     for ( auto kf: mspKeyFrames )
         kf_by_id[kf->mnId] = kf;
     cerr<<"Start Load The Parent!"<ChangeParent(kf_by_id[parent_id]);
 
         //读取当前关键帧的关联关系；
         //先读取当前关键帧的关联关键帧的数目；
         unsigned long int nb_con;
         f.read((char*)&nb_con, sizeof(nb_con));
         //然后读取每一个关联关键帧的ID和weight，并把该关联关键帧加入关系图中；
         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<<"Parent Load OVER!"<ComputeDistinctiveDescriptors();
             mp->UpdateNormalAndDepth();
         }
    }
     f.close();
     cerr<<"Load IS OVER!"<

 　　其过程就是根据保存的顺序依次加载地图点的数目、地图点、关键帧的数目、关键帧、生长树和关联关系。

　　下面是LoadMapPoints函数的构成：

 MapPoint* Map::LoadMapPoint( ifstream &f )
 {
         //主要包括MapPoints的位姿和ID；
         cv::Mat Position(3,1,CV_32F);
         long unsigned int id;
         f.read((char*)&id, sizeof(id));
 
         f.read((char*)&Position.at(0), sizeof(float));
         f.read((char*)&Position.at(1), sizeof(float));
         f.read((char*)&Position.at(2), sizeof(float));
 
         //初始化一个MapPoint，并设置其ID和Position；
         MapPoint* mp = new MapPoint(Position, this );
         mp->mnId = id;
         mp->SetWorldPos( Position );
 
         return mp;
 }

 　　从这里开始涉及到了MapPoint类的初始化问题，由于这里只有Position以及当前的Map，所以需要从新定义MapPoint的构造函数，分别加入到MapPoint的头文件和源文件中：

分别加入到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(NULL)), mnVisible(1), mnFound(1), mbBad(false),
      mpReplaced(static_cast(NULL)), mfMinDistance(0), mfMaxDistance(0), mpMap(pMap)
  {
      Pos.copyTo(mWorldPos);
      mNormalVector = cv::Mat::zeros(3,1,CV_32F);
  
      // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
      unique_lock lock(mpMap->mMutexPointCreation);
      mnId=nNextId++;
  }

　　紧接着是LoadKeyFrame函数的构成，这里由于KeyFrame类需要的初始化信息比较多，因此定义了一个InitKeyFrame类，它通过SystemSetting进行初始化，二SystemSetting的主要作用就是读取设置文件（相机内参，ORB特征参数等），后面将给出SystemSetting和InitKeyFrame类的代码：

 KeyFrame* Map::LoadKeyFrame( ifstream &f, SystemSetting* mySystemSetting )
 {
     //声明一个初始化关键帧的类initkf；
     InitKeyFrame initkf(*mySystemSetting);
 
     //按照保存次序，依次读取关键帧的ID和时间戳；
     f.read((char*)&initkf.nId, sizeof(initkf.nId));
     f.read((char*)&initkf.TimeStamp, sizeof(double));
 
     //读取关键帧位姿矩阵；
     cv::Mat T = cv::Mat::zeros(4,4,CV_32F);
     std::vector 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(i,3),sizeof(float));
     for ( int i = 0; i < 3; i ++ )
         for ( int j = 0; j < 3; j ++ )
             T.at(i,j) = R.at(i,j);
     T.at(3,3) = 1;
 
 //    for ( int i = 0; i < 4; i ++ )
 //    {
 //      for ( int j = 0; j < 4; j ++ )
 //      {
 //              f.read((char*)&T.at(i,j), sizeof(float));
 //              cerr<<"T.at("<(i,j)<KeypointDepth;
 
     std::vector vpMapPoints;
     vpMapPoints = vector(initkf.N,static_cast(NULL));
     //依次读取当前关键帧的特征点和描述符；
     std::vector 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);
 
         //根据需要读取特征点的深度值；
         //float fDepthValue = 0.0;
         //f.read((char*)&fDepthValue, sizeof(float));
         //KeypointDepth.push_back(fDepthValue);
 
         //读取当前特征点的描述符；
         for ( int j = 0; j < 32; j ++ )
                 f.read((char*)&initkf.Descriptors.at(i,j),sizeof(char));
 
         //读取当前特征点和MapPoints的对应关系；
         unsigned long int mpidx;
         f.read((char*)&mpidx, sizeof(mpidx));
 
         //从vmp这个所有的MapPoints中查找当前关键帧的MapPoint，并插入
         if( mpidx == ULONG_MAX )
                 vpMapPoints[i] = NULL;
         else
                 vpMapPoints[i] = vmp[mpidx];
     }
 
     initkf.vRight = vector(initkf.N,-1);
     initkf.vDepth = vector(initkf.N,-1);
     //initkf.vDepth = KeypointDepth;
     initkf.UndistortKeyPoints();
     initkf.AssignFeaturesToGrid();
 
     //使用initkf初始化一个关键帧，并设置相关参数
     KeyFrame* 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;
 }

　　从文件中读取的内容同保存的一致，同时由于是通过InitKeyFrame初始化的关键帧类KeyFrame，因此这里同样需要添加构造函数以及初始化方式：

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


KeyFrame::KeyFrame(InitKeyFrame &initkf, Map *pMap, KeyFrameDatabase *pKFDB, vector &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 ++;
  }

 　　加载了一个关键帧之后还需要计算其BoW向量等操作，同时指定关键帧对地图点的观测。

三、总结

　　加载的过程大概就是这样，时间太晚了，下次在给出InitKeyFrame和SystemSetting两个类的代码，以及其它修改的地方。总结来看，加载时关键帧类和地图点类的初始化是比较烦人的，各种繁琐的参数需要设置，所以很可能存在什么bug也说不定。另外，博客里面的代码部分是参考的网上的代码，部分是自己写的，希望有人看到了有什么错误及时指正。在数据库rgbd_dataset_freiburg1_xyz上的视频测试时是没问题的。

————————————————————————————————————————————

补充SystemSetting和InitKeyFrame两个类的代码。实际上，由于是通过SystemSetting来读取的相机内参以及ORB特征参数，所以就可以将Tracking.cc中关于读取内参的部分替换掉了。

1. SystemSetting.h

#ifndef SYSTEMSETTING_H
#define SYSTEMSETTING_H

#include 
#include "ORBVocabulary.h"
#include

namespace ORB_SLAM2 {
    
    class SystemSetting{
        
        //Load camera parameters from setting file
    public:

        SystemSetting(ORBVocabulary* pVoc);
        //SystemSetting::SystemSetting(ORBVocabulary* pVoc, KeyFrameDatabase* pKFDB );

        bool LoadSystemSetting(const std::string strSettingPath);
        
    public:
        //The Vocabulary and KeyFrameDatabase
        ORBVocabulary* pVocabulary;
        //KeyFrameDatabase* pKeyFrameDatabase;


        //Camera parameters
        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;
        
        //Camera RGB parameters
        int nRGB;
        
        //ORB feature parameters
        int nFeatures;
        float fScaleFactor;
        int nLevels;
        float fIniThFAST;
        float fMinThFAST;
        
        //other parameters
        float ThDepth = -1;
        float DepthMapFactor = -1;
        
    };
    
} //namespace ORB_SLAM2

#endif //SystemSetting

2. SystemSetting.cc

#include 

#include "SystemSetting.h"

using namespace std;

namespace ORB_SLAM2 {
    
    SystemSetting::SystemSetting(ORBVocabulary* pVoc):
        pVocabulary(pVoc)
        {
        }

    //SystemSetting::SystemSetting(ORBVocabulary* pVoc, KeyFrameDatabase* pKFDB):
    //    pVocabulary(pVoc), pKeyFrameDatabase(pKFDB)
    //    {
    //    }


    bool SystemSetting::LoadSystemSetting(const std::string strSettingPath){
        cout<(0,0) = fx;
        tmpK.at(1,1) = fy;
        tmpK.at(0,2) = cx;
        tmpK.at(1,2) = cy;
        tmpK.copyTo(K);
        
        cv::Mat tmpDistCoef(4,1,CV_32F);
        tmpDistCoef.at(0) = fSettings["Camera.k1"];
        tmpDistCoef.at(1) = fSettings["Camera.k2"];
        tmpDistCoef.at(2) = fSettings["Camera.p1"];
        tmpDistCoef.at(3) = fSettings["Camera.p2"];
        const float k3 = fSettings["Camera.k3"];
        if( k3!=0 )
        {
            tmpDistCoef.resize(5);
            tmpDistCoef.at(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:"<(0) << endl;
        cout << "- k2: " << DistCoef.at(1) << endl;
        if(DistCoef.rows==5)
            cout << "- k3: " << DistCoef.at(4) << endl;
        cout << "- p1: " << DistCoef.at(2) << endl;
        cout << "- p2: " << DistCoef.at(3) << endl;
        cout << "- bf: " << bf << endl;
        
        //Load RGB parameter
        nRGB = fSettings["Camera.RGB"];
        
        //Load ORB feature parameters
        nFeatures = 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;
    }
    
    
    
    
}

3. InitKeyFrame.h

#ifndef INITKEYFRAME_H
#define INITKEYFRAME_H

#include "Thirdparty/DBoW2/DBoW2/BowVector.h"
#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
#include "SystemSetting.h"
#include 
#include "ORBVocabulary.h"
#include "KeyFrameDatabase.h"
//#include "MapPoints.h"

namespace ORB_SLAM2
{

#define FRAME_GRID_ROWS 48
#define FRAME_GRID_COLS 64

class 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 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 vKps;
    std::vector vKpsUn;
    cv::Mat Descriptors;
    
    //it's zero for mono
    std::vector vRight;
    std::vector vDepth;
    
    DBoW2::BowVector BowVec;
    DBoW2::FeatureVector FeatVec;
    
    int nScaleLevels;
    float fScaleFactor;
    float fLogScaleFactor;
    std::vector vScaleFactors;
    std::vector vLevelSigma2;
    std::vector vInvLevelSigma2;
    std::vector vInvScaleFactors;
    
    int nMinX;
    int nMinY;
    int nMaxX;
    int nMaxY;
    cv::Mat K;
    cv::Mat DistCoef;    
    
};

} //namespace ORB_SLAM2
#endif //INITKEYFRAME_H

4. InitKeyFrame.cc

#include "InitKeyFrame.h"
#include 
#include "SystemSetting.h"

namespace ORB_SLAM2
{

InitKeyFrame::InitKeyFrame(SystemSetting &SS):pVocabulary(SS.pVocabulary)//, 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(0)!=0.0)
    {
        cv::Mat mat(4,2,CV_32F);
        mat.at(0,0) = 0.0;
        mat.at(0,1) = 0.0;
        mat.at(1,0) = SS.width;
        mat.at(1,1) = 0.0;
        mat.at(2,0) = 0.0;
        mat.at(2,1) = SS.height;
        mat.at(3,0) = SS.width;
        mat.at(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(0,0), mat.at(2,0));
        nMaxX = max(mat.at(1,0), mat.at(3,0));
        nMinY = min(mat.at(0,1), mat.at(1,1));
        nMaxY = max(mat.at(2,1), mat.at(3,1));
    }
    else
    {
        nMinX = 0.0f;
        nMaxX = SS.width;
        nMinY = 0.0f;
        nMaxY = SS.height;
    }

    fGridElementWidthInv=static_cast(FRAME_GRID_COLS)/(nMaxX-nMinX);
    fGridElementHeightInv=static_cast(FRAME_GRID_ROWS)/(nMaxY-nMinY);
    
}

void InitKeyFrame::UndistortKeyPoints()
{
    if( DistCoef.at(0) == 0.0)
    {
        vKpsUn = vKps;
        return;
    }

    cv::Mat mat(N,2,CV_32F);
    for ( int i = 0; i < N; i ++ )
    {
        mat.at(i,0) = vKps[i].pt.x;
        mat.at(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(i,0);
        kp.pt.y = mat.at(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;
}

}

在MapPoint.h和MapPoint.cc中分别添加

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

并在keyframe.h中加上initkeyframe头文件。

然后开始加载地图点和关键帧，同样我还是在system.h中声明了

    void LoadMap(const string &filename,SystemSetting* mySystemSetting);  

并在对应的system.cc中添加了定义

    void System::LoadMap(const string &filename,SystemSetting* mySystemSetting)  
    {  
       mpMap->Load(filename, mySystemSetting);   
    }  

哦，对了，如果这样读进来好像没法做定位只能显示出地图，需要在Map.cc中Load函数读入每一个关键帧后添加到KeyFrameDatabase中去，但这样需要在Map.h中加入KeyFrameDatabase的头文件，这样就可以读入地图后进行relocalisation了。

然后，我读入是直接在system.cc中读入的，
 

    cout << "Do you want to load the map?(Y/N)" << endl;  
        cin >> IsLoadMap;  
        SystemSetting *mySystemSetting = new SystemSetting(mpVocabulary);  
        mySystemSetting->LoadSystemSetting("/home/liuzhen/catkin_ws/src/ORB_SLAM2-master/Examples/ROS/ORB_SLAM2/Asus.yaml");  
        if(IsLoadMap == 'Y' || IsLoadMap == 'y'){  
            mpMap->Load("/home/liuzhen/catkin_ws/src/ORB_SLAM2-master/Examples/ROS/ORB_SLAM2/MapPointandKeyFrame.bin",mySystemSetting);  
    }  

这样应该就可以在运行ros_mono时加载地图了，我的地图加载出来是这样的：