ORB-SLAM2 地圖加載
阿新 • • 發佈：2017-06-10
actor 獲取 看到了 mut 作用 char mes des cti
一、前面說了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上的視頻測試時是沒問題的。
ORB-SLAM2 地圖加載