ORB-SLAM2 地图保存

一、简介

　　在ORB-SLAM2的System.h文件中，有这样一句话：// TODO: Save/Load functions，让读者自己实现地图的保存与加载功能。其实在应用过程中很多场合同样需要先保存当前场景的地图，然后下次启动时直接进行跟踪，这样避免了初始化和建图，减小相机跟踪过程中计算机负载，还有就是进行全场的定位。今天暂时描述一下如何进行地图的保存，其实网上已经有地图保存的代码了（http://recherche.enac.fr/~drouin/slam/orbslam2/poine_orbslam2_04_07_16.tgz，不保证有效），有时间我上传两份（其实是一份）网上代码，但是由于只有代码，所以小菜给配一个教程。

二、地图元素分析

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

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

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

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

三、地图保存代码实例

　　需要明确的是一般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));
 //              //cerr<<"Tcw.at<float>("<<i<<","<<j<<"):"<<Tcw.at<float>(i,j)<<endl;
 //      }
 //    }
 
     //保存当前关键帧包含的ORB特征数目
     //cerr<<"kf->N:"<<kf->N<<endl;
     f.write((char*)&kf->N, sizeof(kf->N));
     //保存每一个ORB特征点
     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));
 
         //保存当前特征点的描述符
         for (int j = 0; j < kf->mDescriptors.cols; j ++ )
                 f.write((char*)&kf->mDescriptors.at<unsigned char>(i,j), sizeof(char));
 
         //保存当前ORB特征对应的MapPoints的索引值
         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函数中的GetMapPointsIdx函数的构成为，它的作用是初始化成员变量：

std::map<MapPoint*, unsigned long int> mmpnMapPointsIdx;

这个成员变量存储的是特征点对应的地图点的索引值。

  void Map::GetMapPointsIdx()
  {
          unique_lock<mutex> lock(mMutexMap);
          unsigned long int i = 0;
          for ( auto mp: mspMapPoints )
          {
                  mmpnMapPointsIdx[mp] = i;
                  i += 1;
          }
  }

 另外，关于旋转矩阵的存储可以通过四元数或矩阵的形式存储，如果使用四元数需要自定义一个矩阵和四元数相互转换的函数，在Converter.cc类里面：

 std::vector<float> Converter::toQuaternion(const cv::Mat &M)
 {   
     Eigen::Matrix<double,3,3> eigMat = toMatrix3d(M);
     Eigen::Quaterniond q(eigMat);
     
     std::vector<float> v(4);
     v[0] = q.x();
     v[1] = q.y();
     v[2] = q.z();
     v[3] = q.w();
     
     return v;
 }

 cv::Mat Converter::toCvMat( const std::vector<float>& v )
 {
         Eigen::Quaterniond q;
         q.x()  = v[0];
         q.y()  = v[1];
         q.z()  = v[2];
         q.w()  = v[3];
         Eigen::Matrix<double,3,3>eigMat(q);
         cv::Mat M = toCvMat(eigMat);
         return M;
 }

三、总结

　　上面就是地图保存部分的代码，经过测试针对TUM的视频是有效的。但是需要在System中设置保存函数并在主函数中调用，尤其是针对无ROS依赖并从摄像头读取图像的时候，这个最后再说。后面继续分享地图的加载部分。