ORB_SLAM2加载地图并重定位详细流程（ros版本）

1.开发背景 ：看第一篇去！地图保存链接
2.写作动力：被网友催了。最近真挺忙，不催我都忘了我还差一篇没写呢。
3.废话不多讲，开始搞，，，，，，额，还是先上图吧，如下加载后重定位效果，测试还不错。
数据总量500s，从200s处加载地图后播放数据，直接定位上了，当然，还测试过数据中其他时间点的重定位效果，都是一次性定上位的
4.步骤：

4.1.创建SystemSetting.h和SystemSetting.cc
创建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;
   }
}

4.2.创建InitKeyFrame.h和InitKeyFrame.cc
创建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;
}
 
}

4.3修改Map.cc和Map.h
修改Map.h：
在头文件中把，地图信息加载函数、地图点加载函数、关键帧加载函数的原型添加进去：

#include "SystemSetting.h"
 
#include "KeyFrameDatabase.h"

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

修改Map.cc:

void Map::Load(const string &filename, SystemSetting *mySystemSetting, KeyFrameDatabase *mpKeyFrameDatabase)
    {
        cerr << "Map reading from:" << filename << endl;
        ifstream f;
        f.open(filename.c_str());

        //按照保存的顺序，先读取MapPoints的数目；
        unsigned long int nMapPoints;
        f.read((char *)&nMapPoints, sizeof(nMapPoints));

        //依次读取每一个MapPoints，并将其加入到地图中
        cerr << "The number of MapPoints:" << nMapPoints << endl;
        for (unsigned int i = 0; i < nMapPoints; i++)
        {
            MapPoint *mp = LoadMapPoint(f);
            AddMapPoint(mp);
        }

        //获取所有的MapPoints；
        std::vector<MapPoint *> vmp = GetAllMapPoints();

        //读取关键帧的数目；
        unsigned long int nKeyFrames;
        f.read((char *)&nKeyFrames, sizeof(nKeyFrames));
        cerr << "The number of KeyFrames:" << nKeyFrames << endl;

        //依次读取每一关键帧，并加入到地图；
        vector<KeyFrame *> kf_by_order;
        // std::cout << __FUNCTION__ << __LINE__ << std::endl;
        for (unsigned int i = 0; i < nKeyFrames; i++)
        {
            // std::cout << __FUNCTION__ << __LINE__ << std::endl;
            KeyFrame *kf = LoadKeyFrame(f, mySystemSetting);
            // std::cout << __FUNCTION__ << __LINE__ << std::endl;
            AddKeyFrame(kf);
            kf_by_order.push_back(kf);

            //将关键帧添加到关键帧数据库中
            mpKeyFrameDatabase->add(kf);
        }
        cerr << "KeyFrame Load OVER!" << endl;
        //读取生长树；
        map<unsigned long int, KeyFrame *> kf_by_id;
        for (auto kf : mspKeyFrames)
            kf_by_id[kf->mnId] = kf;
        cerr << "Start Load The Parent!" << endl;
        for (auto kf : kf_by_order)
        {
            //读取当前关键帧的父节点ID；
            unsigned long int parent_id;
            f.read((char *)&parent_id, sizeof(parent_id));

            //给当前关键帧添加父节点关键帧；
            if (parent_id != ULONG_MAX)
                kf->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!" << endl;
        for (auto mp : vmp)
        {
            if (mp)
            {
                mp->ComputeDistinctiveDescriptors();
                mp->UpdateNormalAndDepth();
            }
        }
        f.close();
        cerr << "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;
}
 
 

4.4修改MapPoint.cc和MapPoint.h
修改MapPoint.h
添加构造函数、成员函数的声明：

public：
MapPoint(const cv::Mat &Pos,Map* pMap);
KeyFrame* SetReferenceKeyFrame(KeyFrame* RFKF);

修改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* MapPoint::SetReferenceKeyFrame(KeyFrame* RFKF)
{
    return mpRefKF = RFKF;
}

4.5修改KeyFrame.h和KeyFrame.cc

添加头文件、构造函数原型、类声明：
修改KeyFrame.h

#include "InitKeyFrame.h"
KeyFrame(InitKeyFrame &initkf,Map* pMap,KeyFrameDatabase* pKFDB,vector<MapPoint*>& vpMapPoints);
class InitKeyFrame;

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

4.6修改System.h和System.cc：
修改System.h：
添加成员函数和成员变量。

public:
//添加成员函数
void LoadMap(const string &filename);
//添加成员变量
std::string mySettingFile;

修改System.cc：
添加函数定义

    void System::LoadMap(const string &filename)
    {
        // SystemSetting *mySystemSetting = new SystemSetting(mpVocabulary);

        //设置定位模式
        char IsPureLocalization;
        cout << "是否开启纯定位模式？(y/n)" << endl;
        cin >> IsPureLocalization;
        if (IsPureLocalization == 'Y' || IsPureLocalization == 'y')
            ActivateLocalizationMode();
        //导入地图
        string strPathMap = filename;
        SystemSetting *mySystemSetting = new SystemSetting(mpVocabulary);
        mySystemSetting->LoadSystemSetting(mySettingFile);
        mpMap->Load(strPathMap, mySystemSetting, mpKeyFrameDatabase);
          
    }
 

在System构造函数里加入一句话，比如我的，添加在了构造函数最后一行。

namespace ORB_SLAM2
{

    System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
                   const bool bUseViewer) : mSensor(sensor), mpViewer(static_cast<Viewer *>(NULL)), mbReset(false), mbActivateLocalizationMode(false),
                                            mbDeactivateLocalizationMode(false)
    {
        // Output welcome message

        cout << "Input sensor was set to: ";

'
'
'
'
'
'
		//就这句，你添加在构造函数哪里都行
        mySettingFile = strSettingsFile;
    }
}

另外，将 cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp)
这个函数中的控制仅定位模式总是置False的语句注释掉

 mpTracker->InformOnlyTracking(true);
// mbActivateLocalizationMode = false;

4.7修改入口程序的
添加SLAM.LoadMap()函数入口，建议添在如下位置处。刚开始没有看，我添加在了rosSpin()后面，只有ctrh+c杀掉rosSpin()后才能进行加载，同理，添加在SLAM.ShutDown()后面也是只有在代码被停止运行时才能加载。

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);
    
    SLAM.LoadMap("/home/bill/work/3dslam/orb_slam2/MapPointandKeyFrame.bin");

4.8修改CMakeList.txt：
其实就是把你创建的SystemSetting.CC和InitKeyFrame.cc添加进来而已。

add_library(${PROJECT_NAME} SHARED
'''''省略'''''
src/InitKeyFrame.cc
src/SystemSetting.cc
)

总结：我这边当初编译时也是遇到了各种问题，不过后来好在编译通过。进行调试时除了遇到rosSpin()这个问题没仔细看，其他的还算顺利，加载地图并重定位实现起来效果也不错。

参考文献：
达达MFZ
达达MFZ