ORB-SLAM2 地圖加載

一、前面說了ORB-SLAM地圖的保存部分，繼續說地圖如何加載，因為加載部分相比保存要稍微復雜一些，所以要多說一點。

二、ORB-SLAM2地圖加載構成

　　首先同樣是在頭文件中聲明加載函數，包含地圖點和關鍵幀類的加載。　　

void Load( const string &filename, SystemSetting* mySystemSetting );
MapPoint* LoadMapPoint( ifstream &f );
KeyFrame* LoadKeyFrame( ifstream &f, SystemSetting* mySystemSetting );

　　下面先是加載主函數Load的構成，關於SystemSetting類后面再說：

void Map::Load ( const string &filename, SystemSetting* mySystemSetting )
 {
     cerr << "Map reading from:"<<filename<<endl;
     ifstream f;
     f.open( filename.c_str() );
 
     //按照保存的順序，先讀取MapPoints的數目；
     unsigned long int nMapPoints;
     f.read((char*)&nMapPoints, sizeof(nMapPoints));
 
     //依次讀取每一個MapPoints，並將其加入到地圖中
     cerr<<"The number of MapPoints:"<<nMapPoints<<endl;
     for ( unsigned int i = 0; i < nMapPoints; i ++ )
     {
         MapPoint* mp = LoadMapPoint(f);
         AddMapPoint(mp);
     }
 
     //獲取所有的MapPoints；
     std::vector<MapPoint*> vmp = GetAllMapPoints();
 
     //讀取關鍵幀的數目；
     unsigned long int nKeyFrames;
     f.read((char*)&nKeyFrames, sizeof(nKeyFrames));
     cerr<<"The number of KeyFrames:"<<nKeyFrames<<endl;
 
     //依次讀取每一關鍵幀，並加入到地圖；
     vector<KeyFrame*>kf_by_order;
     for( unsigned int i = 0; i < nKeyFrames; i ++ )
     {
         KeyFrame* kf = LoadKeyFrame(f, mySystemSetting);
         AddKeyFrame(kf);
         kf_by_order.push_back(kf);
     }
 
     cerr<<"KeyFrame Load OVER!"<<endl;
     //讀取生長樹；
     map<unsigned long int, KeyFrame*> kf_by_id;
     for ( auto kf: mspKeyFrames )
         kf_by_id[kf->mnId] = kf;
     cerr<<"Start Load The Parent!"<<endl;
     for( auto kf: kf_by_order )
     {
         //讀取當前關鍵幀的父節點ID；
         unsigned long int parent_id;
         f.read((char*)&parent_id, sizeof(parent_id));
 
         //給當前關鍵幀添加父節點關鍵幀；
         if ( parent_id != ULONG_MAX )
             kf->ChangeParent(kf_by_id[parent_id]);
 
         //讀取當前關鍵幀的關聯關系；
         //先讀取當前關鍵幀的關聯關鍵幀的數目；
         unsigned long int nb_con;
         f.read((char*)&nb_con, sizeof(nb_con));
         //然后讀取每一個關聯關鍵幀的ID和weight，並把該關聯關鍵幀加入關系圖中；
         for ( unsigned long int i = 0; i < nb_con; i ++ )
         {
             unsigned long int id;
             int weight;
             f.read((char*)&id, sizeof(id));
             f.read((char*)&weight, sizeof(weight));
             kf->AddConnection(kf_by_id[id],weight);
         }
    }
    cerr<<"Parent Load OVER!"<<endl;
    for ( auto mp: vmp )
    {
        if(mp)
        {
             mp->ComputeDistinctiveDescriptors();
             mp->UpdateNormalAndDepth();
         }
    }
     f.close();
     cerr<<"Load IS OVER!"<<endl;
     return;
 }

 　　其過程就是根據保存的順序依次加載地圖點的數目、地圖點、關鍵幀的數目、關鍵幀、生長樹和關聯關系。

