ORB-SLAM2的地图保存

“女生不适合弄硬件”,这句话真的深有体会,今天为了让Pioneer 3-AT给Kinect供电,真的是路途坎坷.首先机器人尘封很久,螺丝都是拧死的,LZ拿着螺丝头去拧,发现螺丝纹丝不动,好吧!需要师兄的协助.找到供电板的12V电源接口,因为操作问题…导致保险丝熔断,LZ的心情啊…默默的买了对应的保险丝,心里想着还是乖乖写代码吧…

回归正题,如文章题目所示,如何保存ORB-SLAM的地图?

如何保存地图?肯定是要先明白在ORB-SLAM中的地图到底有哪些变量?

在之前源码分析和一些跑ORB-SLAM的的显示图像,我们可以看到

地图主要可见的有关键帧(包括相机的pose,相机的内参,ORB特征),3D的地图点( 空间中3D位置,法线方向,ORB的描述子),词袋向量,共视图等.可以看到如果要保存地图点,保存的数据还是相当大的,下面我们就按照http://www.cnblogs.com/mafuqiang/p/6972342.html这个博客的步骤一点点来实现地图的保存操作.

首先要明确一点的是一般SLAM对地图维护的操作均在Map.cc这个函数类中,最终按照博客的想法是把最终的地图保存成二进制的文件（filename.bin）这种类型，如果是大神请绕道，讲的有些确实很基础。

所以，首先要在Map.h中声明几个函数，因为后续添加了Converter.h中的类别，在Map.h的头文件中要添加Converter.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 "<//输出地图点的数目
     cerr << "The number of MapPoints is :"<//地图点的数目
     unsigned long int nMapPoints = mspMapPoints.size();
     f.write((char*)&nMapPoints, sizeof(nMapPoints) );
     //依次保存MapPoints
     for ( auto mp: mspMapPoints )
     //调用SaveMapPoint函数
         SaveMapPoint( f, mp );

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

    //输出关键帧的数量
     cerr <<"The number of KeyFrames:"<//关键帧的数目
     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!"<

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

　　下面是SaveMapPoint函数的构成：

void Map::SaveMapPoint( ofstream& f, MapPoint* mp)
{   
     //保存当前MapPoint的ID和世界坐标值,x,y z
     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(i,j), sizeof(float));
 //              //cerr<<"Tcw.at("<(i,j)<
 //      }
 //    }

     //保存当前关键帧包含的ORB特征数目
     //cerr<<"kf->N:"<N<
     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函数的构成为，它的作用是初始化成员变量,在Map.h中添加成员变量：

protected：
    std::mapunsigned long int> mmpnMapPointsIdx;
    void GetMapPointsIdx()；

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

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

另外，关于旋转矩阵的存储可以通过四元数或矩阵的形式存储，如果使用四元数需要自定义一个矩阵和四元数相互转换的函数，在Converter.cc类里面ORB_SLAM2已经定义好了，所以直接使用就行了。

在Converter.h里面加上如下函数定义

 static cv::Mat toCvMat( const std::vector<float>& v )；

在Converter.cc中加入

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

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

 void SaveMap(const string &filename);  

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

以上就是地图保存的代码，LZ在ros下和rgbd的example下都经过测试，代码是可以保存的，最后再rgbd_tum.cc中加入

SLAM.SaveMap("MapPointandKeyFrame.bin")

在运行的路径下就会有存有地图的二进制文件了。哈哈，运行结果如下图所示O(∩_∩)O哈哈~