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();
std::map<MapPoint*,unsigned long int> mmpnMapPointsIdx;
修改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)
{
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
下,可以根据需求修改路径。
上面就是保存下来的地图数据,这几种类型的区别将在下篇博客中做出说明。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)