　　下面是LoadMapPoints函數的構成：

 MapPoint* Map::LoadMapPoint( ifstream &f )
 {
         //主要包括MapPoints的位姿和ID；
         cv::Mat Position(3,1,CV_32F);
         long unsigned int id;
         f.read((char*)&id, sizeof(id));
 
         f.read((char*)&Position.at<float>(0), sizeof(float));
         f.read((char*)&Position.at<float>(1), sizeof(float));
         f.read((char*)&Position.at<float>(2), sizeof(float));
 
         //初始化一個MapPoint，並設置其ID和Position；
         MapPoint* mp = new MapPoint(Position, this );
         mp->mnId = id;
         mp->SetWorldPos( Position );
 
         return mp;
 }

 　　從這里開始涉及到了MapPoint類的初始化問題，由於這里只有Position以及當前的Map，所以需要從新定義MapPoint的構造函數，分別加入到MapPoint的頭文件和源文件中：

  MapPoint( const cv::Mat& Pos, Map* pMap );

  MapPoint::MapPoint(const cv::Mat &Pos, Map* pMap):
      mnFirstKFid(0), mnFirstFrame(0), nObs(0), mnTrackReferenceForFrame(0), mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0),
      mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(static_cast<KeyFrame*>(NULL)), mnVisible(1), mnFound(1), mbBad(false),
      mpReplaced(static_cast<MapPoint*>(NULL)), mfMinDistance(0), mfMaxDistance(0), mpMap(pMap)
  {
      Pos.copyTo(mWorldPos);
      mNormalVector = cv::Mat::zeros(3,1,CV_32F);
  
      // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
      unique_lock<mutex> lock(mpMap->mMutexPointCreation);
      mnId=nNextId++;
  }

　　緊接着是LoadKeyFrame函數的構成，這里由於KeyFrame類需要的初始化信息比較多，因此定義了一個InitKeyFrame類，它通過SystemSetting進行初始化，二SystemSetting的主要作用就是讀取設置文件（相機內參，ORB特征參數等），后面將給出SystemSetting和InitKeyFrame類的代碼：

 KeyFrame* Map::LoadKeyFrame( ifstream &f, SystemSetting* mySystemSetting )
 {
     //聲明一個初始化關鍵幀的類initkf；
     InitKeyFrame initkf(*mySystemSetting);
 
     //按照保存次序，依次讀取關鍵幀的ID和時間戳；
     f.read((char*)&initkf.nId, sizeof(initkf.nId));
     f.read((char*)&initkf.TimeStamp, sizeof(double));
 
     //讀取關鍵幀位姿矩陣；
     cv::Mat T = cv::Mat::zeros(4,4,CV_32F);
     std::vector<float> Quat(4);
     //Quat.reserve(4);
     for ( int i = 0; i < 4; i ++ )
         f.read((char*)&Quat[i],sizeof(float));
     cv::Mat R = Converter::toCvMat( Quat );
     for ( int i = 0; i < 3; i ++ )
         f.read((char*)&T.at<float>(i,3),sizeof(float));
     for ( int i = 0; i < 3; i ++ )
         for ( int j = 0; j < 3; j ++ )
             T.at<float>(i,j) = R.at<float>(i,j);
     T.at<float>(3,3) = 1;
 
 //    for ( int i = 0; i < 4; i ++ )
 //    {
 //      for ( int j = 0; j < 4; j ++ )
 //      {
 //              f.read((char*)&T.at<float>(i,j), sizeof(float));
 //              cerr<<"T.at<float>("<<i<<","<<j<<"):"<<T.at<float>(i,j)<<endl;
 //      }
 //    }
 
     //讀取當前關鍵幀特征點的數目；
     f.read((char*)&initkf.N, sizeof(initkf.N));
     initkf.vKps.reserve(initkf.N);
     initkf.Descriptors.create(initkf.N, 32, CV_8UC1);
     vector<float>KeypointDepth;
 
     std::vector<MapPoint*> vpMapPoints;
     vpMapPoints = vector<MapPoint*>(initkf.N,static_cast<MapPoint*>(NULL));
     //依次讀取當前關鍵幀的特征點和描述符；
     std::vector<MapPoint*> vmp = GetAllMapPoints();
     for(int i = 0; i < initkf.N; i ++ )
     {
         cv::KeyPoint kp;
         f.read((char*)&kp.pt.x, sizeof(kp.pt.x));
         f.read((char*)&kp.pt.y, sizeof(kp.pt.y));
         f.read((char*)&kp.size, sizeof(kp.size));
         f.read((char*)&kp.angle,sizeof(kp.angle));
         f.read((char*)&kp.response, sizeof(kp.response));
         f.read((char*)&kp.octave, sizeof(kp.octave));
 
         initkf.vKps.push_back(kp);
 
         //根據需要讀取特征點的深度值；
         //float fDepthValue = 0.0;
         //f.read((char*)&fDepthValue, sizeof(float));
         //KeypointDepth.push_back(fDepthValue);
 
         //讀取當前特征點的描述符；
         for ( int j = 0; j < 32; j ++ )
                 f.read((char*)&initkf.Descriptors.at<unsigned char>(i,j),sizeof(char));
 
         //讀取當前特征點和MapPoints的對應關系；
         unsigned long int mpidx;
         f.read((char*)&mpidx, sizeof(mpidx));
 
         //從vmp這個所有的MapPoints中查找當前關鍵幀的MapPoint，並插入
         if( mpidx == ULONG_MAX )
                 vpMapPoints[i] = NULL;
         else
                 vpMapPoints[i] = vmp[mpidx];
     }
 
     initkf.vRight = vector<float>(initkf.N,-1);
     initkf.vDepth = vector<float>(initkf.N,-1);
     //initkf.vDepth = KeypointDepth;
     initkf.UndistortKeyPoints();
     initkf.AssignFeaturesToGrid();
 
     //使用initkf初始化一個關鍵幀，並設置相關參數
     KeyFrame* kf = new KeyFrame( initkf, this, NULL, vpMapPoints );
     kf->mnId = initkf.nId;
     kf->SetPose(T);
     kf->ComputeBoW();

     for ( int i = 0; i < initkf.N; i ++ )
     {
         if ( vpMapPoints[i] )
         {
             vpMapPoints[i]->AddObservation(kf,i);
             if( !vpMapPoints[i]->GetReferenceKeyFrame())
                 vpMapPoints[i]->SetReferenceKeyFrame(kf);
         }
     }
     return kf;
 }

