（一次性搞定）ORB_SLAM2地图保存与加载

（一次性搞定）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 map
    cerr<<"Map.cc :: Map Saving to "<mnId << endl;
    }

    //Print The number of MapPoints
    cerr << "Map.cc :: The number of MapPoints is :"<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 KeyFrames
        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));
        }
    }

    // Save last Frame ID
    // SaveFrameID(f);

    f.close();
    cerr<<"Map.cc :: Map Saving Finished!"<

存储地图点函数——SaveMapPoint()函数的实现：

void Map::SaveMapPoint( ofstream& f, MapPoint* mp)
{   
    //Save ID and the x,y,z coordinates of the current MapPoint
    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));
}

存储关键帧函数——SaveKeyFrame()函数的实现：

void Map::SaveKeyFrame( ofstream &f, KeyFrame* kf )
{
    //Save the ID and timesteps of current KeyFrame
    f.write((char*)&kf->mnId, sizeof(kf->mnId));
    // cout << "saving kf->mnId = " << kf->mnId <mTimeStamp, sizeof(kf->mTimeStamp));
    //Save the Pose Matrix of current KeyFrame
    cv::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(i,j), sizeof(float));
    //         //cerr<<"Tcw.at("<(i,j)< Quat = Converter::toQuaternion(Tcw);
    for ( int i = 0; i < 4; i ++ )
        f.write((char*)&Quat[i],sizeof(float));
    //Save the translation matrix
    for ( int i = 0; i < 3; i ++ )
        f.write((char*)&Tcw.at(i,3),sizeof(float));




    //Save the size of the ORB features current KeyFrame
    //cerr<<"kf->N:"<N<N, sizeof(kf->N));
    //Save each ORB features
    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));

        //Save the Descriptors of current ORB features
        f.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(i,j), sizeof(char));

        //Save the index of MapPoints that corresponds to current ORB features
        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 BoW for relocalization.
    // f.write((char*)&kf->mBowVec, sizeof(kf->mBowVec));
}

Save()函数中的GetMapPointsIdx()函数用于获取地图点的ID号，所以需要在Map.h中定义如下成员变量：

std::map mmpnMapPointsIdx;

GetMapPointsIdx()函数的实现过程为（在Map.cc文件中添加）：

void Map::GetMapPointsIdx()
{
    unique_lock 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& v)
{
    Eigen::Quaterniond q;
    q.x() = v[0];
    q.y() = v[1];
    q.z() = v[2];
    q.w() = v[3];

    Eigen::Matrix 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:"< vmp = GetAllMapPoints();

    // Read the number of KeyFrames
    unsigned long int nKeyFrames;
    f.read((char*)&nKeyFrames, sizeof(nKeyFrames));
    cerr<<"Map.cc :: 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<<"Map.cc :: Max KeyFrame ID is: " << mnMaxKFid << ", and I set mnId to this number" < kf_by_id;
    for ( auto kf: mspKeyFrames )
        kf_by_id[kf->mnId] = kf;
    cerr<<"Map.cc :: Start Load The Parent!"<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"<AddConnection(kf_by_id[id],weight);
        }
   }
   cerr<<"Map.cc :: Parent Load OVER!"<UpdateNormalAndDepth();
            // cout << "Updated Normal And Depth." << endl;
        }
   }
    f.close();
    cerr<<"Map.cc :: Load IS OVER!"<

地图点加载函数：

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(0), sizeof(float));
        f.read((char*)&Position.at(1), sizeof(float));
        f.read((char*)&Position.at(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 quaternion
    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;
    
    // Read feature point number of current Key Frame
    f.read((char*)&initkf.N, sizeof(initkf.N));
    initkf.vKps.reserve(initkf.N);
    initkf.Descriptors.create(initkf.N, 32, CV_8UC1);
    vectorKeypointDepth;

    std::vector vpMapPoints;
    vpMapPoints = vector(initkf.N,static_cast(NULL));
    // Read Keypoints and descriptors of current KeyFrame
    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);
        
        // Read descriptors of keypoints
        f.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(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;
        else
                vpMapPoints[i] = vmp[mpidx];
    }

  
    initkf.vRight = vector(initkf.N,-1);
    initkf.vDepth = vector(initkf.N,-1);
    //initkf.vDepth = KeypointDepth;
    initkf.UndistortKeyPoints();
    initkf.AssignFeaturesToGrid();

    // Use initkf to initialize a KeyFrame and set parameters
    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;
}

第二步：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(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);

     unique_lock 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& vpMapPoints);

在KeyFrame.cc文件中实现该构造函数：

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 ++;
  }

不要忘记在KeyFrame.h中添加相应头文件和命名空间中的类声明

第四步：SystemSetting和InitKeyFrame相关文件

在上面的函数中我们用到了SystemSetting类和InitKeyFrame类。其中SystemSetting类用于读取参数文件中的相关参数，InitKeyFrame类用于进行关键帧初始化。其实现过程如下：

SystemSetting.h

#ifndef SYSTEMSETTING_H
#define SYSTEMSETTING_H

#include
#include"ORBVocabulary.h"
#include


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
#include"SystemSetting.h"

using namespace std;

namespace ORB_SLAM2 {

   SystemSetting::SystemSetting(ORBVocabulary* pVoc):pVocavulary(pVoc)
   {
     
   }
    
   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;
   }
}

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

InitKeyFrame.cc的函数具体实现：

#include "InitKeyFrame.h"
#include 
#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(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;
}

}

第五步：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");

效果如下图：

谢谢！如有错误欢迎指正，感谢其他大神的参考博客！