基于ORB-SLAM2实现SLAM地图加载

1. 基于ORB-SLAM2实现SLAM地图加载（不能重定位）

本文记录了ORB_SLAM2中地图保存与加载的过程。
参考博客：
https://www.cnblogs.com/mafuqiang/p/6972841.html
https://www.cnblogs.com/mafuqiang/p/7002568.html
1.1前面说了ORB-SLAM地图的保存部分，经过测试可以使用。本文我们继续说地图如何加载，因为加载部分相比保存要稍微复杂一些，所以要多说一点。
这个代码可以实现地图加载，加载之后的重定位工作正在摸索中。
1.2修改源码实现地图加载
地图加载是地图保存的逆过程，实际操作要相对麻烦一点。地图加载部分需要修改的较多，所以按所需修改的文件来进行说明。
修改代码：Map.h Map.cc MapPoint.h MapPoint.cc KeyFrame.h KeyFrame.cc System.h System.cc CMakeLists.txt
新建代码：SystemSetting.h SystemSetting.cc InitKeyFrame.h InitKeyFrame.cc
第一步：修改 Map.h Map.cc
修改Map.h
Map.h/class Map/public:

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

修改Map.cc
地图加载函数

//地图加载函数
void Map::Load ( const string &filename, SystemSetting* mySystemSetting)//wuhoup20190924
{
    cerr << "Map.cc :: Map reading from:"<<filename<<endl;
    ifstream f;
    f.open( filename.c_str() );

    // 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 map
    cerr<<"Map.cc :: The number of MapPoints:"<<nMapPoints<<endl;
    for ( unsigned int i = 0; i < nMapPoints; i ++ )
    {
        MapPoint* mp = LoadMapPoint(f);
        AddMapPoint(mp);
    }

    // Get all MapPoints
    std::vector<MapPoint*> vmp = GetAllMapPoints();

    // Read the number of KeyFrames
    unsigned 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 map
    vector<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 quaternion
    cv::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 Frame
    f.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 KeyFrame
    std::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 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<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;
        else
                vpMapPoints[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 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文件
MapPoint.h
MapPoint/public:

//重新定义一种MapPoint类的构造函数
    MapPoint(const cv::Mat &Pos,Map* pMap);//wuhoup20190924
    KeyFrame* SetReferenceKeyFrame(KeyFrame* RFKF);

MapPoint.cc

//由于在加载地图时我们只有Position以及当前的Map，所以需要重新定义一种MapPoint类的构造函数以满足要求。
MapPoint::MapPoint(const cv::Mat &Pos,Map* pMap)://wuhoup20190924
    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* MapPoint::SetReferenceKeyFrame(KeyFrame* RFKF)//wuhoup20190924
{
    return mpRefKF = RFKF;
}

第三步：修改KeyFrame相关文件
KeyFrame.h文件中添加如下构造函数
头文件：

#include "InitKeyFrame.h"

namespace ORB_SLAM2目录下

class InitKeyFrame;

class KeyFrame/public目录下

//构造函数
    //从文件中读取的内容同保存的一致，同时由于是通过InitKeyFrame初始化的关键帧类KeyFrame，
    //因此这里同样需要添加构造函数以及初始化方式：
    KeyFrame(InitKeyFrame &initkf,Map* pMap,KeyFrameDatabase* pKFDB,vector<MapPoint*>& vpMapPoints);

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

//从文件中读取的内容同保存的一致，同时由于是通过InitKeyFrame初始化的关键帧类KeyFrame，
//因此这里同样需要添加构造函数以及初始化方式：
//加载了一个关键帧之后还需要计算其BoW向量等操作，同时指定关键帧对地图点的观测。
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 ++;
  }

第四步：新建SystemSetting和InitKeyFrame相关文件
在上面的函数中我们用到了SystemSetting类和InitKeyFrame类。其中SystemSetting类用于读取参数文件中的相关参数（参数配置文件.yaml），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 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 <opencv2/opencv.hpp>
#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<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 mono
    std::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
system/public:

//声明LoadMap
    void LoadMap(const string &filename, const string &strSettingsFile);
    std::string mySettingFile;
    //std::string strSettingsFile;

这里需要注意，加载地图时需要响函数传递相机参数配置文件.yaml，需要给出.yaml的路径。
System.cc

/地图加载
void System::LoadMap(const string &filename, const string &strSettingsFile)
{
     //std::string strSettingsFile = "/home/whp/whp_ws/orbslam2/src/ORB_SLAM2-master/Examples/Monocular/TUM1.yaml";
     mySettingFile = strSettingsFile;
     SystemSetting* mySystemSetting = new SystemSetting(mpVocabulary);     
     mySystemSetting->LoadSystemSetting(mySettingFile);
     mpMap->Load(filename,mySystemSetting);
}

注意：LoadMap函数中的”mySettingFile"是在System.h，文件中声明的传递yaml文件的变量。在System.cc中，yaml文件的传递都是靠路径，也就是string类型的变量，所以这里”mySettingFile"的声明如下：
std::string mySettingsFile;
但是同时还要在System.cc的构造函数中，将yaml文件的路径传递给他：

mySettingsFile = strSettingsFile;

直接把路径写在那，也是完全OK的。

第六步：修改CMakeList.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文件中对应的示例程序中加入地图加载代码即可实现地图加载功能。
如在mono_tum.cc文件中改写如下：
注释掉建图和存图部分（不注释掉会引起程序崩溃），写入加载地图部分，代码如下：

//加载地图   
    // Create SLAM system. 创建ORB系统对象.
    //It initializes all system threads and gets ready to process frames.
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);
    //SLAM.LoadMap("/home/whp/whp_ws/whpslam/ORB_SLAM2_whp/version_0.2/Examples/Monocular/map.bin");
    SLAM.LoadMap("map.bin",argv[2]);
    cv::waitKey();
     // Stop all threads关闭SLAM系统
    SLAM.Shutdown();