一、前面说了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上的视频测试时是没问题的。
时间： 2024-10-12 16:00:02