目前 ORB_SLAM3 已经提供了地图保存功能。
方法是在 yaml 文件中以下这行配置:
System.SaveAtlasToFile: "map.osa"
保存下来的地图可以在下次运行 ORB_SLAM3 时加载。
然而,我经过搜索,没能找到关于 .osa 文件离线加载和可视化的方法,于是对 ORB_SLAM3 的代码进行了简单的修改,使其可以保存 pcd 格式的点云地图。
修改代码的已发布至:GitHub - DioVei/ORB_SLAM3_with_save: Modify the orb-slam3 code to save the keypoints per frame and the map in PCD format.
代码中仅使用到 PCL 基础的点云保存功能,因此应该兼容不同版本的 PCL 库。
sudo apt-get install libpcl-dev pcl-tools
首先,我们找到代码绘制点云的部分,即 src 文件夹下的 MapDrawer.cc 文件。
第一步,先加入 pcl 保存点云所需的头文件:
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
第二步,找到 DrawMapPoints 函数中的如下代码:
for(set<MapPoint*>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++)
{
if((*sit)->isBad())
continue;
Eigen::Matrix<float,3,1> pos = (*sit)->GetWorldPos();
glVertex3f(pos(0),pos(1),pos(2));
}
将其修改为如下代码:
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_saved(new pcl::PointCloud<pcl::PointXYZ>());
for(set<MapPoint*>::iterator sit=spRefMPs.begin(), send=spRefMPs.end(); sit!=send; sit++)
{
if((*sit)->isBad())
continue;
Eigen::Matrix<float,3,1> pos = (*sit)->GetWorldPos();
glVertex3f(pos(0),pos(1),pos(2));
//modified by Awei
pcl::PointXYZ p;
p.x = pos(0);
p.y = pos(1);
p.z = pos(2);
cloud_saved->points.push_back(p);
}
if (cloud_saved->points.size())
pcl::io::savePCDFileBinary("map.pcd", *cloud_saved);
代码修改部分十分简单,最后一步就是在 CMakeLists.txt 中添加 PCL 库:
find_package(PCL REQUIRED)
include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/include
${PROJECT_SOURCE_DIR}/include/CameraModels
${PROJECT_SOURCE_DIR}/Thirdparty/Sophus
${EIGEN3_INCLUDE_DIR}
${Pangolin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
target_link_libraries(${PROJECT_NAME}
${OpenCV_LIBS}
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PCL_LIBRARIES}
${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
-lboost_serialization
-lcrypto
)
最终,保存下来的地图可以使用 pcl_viewer 进行查看:
pcl_viewer map.pcd
提醒读者一下,保存下来的map.pcd是在终端运行命令时所在的路径下的,所以建议各位可以新建一个文件夹,在该文件夹下运行命令,这样方便找到和整理点云地图。
更新
以上改法是Stereo模式在EuRoC V1_01_easy数据集下不使用ROS测试有效的,最终保存下来的点云地图如下
但是,后来我也尝试了自己的数据集,并发现在大型数据集下用上面的改法不能保存完整地图,因此代码按照以下方式修改。
找到以下代码段
for(size_t i=0, iend=vpMPs.size(); i<iend;i++)
{
if(vpMPs[i]->isBad() || spRefMPs.count(vpMPs[i]))
continue;
Eigen::Matrix<float,3,1> pos = vpMPs[i]->GetWorldPos();
glVertex3f(pos(0),pos(1),pos(2));
}
将其修改为
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_saved(new pcl::PointCloud<pcl::PointXYZ>());
for(size_t i=0, iend=vpMPs.size(); i<iend;i++)
{
if(vpMPs[i]->isBad() || spRefMPs.count(vpMPs[i]))
continue;
Eigen::Matrix<float,3,1> pos = vpMPs[i]->GetWorldPos();
glVertex3f(pos(0),pos(1),pos(2));
//modified by Awei
pcl::PointXYZ p;
p.x = pos(0);
p.y = pos(1);
p.z = pos(2);
cloud_saved->points.push_back(p);
}
if (cloud_saved->points.size() > pre_num)
{
pcl::io::savePCDFileBinary("map.pcd", *cloud_saved);
pre_num = cloud_saved->points.size();
}
并在 MapDrawer.cc 开头加入全局变量 pre_num。文章来源:https://www.toymoban.com/news/detail-463991.html
extern int pre_num = 0;
另外,若不想每一关键帧都覆盖掉原来的map.pcd,则可以将KeyFrame ID传入MapDrawer::DrawMapPoints()函数,以ID作为文件名进行保存,此处不贴出代码,读者可以找到调用该函数的地方进行修改。文章来源地址https://www.toymoban.com/news/detail-463991.html
到了这里,关于ORB SLAM3 点云地图保存的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!