[学习SLAM]ORB-SLAM2 地图保存与加载 基础构建

一、简介

　　在ORB-SLAM2的System.h文件中，有这样一句话：// TODO: Save/Load functions，让读者自己实现地图的保存与加载功能。其实在应用过程中很多场合同样需要先保存当前场景的地图，然后下次启动时直接进行跟踪，这样避免了初始化和建图，减小相机跟踪过程中计算机负载，还有就是进行全场的定位。其实网上已经有地图保存的代码了https://github.com/xiaopengsu/ORB_SLAM2_map

二、地图元素分析

　　所谓地图保存，就是保存地图“Map”中的各个元素，以及它们之间的关系，凡是跟踪过程中需要用到的东西自然也就是需要保存的对象，上一节曾经说过地图主要包含关键帧、3D地图点、BoW向量、共视图、生长树等，在跟踪过程中有三种跟踪模型和局部地图跟踪等过程，局部地图跟踪需要用到3D地图点、共视关系等元素，参考帧模型需要用到关键帧的BoW向量，重定位需要用到BoW向量、3D点等（具体哪里用到了需要翻看代码），所以基本上述元素都需要保存。

　　另一方面，关键帧也是一个抽象的概念（一个类），我们看看具体包含什么（其实都在关键帧类里面了），关键帧是从普通帧来的，所以来了视频帧首先需要做的就是检测特征点，计算描述符，还有当前帧的相机位姿，作为关键帧之后需要有对应的ID编号，以及特征点进行三角化之后的3D地图点等。

　　关于3D地图点需要保存的就只有世界坐标了，至于其它的关联关系可以重关键帧获得。需要单独说的是在关键帧类中包含了特征点和描述符，所以BoW向量是不需要保存的（也没办法保存），只需要在加载了关键帧之后利用特征描述符重新计算即可。

　　所以现在需要保存的东西包括关键帧、3D地图点、共视图、生长树。

 地图保存

三、地图保存代码实例

保存的代码主要是在Map.h和Map.cc敲，根据代码里的保存格式，应该是保存成二进制文件，对应的代码敲进对应的位置后，我在system.cc中定义了一个save函数去调用map中的save函数，然后在ros_mono.cc中调用了system中的save函数，可能我这样的做法有点太绕了，或许可以直接在ros_mono中调用map函数：

• 在system.h和system.cc中分别添加声明和定义：

     void SaveMap(const string &filename);  
      
    void System::SaveMap(const string &filename)  
    {  
       mpMap->Save(filename);   
    }  

• 然后在ros_mono.cc中添加

    char IsSaveMap;  
        cout << "Do you want to save the map?(Y/N)" << endl;  
        cin >> IsSaveMap;  
        if(IsSaveMap == 'Y' || IsSaveMap == 'y')  
            SLAM.SaveMap("MapPointandKeyFrame.bin");  

IsSaveMap是一个flag，让你确定是否保存地图的，当然这里输入Y或者y就是保存，输入其他的字符都是不保存啦，写得不是很规范。这样MapPoint 和KeyFrame都可以保存在了执行目录下的MapPointandKeyFrame.bin文件中了(SLAM.SaveMap("Slam_latest_Map.bin");        // MapPointandKeyFrame.bin --Slam_latest_Map.bin)。

[参考https://www.cnblogs.com/mafuqiang/p/6972342.html ]

• 在Map.h中声明,在Map.cc定义

　　需要明确的是一般SLAM系统对地图的维护均在Map.cc这个函数类中，最终把地图保存成二进制文件，所以现在Map.h中声明几个函数吧：

public:
    void Save( const string &filename );
protected:
    void SaveMapPoint( ofstream &f, MapPoint* mp );
    void SaveKeyFrame( ofstream &f, KeyFrame* kf );

 　　下面关于Save函数的构成：

void Map::Save ( const string& filename )
 {
     cerr<<"Map Saving to "<<filename <<endl;
     ofstream f;
     f.open(filename.c_str(), ios_base::out|ios::binary);
     cerr << "The number of MapPoints is :"<<mspMapPoints.size()<<endl;
 
     //地图点的数目
     unsigned long int nMapPoints = mspMapPoints.size();
     f.write((char*)&nMapPoints, sizeof(nMapPoints) );
     //依次保存MapPoints
     for ( auto mp: mspMapPoints )
         SaveMapPoint( f, mp );

     //获取每一个MapPoints的索引值，即从0开始计数，初始化了mmpnMapPointsIdx  
     GetMapPointsIdx(); 

     cerr <<"The number of KeyFrames:"<<mspKeyFrames.size()<<endl;
     //关键帧的数目
     unsigned long int nKeyFrames = mspKeyFrames.size();
     f.write((char*)&nKeyFrames, sizeof(nKeyFrames));
 
     //依次保存关键帧KeyFrames
     for ( auto kf: mspKeyFrames )
         SaveKeyFrame( f, kf );
 
     for (auto kf:mspKeyFrames )
     {
         //获得当前关键帧的父节点，并保存父节点的ID
         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));
         //获得当前关键帧的关联关键帧的大小，并依次保存每一个关联关键帧的ID和weight；//共视图生长树，吗
         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));
         }
     }
 
     f.close();
     cerr<<"Map Saving Finished!"<<endl;
 }

　　可以看到，Save函数依次保存了地图点的数目、所有的地图点、关键帧的数目、所有关键帧、关键帧的生长树节点和关联关系；

　　下面是SaveMapPoint函数的构成：

void Map::SaveMapPoint( ofstream& f, MapPoint* mp)
{   
     //保存当前MapPoint的ID和世界坐标值
     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));
}

 

　　其实主要就是通过MapPoint类的GetWorldPos（）函数获取了地图点的坐标值并保存下来；

　　下面是SaveKeyFrame函数的构成：

void Map::SaveKeyFrame( ofstream &f, KeyFrame* kf )
 {
//保存当前关键帧的ID和时间戳
     f.write((char*)&kf->mnId, sizeof(kf->mnId));
     f.write((char*)&kf->mTimeStamp, sizeof(kf->mTimeStamp));
     //保存当前关键帧的位姿矩阵
     cv::Mat Tcw = kf->GetPose();
     //通过四元数保存旋转矩阵
     std::vector<float> Quat = Converter::toQuaternion(Tcw);
     for ( int i = 0; i < 4; i ++ )
         f.write((char*)&Quat[i],sizeof(float));
     //保存平移矩阵
     for ( int i = 0; i < 3; i ++ )
         f.write((char*)&Tcw.at<float>(i,3),sizeof(float));
 
 
     //直接保存旋转矩阵
 //  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));
 //              /