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上的视频测试时是没问题的。
