基于ORB

1. 基于ORB-SLAM2实现SLAM地图保存

本文记录了ORB_SLAM2中地图保存与加载的过程。
参考博客：
blogs./mafuqiang/p/6972342.html
blogs./mafuqiang/p/6972841.html
1.1 简介
在ORB-SLAM2的System.h文件中，有这样一句话：// TODO: Save/Load functions，让读者自己实现地图的保存与加载功能。其实在应用过程中很多场合同样需要先保存当前场景的地图，然后下次启动时直接进行跟踪，这样避免了初始化和建图，减小相机跟踪过程中计算机负载，还有就是进行全场的定位。今天暂时描述一下如何进行地图的保存，地图保存过程比较简单，只需要按照参考一步步修改源码即可。接下来我们一起来走一遍这个过程。
1.2 地图元素分析
所谓地图保存，就是保存地图“Map”中的各个元素，以及它们之间的关系，凡是跟踪过程中需要用到的东西自然也就是需要保存的对象，上一节曾经说过地图主要包含关键帧、3D地图点、BoW向量、共视图、生长树等，在跟踪过程中有三种跟踪模型和局部地图跟踪等过程，局部地图跟踪需要用到3D地图点、共视关系等元素，参考帧模型需要用到关键帧的BoW向量，重定位需要用到BoW向量、3D点等（具体哪里用到了需要翻看代码），所以基本上述元素都需要保存。
　　另一方面，关键帧也是一个抽象的概念（一个类），我们看看具体包含什么（其实都在关键帧类里面了），关键帧是从普通帧来的，所以来了视频帧首先需要做的就是检测特征点，计算描述符，还有当前帧的相机位姿，作为关键帧之后需要有对应的ID编号，以及特征点进行三角化之后的3D地图点等。
　　关于3D地图点需要保存的就只有世界坐标了，至于其它的关联关系可以重关键帧获得。需要单独说的是在关键帧类中包含了特征点和描述符，所以BoW向量是不需要保存的（也没办法保存），只需要在加载了关键帧之后利用特征描述符重新计算即可。
　　所以现在需要保存的东西包括关键帧、3D地图点、共视图、生长树。
1.3 修改源码实现地图保存
涉及Map.h Map. Converter.h Converter. System.h System.c 程序修改。
需要明确的是一般SLAM系统对地图的维护均在Map.这个函数类中，最终把地图保存成二进制文件，首先在Map.h中声明几个函数：
第一步：修改 Map.h
Map.h/class Map/public:

    //保存地图信息void Save(const string &filename);//wuhoup20190923

Map.h/class Map/protected::

     //保存地图点和关键帧 void SaveMapPoint(ofstream &f,MapPoint* mp);//存储地图点函数 void SaveKeyFrame(ofstream &f,KeyFrame* kf);//存储关键帧函数 void GetMapPointsIdx();//获取地图点ID std::map<MapPoint*,unsigned long int> mmpnMapPointsIdx;//地图点的ID号 

第二步：修改Map.
下面关于Save函数的构成：

//保存地图信息,包括关键帧、3D地图点、共视图、生长树。
//Save函数依次保存了地图点的数目、所有的地图点、关键帧的数目、所有关键帧、关键帧的生长树节点和关联关系；
void Map::Save ( const string& filename )//wuhoup20190923
{//Print the information of the saving mapcerr<<"Map. :: Map Saving to "<<filename <<endl;//驱动程序输入存储路径和名称ofstream f;f.open(filename.c_str(), ios_base::out|ios::binary);cerr<<"Map. ::The number of MapPoints is :"<<mspMapPoints.size()<<endl;//地图点的数目Number of MapPointsunsigned long int nMapPoints = mspMapPoints.size();f.write((char*)&nMapPoints, sizeof(nMapPoints) );//依次保存MapPoints, Save MapPoint sequentiallyfor ( auto mp: mspMapPoints ){//Save MapPointSaveMapPoint( f, mp );// cerr << "Map. :: Saving map point number: " << mp->mnId << endl;}//Print The number of MapPointscerr << "Map. :: The number of MapPoints is :"<<mspMapPoints.size()<<endl;//获取每一个MapPoints的索引值，即从0开始计数，初始化了mmpnMapPointsIdx //Grab the index of each MapPoint, count from 0, in which we initialized mmpnMapPointsIdx  GetMapPointsIdx(); //Print the number of KeyFramescerr <<"Map. :: The number of KeyFrames:"<<mspKeyFrames.size()<<endl;//关键帧的数目Number of KeyFramesunsigned long int nKeyFrames = mspKeyFrames.size();f.write((char*)&nKeyFrames, sizeof(nKeyFrames));//依次保存关键帧KeyFrames//Save KeyFrames sequentiallyfor ( auto kf: mspKeyFrames )SaveKeyFrame( f, kf );for (auto kf:mspKeyFrames ){//获得当前关键帧的父节点，并保存父节点的ID//Get parent of current KeyFrame and save the ID of this parentKeyFrame* 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；//Get the size of the Connected KeyFrames of the current KeyFrames//and then save the ID and weight of the Connected KeyFramesunsigned 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));}}// Save last Frame ID// SaveFrameID(f);f.close();cerr<<"Map. :: Map Saving Finished!"<<endl;
}

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

下面是SaveMapPoint函数的构成：

//存储地图点函数
//主要就是通过MapPoint类的GetWorldPos（）函数获取了地图点的坐标值并保存下来；
void Map::SaveMapPoint( ofstream& f, MapPoint* mp)//wuhoup20190923
{//保存当前MapPoint的ID和世界坐标值//Save ID and the x,y,z coordinates of the current MapPointf.write((char*)&mp->mnId, sizeof(mp->mnId));cv::Mat mpWorldPos = mp->GetWorldPos();f.write((char*)& mpWorldPos.at<float>(0),sizeof(float));f.write((char*)& mpWorldPos.at<float>(1),sizeof(float));f.write((char*)& mpWorldPos.at<float>(2),sizeof(float));
}

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

