一、简介

　　在ORB-SLAM2的System.h文件中，有这样一句话：// TODO: Save/Load functions，让读者自己实现地图的保存与加载功能。其实在应用过程中很多场合同样需要先保存当前场景的地图，然后下次启动时直接进行跟踪，这样避免了初始化和建图，减小相机跟踪过程中计算机负载，还有就是进行全场的定位。今天暂时描述一下如何进行地图的保存，其实网上已经有地图保存的代码了（http://recherche.enac.fr/~drouin/slam/orbslam2/poine_orbslam2_04_07_16.tgz，不保证有效），有时间我上传两份（其实是一份）网上代码，但是由于只有代码，所以小菜给配一个教程。

二、地图元素分析

　　所谓地图保存，就是保存地图“Map”中的各个元素，以及它们之间的关系，凡是跟踪过程中需要用到的东西自然也就是需要保存的对象，上一节曾经说过地图主要包含关键帧、3D地图点、BoW向量、共视图、生长树等，在跟踪过程中有三种跟踪模型和局部地图跟踪等过程，局部地图跟踪需要用到3D地图点、共视关系等元素，参考帧模型需要用到关键帧的BoW向量，重定位需要用到BoW向量、3D点等（具体哪里用到了需要翻看代码），所以基本上述元素都需要保存。

　　另一方面，关键帧也是一个抽象的概念（一个类），我们看看具体包含什么（其实都在关键帧类里面了），关键帧是从普通帧来的，所以来了视频帧首先需要做的就是检测特征点，计算描述符，还有当前帧的相机位姿，作为关键帧之后需要有对应的ID编号，以及特征点进行三角化之后的3D地图点等。

　　关于3D地图点需要保存的就只有世界坐标了，至于其它的关联关系可以重关键帧获得。需要单独说的是在关键帧类中包含了特征点和描述符，所以BoW向量是不需要保存的（也没办法保存），只需要在加载了关键帧之后利用特征描述符重新计算即可。

　　所以现在需要保存的东西包括关键帧、3D地图点、共视图、生长树。

三、地图保存代码实例

　　需要明确的是一般SLAM系统对地图的维护均在Map.cc这个函数类中，最终把地图保存成二进制文件，所以现在Map.h中声明几个函数吧：

public:
void Save( const string &filename );
protected:
void SaveMapPoint( ofstream &f, MapPoint* mp );
void SaveKeyFrame( ofstream &f, KeyFrame* kf );

　　下面关于Save函数的构成：

void Map::Save ( const string& filename )
{
cerr<<"Map Saving to "<<filename <<endl;
ofstream f;
f.open(filename.c_str(), ios_base::out|ios::binary);
cerr << "The number of MapPoints is :"<<mspMapPoints.size()<<endl;

 //地图点的数目  
 unsigned long int nMapPoints = mspMapPoints.size();  
 f.write((char\*)&nMapPoints, sizeof(nMapPoints) );  
 //依次保存MapPoints  
 for ( auto mp: mspMapPoints )  
     SaveMapPoint( f, mp );  
 //获取每一个MapPoints的索引值，即从0开始计数，初始化了mmpnMapPointsIdx    
 GetMapPointsIdx();   
 cerr <<"The number of KeyFrames:"<<mspKeyFrames.size()<<endl;  
 //关键帧的数目  
 unsigned long int nKeyFrames = mspKeyFrames.size();  
 f.write((char\*)&nKeyFrames, sizeof(nKeyFrames));

 //依次保存关键帧KeyFrames  
 for ( auto kf: mspKeyFrames )  
     SaveKeyFrame( f, kf );

 for (auto kf:mspKeyFrames )  
 {  
     //获得当前关键帧的父节点，并保存父节点的ID  
     KeyFrame\* parent = kf->GetParent();  
     unsigned long int parent\_id = ULONG\_MAX;  
     if ( parent )  
         parent\_id = parent->mnId;  
     f.write((char\*)&parent\_id, sizeof(parent\_id));  
     //获得当前关键帧的关联关键帧的大小，并依次保存每一个关联关键帧的ID和weight；  
     unsigned long int nb\_con = kf->GetConnectedKeyFrames().size();  
     f.write((char\*)&nb\_con, sizeof(nb\_con));  
     for ( auto ckf: kf->GetConnectedKeyFrames())  
     {  
         int weight = kf->GetWeight(ckf);  
         f.write((char\*)&ckf->mnId, sizeof(ckf->mnId));  
         f.write((char\*)&weight, sizeof(weight));  
     }  
 }

 f.close();  
 cerr<<"Map Saving Finished!"<<endl;  

}

　　可以看到，Save函数依次保存了地图点的数目、所有的地图点、关键帧的数目、所有关键帧、关键帧的生长树节点和关联关系；

　　下面是SaveMapPoint函数的构成：

