ORB SLAM3 点云地图保存

目前 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。

extern int pre_num = 0;

        另外，若不想每一关键帧都覆盖掉原来的map.pcd，则可以将KeyFrame ID传入MapDrawer::DrawMapPoints()函数，以ID作为文件名进行保存，此处不贴出代码，读者可以找到调用该函数的地方进行修改。