下面是SaveKeyFrame函数的构成：

//存储关键帧函数
//保存关键帧的函数稍微复杂一点，
//首先需要明白一幅关键帧包含特征点，描述符，以及哪些特征点通过三角化成为了地图点。
void Map::SaveKeyFrame( ofstream &f, KeyFrame* kf )//wuhoup20190923
{//保存当前关键帧的ID和时间戳//Save the ID and timesteps of current KeyFramef.write((char*)&kf->mnId, sizeof(kf->mnId));//cout << "saving kf->mnId = " << kf->mnId <<endl;f.write((char*)&kf->mTimeStamp, sizeof(kf->mTimeStamp));//保存当前关键帧的位姿矩阵//Save the Pose Matrix of current KeyFramecv::Mat Tcw = kf->GetPose();//直接保存旋转矩阵//Save the rotation matrix// for ( int i = 0; i < Tcw.rows; i ++ )// {//     for ( int j = 0; j < Tcw.cols; j ++ )//     {//         f.write((char*)&Tcw.at<float>(i,j), sizeof(float));//         //cerr<<"Tcw.at<float>("<<i<<","<<j<<"):"<<Tcw.at<float>(i,j)<<endl;//     }// }//通过四元数保存旋转矩阵//Save the rotation matrix in Quaternionstd::vector<float> Quat = Converter::toQuaternion(Tcw);for ( int i = 0; i < 4; i ++ )f.write((char*)&Quat[i],sizeof(float));//保存平移矩阵//Save the translation matrixfor ( int i = 0; i < 3; i ++ )f.write((char*)&Tcw.at<float>(i,3),sizeof(float));//保存当前关键帧包含的ORB特征数目//Save the size of the ORB features current KeyFrame//cerr<<"kf->N:"<<kf->N<<endl;f.write((char*)&kf->N, sizeof(kf->N));//保存每一个ORB特征点//Save each ORB featuresfor( int i = 0; 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));//保存当前特征点的描述符//Save the Descriptors of current ORB featuresf.write((char*)&kf->mDescriptors.cols, sizeof(kf->mDescriptors.cols)); //kf->mDescriptors.cols is always 32 here.for (int j = 0; j < kf->mDescriptors.cols; j ++ )f.write((char*)&kf->mDescriptors.at<unsigned char>(i,j), sizeof(char));//保存当前ORB特征对应的MapPoints的索引值//Save the index of MapPoints that corresponds to current ORB featuresunsigned long int mnIdx;MapPoint* mp = kf->GetMapPoint(i);if (mp == NULL  )mnIdx = ULONG_MAX;elsemnIdx = mmpnMapPointsIdx[mp];f.write((char*)&mnIdx, sizeof(mnIdx));}
}

保存关键帧的函数稍微复杂一点，首先需要明白一幅关键帧包含特征点，描述符，以及哪些特征点通过三角化成为了地图点。
　　其中在Save函数中的GetMapPointsIdx函数的构成如下，定义在Map.h中，它的作用是初始化成员变量：

std::map<MapPoint*,unsigned long int> mmpnMapPointsIdx;//地图点的ID号

这个成员变量存储的是特征点对应的地图点的索引值。
下面是**GetMapPointsIdx()**函数的实现过程

//获取地图点的ID号void Map::GetMapPointsIdx()//wuhoup20190923
{unique_lock<mutex> lock(mMutexMap);unsigned long int i = 0;for ( auto mp: mspMapPoints ){mmpnMapPointsIdx[mp] = i;i += 1;}
}

注意：增加头文件#include "Converter.h"
第三步：修改Converter. Converter.h程序
关于旋转矩阵的存储可以通过四元数或矩阵的形式存储，如果使用四元数需要自定义一个矩阵和四元数相互转换的函数，在Converter.类里面：

//旋转矩阵可以通过四元数或矩阵的形式存储
//如果使用四元数需要自定义一个矩阵和四元数相互转换的函数
cv::Mat Converter::toCvMat(const std::vector<float>& v)
{Eigen::Quaterniond q;q.x() = v[0];q.y() = v[1];q.z() = v[2];q.w() = v[3];Eigen::Matrix<double,3,3> eigMat(q);cv::Mat M = toCvMat(eigMat);return M;
}

在头文件Converter.h声明转换函数

//自定义一个矩阵和四元数相互转换的函数toCvMat(const std::vector<float>& v)
static cv::Mat toCvMat(const std::vector<float>& v);

第四步：修改System. System.h程序
上述修改完成之后，还需要在命名空间ORB_SLAM2对SaveMap进行声明和定义。分别对System.h和System.文件进行修改。在System中设置保存函数并在主函数中调用。
System.h文件：
namespace ORB_SLAM2/class System/public:

//声明SaveMap
void SaveMap(const string &filename); 

System.文件：
namespace ORB_SLAM2/class System/public:

//地图保存
void System::SaveMap(const string &filename)//wuhoup20190923
{mpMap->Save(filename);
}

至此，code修改完成。
1.4 测试
做完这些修改之后，在Examples文件中对应的示例程序中加入地图存储代码即可实现地图存储功能。
如在mono——tum.文件中加入如下语句：

//保存地图的二进制文件
SLAM.SaveMap("map.bin");

在运行的路径下就会有存有地图的二进制文件了。
在ros下和rgbd的example下都经过测试，地图是可以保存的，