ORB_SLAM2地图保存

ORB_SLAM2地图保存

在安装好orb_slam2后按照教程中的方法做了地图构建的实验，但是当地图达到想要的标准之后，却发现没有办法保存地图，查看ORB_SLAM2源码发现在System.h中有如下一段话
，这段话意思就是在ORB_SLAM2中并没有地图保存和加载功能，需要读者自行添加。

源码修改

需要修改的有Map.h Map.cc Converter.h Converter.cc System.h System.cc

Map.h修改

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

protected:
    std::set<MapPoint*> mspMapPoints;
    std::set<KeyFrame*> mspKeyFrames;

    std::vector<MapPoint*> mvpReferenceMapPoints;


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

修改Map.cc

添加#include "Converter.h"头文件

#include "Converter.h"

修改Converter.h

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

修改Converter.cc

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

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;
}

修改System.h

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

修改System.cc

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

到这里，已经修改完成，在Example对应文件中的示例程序中加入地图存储代码既可以实现地图存储功能
我这里用的是stereo相机，所以修改的是ros_stereo.cc

SLAM.SaveMap("map.bin");

我这里还写了其他格式的地图保存

对代码重新编译后既可以使用

cd ORB_SLAM2
./build.sh
./build_ros.sh

我们启动gezebo和ORB_SLAM2

rosrun ORB_SLAM2 Stereo src/ORB_SLAM2/Vocabulary/ORBvoc.txt src/ORB_SLAM2/Examples/Stereo/Stereo_setting.yaml false

控制小车运动，当地图点云达到你想要的标准时，ctrl+c退出即可保存
这里的路径默认保存在我的工作空间catkin_ws下，可以根据需求修改路径。

上面就是保存下来的地图数据，这几种类型的区别将在下篇博客中做出说明。