ORB-SLAM2地图加载

前一篇讲了地图保存，经测试是可以使用的，现在进行之前保存地图的读取模式，但是这个代码会有几个问题，后续还得慢慢解决。

首先在Map.h加入函数的定义，Map.cc中用到SystemSetting，Map.h中需要加入SystemSetting.h,也需要添加“class SystemSetting;”后面会涉及到。

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

下面是加载主函数Load的构成：

void Map::Load ( const string &filename, SystemSetting* mySystemSetting )
 {
     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;
     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!"<<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;
 }

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

　　下面是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<float>(0), sizeof(float));
         f.read((char*)&Position.at<float>(1), sizeof(float));
         f.read((char*)&Position.at<float>(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.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);

      // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
      unique_lock<mutex> 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<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;

 //    for ( int i = 0; i < 4; i ++ )
 //    {
 //      for ( int j = 0; j < 4; j ++ )
 //      {
 //              f.read((char*)&T.at<float>(i,j), sizeof(float));
 //              cerr<<"T.at<float>("<<i<<","<<j<<"):"<<T.at<float>(i,j)<<endl;
 //      }
 //    }

     //读取当前关键帧特征点的数目；
     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));
     //依次读取当前关键帧的特征点和描述符；
     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);

         //根据需要读取特征点的深度值；
         //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<unsigned char>(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<float>(initkf.N,-1);
     initkf.vDepth = vector<float>(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.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 ++;
  }

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

在MapPoint.h中添加函数定义

KeyFrame* SetReferenceKeyFrame(KeyFrame* RFKF);  

在MapPoint.cc中添加

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

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

创建新的头文件SystemSetting.h

#ifndef SYSTEMSETTING_H
#define SYSTEMSETTING_H

#include <string>
#include "ORBVocabulary.h"
#include<opencv2/core/core.hpp>

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

SystemSetting.cc的函数具体实现：

#include <iostream>

#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<<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，并在keyframe.h中加上InitKeyFrame头文件,还需要加上“class InitKeyFrame”.

#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.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<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.h中添加函数定义：

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

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

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

还需要在System.h中添加声明

std::string mySettingsFile;

同时添加构造函数

mySettingsFile = strSettingsFile;

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

会出现什么样的问题呢？如果在重新播放之前的场景，并不能进行重定位，同一个特征点可能会被认为是不同的点，即在空间中会产生两个点。导致地图错位！但是，本篇博客主要是记载如何进行mapName.bin的地图加载，其他工作还得完善。

这是直接加载的之前一篇文章的地图：

没有办法进行重定位，一直都是下图的状态：

参考博客地址：
https://blog.csdn.net/u012177641/article/details/78802315
http://www.cnblogs.com/mafuqiang/p/6972841.html