　　從文件中讀取的內容同保存的一致，同時由於是通過InitKeyFrame初始化的關鍵幀類KeyFrame，因此這里同樣需要添加構造函數以及初始化方式：

KeyFrame(InitKeyFrame &initkf, Map* pMap, KeyFrameDatabase* pKFDB,vector< MapPoint*>& vpMapPoints);


KeyFrame::KeyFrame(InitKeyFrame &initkf, Map *pMap, KeyFrameDatabase *pKFDB, vector<MapPoint*> &vpMapPoints):
      mnFrameId(0), mTimeStamp(initkf.TimeStamp), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS),
      mfGridElementWidthInv(initkf.fGridElementWidthInv), mfGridElementHeightInv(initkf.fGridElementHeightInv),
      mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0),
      mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnBAGlobalForKF(0),
      fx(initkf.fx), fy(initkf.fy), cx(initkf.cx), cy(initkf.cy), invfx(initkf.invfx),
      invfy(initkf.invfy), mbf(initkf.bf), mb(initkf.b), mThDepth(initkf.ThDepth), N(initkf.N),
      mvKeys(initkf.vKps), mvKeysUn(initkf.vKpsUn), mvuRight(initkf.vRight), mvDepth(initkf.vDepth),
      mDescriptors(initkf.Descriptors.clone()), mBowVec(initkf.BowVec), mFeatVec(initkf.FeatVec),
      mnScaleLevels(initkf.nScaleLevels), mfScaleFactor(initkf.fScaleFactor), mfLogScaleFactor(initkf.fLogScaleFactor),
      mvScaleFactors(initkf.vScaleFactors), mvLevelSigma2(initkf.vLevelSigma2),mvInvLevelSigma2(initkf.vInvLevelSigma2),
      mnMinX(initkf.nMinX), mnMinY(initkf.nMinY), mnMaxX(initkf.nMaxX), mnMaxY(initkf.nMaxY), mK(initkf.K),
      mvpMapPoints(vpMapPoints), mpKeyFrameDB(pKFDB), mpORBvocabulary(initkf.pVocabulary),
      mbFirstConnection(true), mpParent(NULL), mbNotErase(false), mbToBeErased(false), mbBad(false),
      mHalfBaseline(initkf.b/2), mpMap(pMap)
  {
    mnId = nNextId ++;
  }

 　　加載了一個關鍵幀之后還需要計算其BoW向量等操作，同時指定關鍵幀對地圖點的觀測。

三、總結

　　加載的過程大概就是這樣，時間太晚了，下次在給出InitKeyFrame和SystemSetting兩個類的代碼，以及其它修改的地方。總結來看，加載時關鍵幀類和地圖點類的初始化是比較煩人的，各種繁瑣的參數需要設置，所以很可能存在什么bug也說不定。另外，博客里面的代碼部分是參考的網上的代碼，部分是自己寫的，希望有人看到了有什么錯誤及時指正。在數據庫rgbd_dataset_freiburg1_xyz上的視頻測試時是沒問題的。