ORB_SLAM2保存地图详细流程（ros版本）

1.开发背景
首先，之前提到过的博主最近换了工作，以后打算努力学习slam，之前做激光嘛，当然，也是个菜鸟，现在突然做视觉SLAM-orbslam，接触起来的第一感觉是：哇，好大一个工程。反正既然做了，就认真学嘛，项目老哥要求把地图保存并加载，所以，这一篇就记录一下保存地图的流程。

2. 软硬件环境
传感器：azure-kinect-sdk，驱动用的是v1.4(当初搭建这个ros版本的驱动可真是折磨死我了，不过后来总结了一下经验，搭建起来也不是太费劲了，需要的话，后面我再写一篇讲搭建linux下ros驱动的过程)
开发环境：ros-melodic,ubuntu18.04（虚拟机），RGBD_ROS
视觉框架：orb_slam3(目前初步成功了，只是在orbslam2上，后面还是要移到orbslam3上的，这里就记录在orbslam2上改动的流程)

3.Let’s go!!!
先上图吧！看一下大致场景,下一篇博客讲重定位流程。

这是之前我们自己采的数据跑出来的图，室内的话还好，rgb+depimg，不过加上Imu的版本才是最稳定的，这个图就是我最终保存下来的，下一篇加载出来的也是它，下篇再讲加载的。

- 修改Map.h和Map.cc
修改Map.h

//头文件需要导入Converter.h
#include "Converter.h"
 
public:
    //保存地图信息函数
    void Save(const string &filename);
    //获取地图点ID
    void GetMapPointsIdx();
    
protected:
    //添加成员变量，这个私有公有都可以
    std::map<MapPoint*, unsigned long int> mmpnMapPointsIdx;
     //保存地图点和关键帧 
    void SaveMapPoint( ofstream &f, MapPoint* mp);
    void SaveKeyFrame( ofstream &f, KeyFrame* kf);
 

修改Map.cc

//保存地图信息
void Map::Save ( const string& filename )
{
    //Print the information of the saving map
    cerr<<"Map.cc :: Map Saving to "<<filename <<endl;
    ofstream f;
    f.open(filename.c_str(), ios_base::out|ios::binary);
 
    //Number of MapPoints
    unsigned long int nMapPoints = mspMapPoints.size();
    f.write((char*)&nMapPoints, sizeof(nMapPoints) );
    //Save MapPoint sequentially
    for ( auto mp: mspMapPoints ){
        //Save MapPoint
        SaveMapPoint( f, mp );
        // cerr << "Map.cc :: Saving map point number: " << mp->mnId << endl;
    }
 
    //Print The number of MapPoints
    cerr << "Map.cc :: The number of MapPoints is :"<<mspMapPoints.size()<<endl;
        
 
    //Grab the index of each MapPoint, count from 0, in which we initialized mmpnMapPointsIdx  
    GetMapPointsIdx(); 
 
    //Print the number of KeyFrames
    cerr <<"Map.cc :: The number of KeyFrames:"<<mspKeyFrames.size()<<endl;
    //Number of KeyFrames
    unsigned long int nKeyFrames = mspKeyFrames.size();
    f.write((char*)&nKeyFrames, sizeof(nKeyFrames));
 
    //Save KeyFrames sequentially
    for ( auto kf: mspKeyFrames )
        SaveKeyFrame( f, kf );
 
    for (auto kf:mspKeyFrames )
    {
        //Get parent of current KeyFrame and save the ID of this parent
        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));
 
        //Get the size of the Connected KeyFrames of the current KeyFrames
        //and then save the ID and weight of the Connected KeyFrames
        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));
        }
    }
 
    // Save last Frame ID
    // SaveFrameID(f);
 
    f.close();
    cerr<<"Map.cc :: Map Saving Finished!"<<endl;
}
 
//保存地图点
void Map::SaveMapPoint( ofstream& f, MapPoint* mp)
{   
    //Save ID and the x,y,z coordinates of the current MapPoint
    f.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));
}
 
//保存关键帧
void Map::SaveKeyFrame( ofstream &f, KeyFrame* kf )
{
    //Save the ID and timesteps of current KeyFrame
    f.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 KeyFrame
    cv::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("<<i<<","<<j<<"):"<<Tcw.at<float>(i,j)<<endl;
    //     }
    // }
 
    //Save the rotation matrix in Quaternion
    std::vector<float> Quat = Converter::toQuaternion(Tcw);
    for ( int i = 0; i < 4; i ++ )
        f.write((char*)&Quat[i],sizeof(float));
    //Save the translation matrix
    for ( int i = 0; i < 3; i ++ )
        f.write((char*)&Tcw.at<float>(i,3),sizeof(float));
 
 
 
 
    //Save the size of the ORB features current KeyFrame
    //cerr<<"kf->N:"<<kf->N<<endl;
    f.write((char*)&kf->N, sizeof(kf->N));
    //Save each ORB features
    for( 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 features
        f.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));
 
        //Save the index of MapPoints that corresponds to current ORB features
        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 BoW for relocalization.
    // f.write((char*)&kf->mBowVec, sizeof(kf->mBowVec));
}
 
//获取地图点ID
void Map::GetMapPointsIdx()
{
    unique_lock<mutex> lock(mMutexMap);
    unsigned long int i = 0;
    for ( auto mp: mspMapPoints )
    {
        mmpnMapPointsIdx[mp] = i;
        i += 1;
    }
}
 

- 修改Converter.h和Converter.cc
修改Converter.h
添加函数声明：

public:
static cv::Mat Converter::toCvMat(const std::vector<float>& v);

修改Converter.cc
添加函数定义：

cv::Mat 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;
}

- 修改System.cc和System.h
修改System.h

添加函数声明：

public:
void SaveMap(const string &filename); 

修改System.cc
添加函数定义：

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

- 入口程序修改

我用的是rgbd_ros，所以我修改的也是Example/ROS/ORBSLAM2下的ros_rgbd.cc文件：

    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
    SLAM.SaveMap("MapPointandKeyFrame.bin");

地图保存倒还好，没什么坑，ros::spin()和SLAM.Shutdown()的存在注定了这个地图的保存只有在你按ctrl+c的时候才进行地图保存。当然，你可以把地图保存这句话放在ros::spin或者SLAM.Shutdown（）的前面，但是，建图卡死也是分分钟的事。保存完成后会在运行目录下多一个文件，如图：

也要注意按下ctrl+c时终端打印的信息，因为SaveMap函数中加了很多打印来提示地图保存到哪一步了，做的时候看一下终端也大致可以判断出来是否保存成功了！

参考文献：（感谢各位大神）
Felaim
达达MFZ