void Map::SaveMapPoint( ofstream& f, MapPoint* mp)
{
//保存当前MapPoint的ID和世界坐标值
f.write((char*)&mp->mnId, sizeof(mp->mnId));
cv::Mat mpWorldPos = mp->GetWorldPos();
f.write((char*)& mpWorldPos.at(),sizeof(float));
f.write((char*)& mpWorldPos.at(),sizeof(float));
f.write((char*)& mpWorldPos.at(),sizeof(float));
}

　　其实主要就是通过MapPoint类的GetWorldPos（）函数获取了地图点的坐标值并保存下来；

　　下面是SaveKeyFrame函数的构成：

void Map::SaveKeyFrame( ofstream &f, KeyFrame* kf )
{
//保存当前关键帧的ID和时间戳
f.write((char*)&kf->mnId, sizeof(kf->mnId));
f.write((char*)&kf->mTimeStamp, sizeof(kf->mTimeStamp));
//保存当前关键帧的位姿矩阵
cv::Mat Tcw = kf->GetPose();
//通过四元数保存旋转矩阵
std::vector Quat = Converter::toQuaternion(Tcw);
for ( int i = ; i < ; i ++ ) f.write((char*)&Quat[i],sizeof(float)); //保存平移矩阵 for ( int i = ; i < ; i ++ ) f.write((char*)&Tcw.at(i,),sizeof(float));

 //直接保存旋转矩阵  

// for ( int i = 0; i < Tcw.rows; i ++ ) // { // for ( int j = 0; j < Tcw.cols; j ++ ) // { // f.write((char*)&Tcw.at(i,j), sizeof(float));
// //cerr<<"Tcw.at("<(i,j)<<endl;
// }
// }

 //保存当前关键帧包含的ORB特征数目  
 //cerr<<"kf->N:"<<kf->N<<endl;  
 f.write((char\*)&kf->N, sizeof(kf->N));  
 //保存每一个ORB特征点  
 for( int i = ; i < kf->N; i ++ )  
 {  
     cv::KeyPoint kp = kf->mvKeys\[i\];  
     f.write((char\*)&kp.pt.x, sizeof(kp.pt.x));  
     f.write((char\*)&kp.pt.y, sizeof(kp.pt.y));  
     f.write((char\*)&kp.size, sizeof(kp.size));  
     f.write((char\*)&kp.angle,sizeof(kp.angle));  
     f.write((char\*)&kp.response, sizeof(kp.response));  
     f.write((char\*)&kp.octave, sizeof(kp.octave));

     //保存当前特征点的描述符  
     for (int j = ; j < kf->mDescriptors.cols; j ++ )  
             f.write((char\*)&kf->mDescriptors.at<unsigned char>(i,j), sizeof(char));

     //保存当前ORB特征对应的MapPoints的索引值  
     unsigned long int mnIdx;  
     MapPoint\* mp = kf->GetMapPoint(i);  
     if (mp == NULL  )  
             mnIdx = ULONG\_MAX;  
     else  
             mnIdx = mmpnMapPointsIdx\[mp\];

     f.write((char\*)&mnIdx, sizeof(mnIdx));  
 }  

}

　　保存关键帧的函数稍微复杂一点，首先需要明白一幅关键帧包含特征点，描述符，以及哪些特征点通过三角化成为了地图点。

　　其中在Save函数中的GetMapPointsIdx函数的构成为，它的作用是初始化成员变量：

std::map mmpnMapPointsIdx;

这个成员变量存储的是特征点对应的地图点的索引值。

void Map::GetMapPointsIdx()
{
unique_lock lock(mMutexMap);
unsigned long int i = ;
for ( auto mp: mspMapPoints )
{
mmpnMapPointsIdx[mp] = i;
i += ;
}
}

另外，关于旋转矩阵的存储可以通过四元数或矩阵的形式存储，如果使用四元数需要自定义一个矩阵和四元数相互转换的函数，在Converter.cc类里面：

std::vector Converter::toQuaternion(const cv::Mat &M)
{
Eigen::Matrix eigMat = toMatrix3d(M);
Eigen::Quaterniond q(eigMat);

 std::vector<float> v();  
 v\[\] = q.x();  
 v\[\] = q.y();  
 v\[\] = q.z();  
 v\[\] = q.w();

 return v;  

}

cv::Mat Converter::toCvMat( const std::vector& v )
{
Eigen::Quaterniond q;
q.x() = v[];
q.y() = v[];
q.z() = v[];
q.w() = v[];
Eigen::MatrixeigMat(q);
cv::Mat M = toCvMat(eigMat);
return M;
}

三、总结

　　上面就是地图保存部分的代码，经过测试针对TUM的视频是有效的。但是需要在System中设置保存函数并在主函数中调用，尤其是针对无ROS依赖并从摄像头读取图像的时候，这个最后再说。后面继续分享地图的加载部分。