ORB-SLAM2地图保存、加载和用于定位

ORB-SLAM2在跟踪建图过程中得到的是稀疏地图，目前看来只能用于定位，源代码里没有地图保存和加载的模块，在此参考网上的多篇博客实现ORB-SLAM2稀疏点云地图的保存、加载以及用于定位（注意只能用于定位不能用于导航）。

网上关于地图保存和加载的代码有很多，根源上好像都是这一篇：https://www.cnblogs.com/mafuqiang/p/6972841.html

我主要参考的是这一篇：https://blog.csdn.net/u014709760/article/details/86319090

在上面这篇博客的基础上，我实现了地图保存、加载并用于定位，但这篇不介绍任何原理，只是填补一些操作细节。（尽管原博已经讲得够细了）

目录

1.地图保存

1.1 修改Map.h和Map.cc

Map.h的修改：

Map.cc的修改：

1.2 Converter.h和Conerter.cc的修改

Converter.h的修改：

Converter.cc的修改：

1.3 System.c和System.h的修改

System.h的修改：

System.cc的修改：

1.4 入口程序处的修改：

1.5 编译并测试

2.地图加载

2.1 Map.cc和Map.h的修改

Map.h的修改：

Map.cc的修改：

2.2 MapPoint.cc和MapPoint.h的修改

MapPoint.h的修改：

MapPoint.cc的修改：

2.3 KeyFrame.h和KeyFrame.cc的修改

KeyFrame.h的修改：

KeyFrame.cc的修改：

2.4 SystemSetting.h和SystemSetting.cc的创建

SystemSetting.h的创建：

SystemSetting.cc的创建：

2.5 InitKeyFrame.h和InitKeyFrame.cc的创建

InitKeyFrame.h的创建：

InitKeyFrame.cc的创建：

2.6 System.h和System.cc的修改：

System.h的修改：

System.cc的修改：

2.7 入口程序的修改

2.8 CMakeList.txt的修改：

2.9 编译并测试

3.地图加载后用于定位，补充一些选择机制

3.1修改Map.h和Map.cc

3.2修改System.cc

3.3修改Mono_tum.cc做一个选择的机制

4.问题

---

1.地图保存

1.1 修改Map.h和Map.cc

Map.h的修改：

添加如下头文件、成员变量、函数：

#include "Converter.h"

public:
    //保存地图信息函数
    void Save(const string &filename);
    //获取地图点ID
    void GetMapPointsIdx();
    //添加成员变量
    std::map mmpnMapPointsIdx;
    
protected:
    //保存地图点和关键帧 
    void SaveMapPoint(ofstream &f,MapPoint* mp);
    void SaveKeyFrame(ofstream &f,KeyFrame* kf);

Map.cc的修改：

添加如下成员函数：

//保存地图信息
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!"<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));
}

//保存关键帧
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));
}

//获取地图点ID
void Map::GetMapPointsIdx()
{
    unique_lock lock(mMutexMap);
    unsigned long int i = 0;
    for ( auto mp: mspMapPoints )
    {
        mmpnMapPointsIdx[mp] = i;
        i += 1;
    }
}

1.2 Converter.h和Conerter.cc的修改

Converter.h的修改：

添加函数原型：

public:
static cv::Mat Converter::toCvMat(const std::vector& v);

Converter.cc的修改：

添加函数定义：

cv::Mat 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;
}

1.3 System.c和System.h的修改

System.h的修改：

添加函数原型：

public:
void SaveMap(const string &filename); 

System.cc的修改：

//地图保存
void System::SaveMap(const string &filename)
{
    mpMap->Save(filename);
}

1.4 入口程序处的修改：

在此以mono_tum.cc为例，调用SaveMap函数的位置可以在SLAM.Shutdown之前，也可在之后，其中参数是保存地图路径，需要根据自己的路径修改。

 SLAM.SaveMap("/home/greenhand/ORB_SLAM2-Mapsave/Examples/Monocular/map.bin");
 SLAM.Shutdown();

1.5 编译并测试

编译步骤与普通ORB-SLAM2的编译一样，执行./build.sh就可以了，之后按一般执行ORB-SLAM2的指令来执行即可，可看到如下结果：

然后发现路径里有一个map.bin文件

2.地图加载

