ORB-SLAM2稠密建图实战：从编译到实时彩色点云生成与保存

1. ORB-SLAM2稠密建图实战入门

第一次接触ORB-SLAM2的稠密建图功能时，我被它生成的3D环境模型震撼到了。不同于传统的稀疏特征点地图，稠密建图能呈现更丰富的环境细节，特别适合需要高精度三维重建的场景。今天要分享的实战教程，会带你从零开始搭建一个能实时显示彩色点云并支持地图保存的完整系统。

这个教程适合有一定ROS和C++基础的开发者，如果你刚接触SLAM也不用担心，我会尽量把每个步骤都拆解清楚。我们使用的环境是Ubuntu 18.04 + ROS Melodic，这是目前最稳定的组合。整个流程包括源码编译、环境配置、代码修改和功能验证四个主要阶段，预计完整走通需要2-3小时。

在开始前，建议准备好以下环境：

• 至少8GB内存的电脑（处理点云数据很吃内存）
• 安装好NVIDIA显卡驱动（CUDA加速会大幅提升性能）
• 20GB以上的可用磁盘空间（用于存储点云数据）

2. 环境准备与源码编译

2.1 基础环境搭建

首先确保你的系统已经安装好ROS Melodic完整版。我遇到过不少问题都是因为ROS安装不完整导致的，建议用以下命令检查：

sudo apt-get install ros-melodic-desktop-full

接着安装必要的依赖库，这些是ORB-SLAM2运行的基础：

sudo apt-get install libglew-dev libpython2.7-dev libeigen3-dev libboost-all-dev

特别提醒，PCL库的版本很重要。我们需要的1.7版本可能不在默认源中，建议手动编译安装。我曾经因为PCL版本不匹配导致点云显示异常，折腾了好久才发现问题。

2.2 源码获取与预处理

推荐使用高翔博士修改过的ORB-SLAM2_with_pointcloud_map版本，这个版本已经集成了点云地图功能。下载源码到你的工作空间：

cd ~/catkin_ws/src git clone https://github.com/gaoxiang12/ORB_SLAM2_with_pointcloud_map.git

这里有个容易踩坑的地方：一定要删除原有的build文件夹。我建议执行以下清理命令：

cd ORB_SLAM2_with_pointcloud_map rm -rf Thirdparty/DBoW2/build Thirdparty/g2o/build Examples/ROS/ORB_SLAM2/build

2.3 关键编译配置修改

打开CMakeLists.txt文件，找到编译器设置部分。现代CPU通常不需要-march=native优化，反而可能引起兼容性问题。删除以下两处设置：

set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native")

在Thirdparty/g2o/CMakeLists.txt中同样删除-march=native选项。这个细节很容易被忽略，但会导致在一些CPU架构上编译失败。

3. 实现彩色点云可视化

3.1 代码修改要点

原始ORB-SLAM2只支持灰度点云显示，我们要修改几处关键代码来实现彩色显示。首先在Tracking.h中添加RGB图像存储变量：

// Current Frame Frame mCurrentFrame; cv::Mat mImRGB; // 新增这行 cv::Mat mImGray; cv::Mat mImDepth;

然后在Tracking.cc中修改两处关键代码。第一处在GrabImageRGBD函数开头：

mImRGB = imRGB; // 保存彩色图像 mImGray = imRGB; // 原始灰度图像 mImDepth = imD;

第二处是点云生成部分，将原来的灰度图像替换为RGB图像：

mpPointCloudMapping->insertKeyFrame(pKF, this->mImRGB, this->mImDepth);

3.2 编译与测试

完成修改后，重新编译项目：

chmod +x build.sh ./build.sh

测试时建议使用TUM数据集，运行命令格式如下：

./bin/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml [数据集路径] Examples/RGB-D/associations/fr1_desk.txt

如果一切顺利，你现在应该能看到彩色的点云地图了。我第一次成功时，发现办公室的绿植在点云中呈现出真实的颜色，效果非常惊艳。

4. 点云地图保存功能实现

4.1 PCL库集成

要实现点云保存功能，我们需要使用PCL库的文件IO模块。在pointcloudmapping.cc文件中添加头文件：

#include <pcl/io/pcd_io.h>

然后在viewer()函数中找到全局地图生成的循环，在循环结束后添加保存命令：

pcl::io::savePCDFileBinary("vslam.pcd", *globalMap);

这个位置很关键，太早保存会得到不完整的地图，太晚可能程序已经退出。我建议放在所有关键帧处理完成后的位置。

4.2 点云查看工具安装

保存的点云文件可以用pcl-viewer查看：

sudo apt-get install pcl-tools pcl_viewer vslam.pcd

在实际项目中，我发现点云文件可能很大（几百MB），可以用以下命令压缩：

pcl_compress_point_cloud vslam.pcd compressed.pcd

5. ROS集成与常见问题解决

5.1 ROS环境配置

为了让ORB-SLAM2在ROS中运行，需要设置ROS_PACKAGE_PATH。编辑~/.bashrc文件：

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:~/catkin_ws/src/ORB_SLAM2_with_pointcloud_map/Examples/ROS

然后刷新环境：

source ~/.bashrc

验证路径是否设置成功：

echo $ROS_PACKAGE_PATH

5.2 常见编译错误处理

错误1：PCL头文件找不到 这是因为系统中有多个PCL版本。解决方法是在CMakeLists.txt中修改：

find_package(PCL REQUIRED)

错误2：boost_system链接错误 在Examples/ROS/ORB_SLAM2/CMakeLists.txt中添加：

target_link_libraries(ORB_SLAM2 ${Boost_SYSTEM_LIBRARY})

错误3：ROS路径冲突 如果遇到奇怪的路径错误，可能是ROS缓存问题。尝试：

rosclean purge

6. 性能优化与实用技巧

6.1 实时性优化建议

默认配置在复杂场景下可能卡顿，可以通过以下方式优化：

1. 降低点云分辨率：修改pointcloudmapping.cc中的点云生成参数
2. 启用GPU加速：确保CUDA已正确安装，并在CMake中开启CUDA支持
3. 限制关键帧数量：在Tracking.cc中调整关键帧插入策略

6.2 实用调试技巧

我总结了几条实用调试经验：

• 使用rviz实时监控点云生成过程
• 保存中间结果时加上时间戳，方便对比不同版本效果
• 在关键函数中添加调试输出，了解系统运行状态

std::cout << "Point cloud size: " << globalMap->size() << std::endl;

7. 进阶功能扩展

7.1 点云后处理

保存的原始点云通常包含噪声，可以用PCL进行滤波：

pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor; sor.setInputCloud(cloud); sor.setMeanK(50); sor.setStddevMulThresh(1.0); sor.filter(*filtered_cloud);

7.2 地图重定位功能

通过保存和加载点云特征，可以实现重定位：

pcl::io::savePCDFile("keypoints.pcd", *keypoints_cloud);

在实际项目中，这套系统已经成功应用于室内三维重建和AR导航场景。记得第一次完整跑通流程时，看着自己办公室的精细三维模型在屏幕上旋转，那种成就感至今难忘。