ORB-SLAM2 地图加载

Posted 2020-09-22 达达MFZ

一、前面说了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, max_id = 0;
     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);
         if ( mp->mnId >= max_id ) max_id = mp->mnId;
         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上的视频测试时是没问题的。