先说明一点，这个地图加载步骤仅仅就是可视化一下而已，如果需要进行定位需要在地图加载的基础上进行微小的修改，读者如果想用于定位可以直接结合第三部分一起看。

2.1 Map.cc和Map.h的修改

Map.h的修改：

添加头文件、地图信息加载函数、地图点加载函数、关键帧加载函数的原型：

#include "SystemSetting.h"

//加载地图信息
public：
    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!"<(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;
}

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

2.3 KeyFrame.h和KeyFrame.cc的修改

KeyFrame.h的修改：

添加头文件、构造函数原型、类声明：

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



class InitKeyFrame;

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

2.4 SystemSetting.h和SystemSetting.cc的创建

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

2.5 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 
#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;
}

}

2.6 System.h和System.cc的修改：

System.h的修改：

添加成员函数原型和成员变量。

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

System.cc的修改：

添加函数定义，并在System的构造函数中加入以下那一句。

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

//在System构造函数里加入这一句
mySettingFile = strSettingsFile;

2.7 入口程序的修改

这一句添加在SLAM对象声明之后，循环读入图片之前，路径为第一部分测试保存的地图路径。

SLAM.LoadMap("/home/greenhand/ORB_SLAM2-Mapsave/Examples/Monocular/map.bin");

2.8 CMakeList.txt的修改：

在生成库时添加SystemSetting.CC和InitKeyFrame.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
)

2.9 编译并测试

像往常一样执行，这一步测试的前提是在上一步测试后，已经保存了一张地图。

我在这想讲一下一些可能出现的问题：在第一部分我们在入口文件里加入了保存地图代码，而这一次我们在入口文件里加入了加载地图代码，这一次测试是装载上一次保存后的地图。

那么系统先装载地图，然后按照正常流程执行，所以这一次测试之后，再测试时就会用加载地图后的地图，可能出现崩溃的情况。读者在这一步可以根据实际调试需要，灵活注释掉mono_tum.cc里的保存和加载语句，也可以自己写一些条件语句使自己可以选择。我个人的做法会在后面一部分讲。

3.地图加载后用于定位，补充一些选择机制

这一部分我和原博做的略有不同。

3.1修改Map.h和Map.cc

Map.h中添加头文件并修改Load函数原型：

#include "KeyFrameDatabase.h"

void Load(const string &filename,SystemSetting* mySystemSetting, KeyFrameDatabase* mpKeyFrameDatabase);

Map.cc修改Load函数定义，只需在原函数的一个循环结构末尾添加一句，就可将读取到的保存的关键帧加入KeyFrameDatabase

//函数头修改为和原型一致
void Map::Load ( const string &filename, SystemSetting* mySystemSetting,KeyFrameDatabase* mpKeyFrameDatabase)


//找到下面这个循环结构，添加一句
for( unsigned int i = 0; i < nKeyFrames; i ++ )
    {
        KeyFrame* kf = LoadKeyFrame(f, mySystemSetting);
        AddKeyFrame(kf);
        kf_by_order.push_back(kf);
        //将关键帧添加到关键帧数据库中
        mpKeyFrameDatabase->add(kf);
    }

3.2修改System.cc

将LoadMap函数修改为如下形式：

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

将cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp)这个函数中的控制仅定位模式总是置False的语句注释掉（关于注释哪个函数根据自己的需求来，我是跑单目就注释掉单目里的）

            mpTracker->InformOnlyTracking(true);
            //mbActivateLocalizationMode = false;把这句像这样注释掉

3.3修改Mono_tum.cc做一个选择的机制

其实我就是加了两个条件语句来让用户自己选择是否要保存地图、是否加载地图——>是否选择纯定位模式。这个代码也没什么好贴的，大家自己用个字符输入来做条件吧！

 

4.问题

说一下我遇到的问题。

1.有时会出现定位失败后一直卡在重启系统“SystemReseting"这一步骤，但有时又会很顺滑地Resetting成功。

2.在加载地图时卡在Start Load the parent这一步，有时又不会。

 

起初我以为是我与原博在定位方面做法不同导致的，但两种做法或多或少都会出现一些系统卡死问题，我猜测时因为虚拟机配置太低的问题。目前我也处于初学阶段，所以并没有什么解决办法，希望如果有人在实践并解决了类似的问题以后可以告诉我。