（一次性搞定）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 "<<filename <<endl;
    ofstream f;
    f.open(filename.c_str(), ios_base::out|ios::binary);

    //Number of MapPoints
    unsigned long int nMapPoints = mspMapPoints.size();
    f.write((char*)&nMapPoints, sizeof(nMapPoints) );
    //Save MapPoint sequentially
    for ( auto mp: mspMapPoints ){
        //Save MapPoint
        SaveMapPoint( f, mp );
        // cerr << "Map.cc :: Saving map point number: " << mp->mnId << endl;
    }

    //Print The number of MapPoints
    cerr << "Map.cc :: The number of MapPoints is :"<<mspMapPoints.size()<<endl;
        

    //Grab the index of each MapPoint, count from 0, in which we initialized mmpnMapPointsIdx  
    GetMapPointsIdx(); 

    //Print the number of KeyFrames
    cerr <<"Map.cc :: The number of KeyFrames:"<<mspKeyFrames.size()<<endl;
    //Number of KeyFrames
    unsigned long int nKeyFrames = mspKeyFrames.size();
    f.write((char*)&nKeyFrames, sizeof(nKeyFrames));

    //Save KeyFrames sequentially
    for ( auto kf: mspKeyFrames )
        SaveKeyFrame( f, kf );

    for (auto kf:mspKeyFrames )
    {
        //Get parent of current KeyFrame and save the ID of this parent
        KeyFrame* parent = kf->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!"<<endl;
}

存储地图点函数——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<float>(0),sizeof(float));
    f.write((char*)& mpWorldPos.at<float>(1),sizeof(float));
    f.write((char*)& mpWorldPos.at<float>(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 <<endl;
    f.write((char*)&kf->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<float>(i,j), sizeof(float));
    //         //cerr<<"Tcw.at<float>("<<i<<","<<j<<"):"<<Tcw.at<float>(i,j)<<endl;
    //     }
    // }

    //Save the rotation matrix in Quaternion
    std::vector<float> 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<float>(i,3),sizeof(float));




    //Save the size of the ORB features current KeyFrame
    //cerr<<"kf->N:"<<kf->N<<endl;
    f.write((char*)&kf->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->mDescrip