ORB__SLAM3，使用PCL库保存为PCD类型地图文件

参考连接：
【ORB_SLAM3】Ubuntu20.04 ros运行并保存PCD地图文件_ros2加载pcd-CSDN博客Ubuntu18.04：ORB-SLAM3使用数据集构建地图和保存点云地图_orbslam3保存pcd点云图-CSDN博客
 

前提是已经配置完ORBSLAM3，可以正常运行数据集或者自己的相机案例。

安装PCL库：

sudo apt-get install libpcl-dev pcl-tools

1、修改ORB_SLAM3/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
)

src目录下的MapDrawer.cc 文件修改内容：
1、添加头文件：

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>

2、找到 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));
 
}

将其修改为如下代码：

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

3、 找到以下代码段

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;

在修改完之后，建议两个都编译一下，第一遍的时候改完只编译了ros下的，没效果：

chmod +x build.sh
chmod +x build_ros.sh
./build.sh
./build_ros.sh

再次运行即可构建点云地图，我运行的指令：

rosrun ORB_SLAM3 Stereo Vocabulary/ORBvoc.txt Examples/Stereo/EuRoC.yaml false

在ORB_SLAM3目录下生成map.pcd文件

在ORB_SLAM3目录下启动终端，输入下列命令，即可查看地图文件

pcl_viewer map.pcd