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依賴並從攝像頭讀取圖像的時候，這個最后再說。后面繼續分享地圖的加載部分。