zoukankan      html  css  js  c++  java
  • 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上的视频测试时是没问题的。

  • 相关阅读:
    POJ 3630 Phone List/POJ 1056 【字典树】
    HDU 1074 Doing Homework【状态压缩DP】
    POJ 1077 Eight【八数码问题】
    状态压缩 POJ 1185 炮兵阵地【状态压缩DP】
    POJ 1806 Manhattan 2025
    POJ 3667 Hotel【经典的线段树】
    状态压缩 POJ 3254 Corn Fields【dp 状态压缩】
    ZOJ 3468 Dice War【PD求概率】
    POJ 2479 Maximum sum【求两个不重叠的连续子串的最大和】
    POJ 3735 Training little cats【矩阵的快速求幂】
  • 原文地址:https://www.cnblogs.com/mafuqiang/p/6972841.html
Copyright © 2011-2022 走看看