0 引言

ORB-SLAM2_RGBD_DENSE_MAP是基于ORB-SLAM2框架的一种RGB-D稠密地图构建算法。ORB-SLAM2是一种基于单目、双目和RGB-D相机的实时定位与建图（SLAM）系统，旨在通过计算机视觉技术实现机器人和自主驾驶汽车等设备的自主定位和地图构建。

ORB-SLAM2_RGBD_DENSE_MAP的主要思想是结合ORB-SLAM2框架的稠密重建算法和体素网格（Voxel Grid）滤波器，对RGB-D相机采集到的图像数据进行处理，从而得到一个稠密的3D点云表示，进而构建出稠密的地图。该算法通过对相邻帧之间的视差图进行计算，利用三角测量法估计相机到场景中每个像素的深度值，并通过体素网格滤波器对深度图进行降采样，从而减少计算量和存储空间。最终，该算法可以生成一个具有高精度和高分辨率的稠密地图。

ORB-SLAM2_RGBD_DENSE_MAP算法的优点是具有较高的重建精度和鲁棒性，在各种环境和光照条件下均表现良好。然而，由于该算法需要进行大量的计算和存储，因此它的实时性和运行效率可能受到一定的影响。

本文系统环境：

• Ubuntu18.04
• ROS-melodic
• OpenCV3.2.0
• Eigen 3.3.4
• Pangolin-0.6
• PCL 1.8.1
• realsense-ros (若用硬件D435i)

1 安装依赖

1.1 其他库安装

ORB-SLAM2算法1已针对Ubuntu20.04安装ORB-SLAM2所需的依赖库，安装 OpenCV，Eigen，Pangolin 可以参考下。

1.2 pcl库安装

但ORB-SLAM2_RGBD_DENSE_MAP 因为是稠密建图，还需要安装pcl1.7以上版本，本文是安装了pcl1.8大版本，可先下载 pcl 1.8.1 版本（点击Source code(zip）

先安装pcl1.8.1所需的依赖库：

sudo apt-get update  
sudo apt-get install git build-essential linux-libc-dev
sudo apt-get install cmake cmake-gui
sudo apt-get install libusb-1.0-0-dev libusb-dev libudev-dev
sudo apt-get install mpi-default-dev openmpi-bin openmpi-common 
sudo apt-get install libflann1.9 libflann-dev
sudo apt-get install libeigen3-dev
sudo apt-get install libboost-all-dev
sudo apt-get install libvtk7.1-qt libvtk7.1
sudo apt-get install libqhull* libgtest-dev
sudo apt-get install freeglut3-dev pkg-config
sudo apt-get install libxmu-dev libxi-dev
sudo apt-get install mono-complete
sudo apt-get install openjdk-8-jdk openjdk-8-jre

然后解压下载的pcl1.8.1源码，并进入到pcl1.8.1文件夹中

mkdir build
cd build
cmake ..
make -j4
sudo make install

测试pcl是否安装成功的话，可用pcl_viewer xxx.pcd；如果没有pcd文件，可去点云库PCL(Point Cloud Library)的学习资源汇总下载rabbit.pcd

pcl_viewer rabbit.pcd

2 编译ORB-SLAM2_RGBD_DENSE_MAP

编译类似ORB-SLAM2的安装，但可能会出现不一样的问题。首先下载ORB-SLAM2_RGBD_DENSE_MAP

git clone https://github.com/xiaobainixi/ORB-SLAM2_RGBD_DENSE_MAP.git

由于安装的是pcl1.8，首先将CMakeLists.txt中find_package( PCL 1.7 REQUIRED )改成了find_package( PCL 1.8 REQUIRED )

2.1 build.sh

# 下载的ORB-SLAM2_RGBD_DENSE_MAP工程目录下
chmod +x build.sh
./build.sh

tips: ./build.sh > build_log.txt 2>&1 可打印整个编译log到文件

1. 编译问题1

make: *** [all] Error 1
Converting vocabulary to binary
./build.sh: line 30: ./tools/bin_vocabulary: Permission denied

原因是./tools/bin_vocabulary 词袋文件没有执行权限，如下给其权限

# ORB-SLAM2_RGBD_DENSE_MAP 目录下执行
sudo chmod +x tools/bin_vocabulary

2. 编译问题2

../Thirdparty/DBoW2/lib/libDBoW2.so: undefined reference to `cv::_OutputArray::_OutputArray(cv::Mat&)'
collect2: error: ld returned 1 exit status
CMakeFiles/rgbd_tum.dir/build.make:391: recipe for target '../bin/rgbd_tum' failed
make[2]: *** [../bin/rgbd_tum] Error 1
CMakeFiles/Makefile2:124: recipe for target 'CMakeFiles/rgbd_tum.dir/all' failed
make[1]: *** [CMakeFiles/rgbd_tum.dir/all] Error 2
Makefile:90: recipe for target 'all' failed
make: *** [all] Error 2
Converting vocabulary to binary
./tools/bin_vocabulary: error while loading shared libraries: libopencv_core3.so.3.3: cannot open shared object file: No such file or directory

Error1一开始以为OpenCV的问题，但后来排查到我这个是由于之前尝试编译时，第三方库DBoW2和g2o没有完全编译，删除两个第三方库中的build文件夹，重新编译即可。

Error2 是tools中的bin_vocabulary词袋找不到libopencv_core3.so.3.3，而且ORB-SLAM2_RGBD_DENSE_MAP的lib也没有该版本的opencv动态链接库，而本文系统安装的是OpenCV 3.2.0，版本也不符合，所以临时解决办法是不用bin_vocabulary词袋可执行文件，改用ORB-SLAM2工程中的词袋文件ORBvoc.txt

# 首先注释掉 build.sh 中的bin_vocabulary部分
# 最后几行注释掉

# cd ..
# echo "Converting vocabulary to binary"
# ./tools/bin_vocabulary

最后复制ORB-SLAM2中的词袋文件夹Vocabulary到ORB-SLAM2_RGBD_DENSE_MAP工程目录下。

重新编译，即可编译成功

/usr/include/eigen3/Eigen/src/Core/util/Constants.h:162:37: note: declared here
 EIGEN_DEPRECATED const unsigned int AlignedBit = 0x80;
                                     ^~~~~~~~~~
[ 94%] Linking CXX executable ../bin/mono_tum
[ 97%] Linking CXX executable ../bin/mono_kitti
[100%] Linking CXX executable ../bin/mono_euroc
[100%] Built target mono_tum
[100%] Built target mono_kitti
[100%] Built target mono_euroc

2.2 build_ros.sh

首先添加该工程到ROS_PACKAGE_PATH

gedit ~/.bashrc
# 最后一行新增,其中冒号后的PATH是自己ORB-SLAM2_RGBD_DENSE_MAP工程存放目录
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB-SLAM2_RGBD_DENSE_MAP/Examples/ROS
# 保存后source
source ~/.bashrc

然后执行如下命令进行编译

# 下载的ORB-SLAM2_RGBD_DENSE_MAP工程目录下
chmod +x build_ros.sh
./build_ros.sh

3 运行ORB-SLAM2_RGBD_DENSE_MAP

3.1 build.sh编译版本

由于用到rgb和depth图，所以参考ORB-SLAM2算法2下载TUM的rgbd_dataset_freiburg1_desk数据集，准备好数据集后，可执行以下命令，其中PATH是rgbd_dataset_freiburg1_desk文件夹的存放目录

# 下载的ORB-SLAM2_RGBD_DENSE_MAP工程目录下
./bin/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml PATH/rgbd_dataset_freiburg1_desk PATH/rgbd_dataset_freiburg1_desk/associations.txt

1. 运行问题1

Depth Threshold (Close/Far Points): 3.09294

-------
Start processing sequence ...
Images in the sequence: 573

New map created with 945 points
Segmentation fault (core dumped)

这种 段错误(核心已转储)的问题本文暂未完全解决（如有，欢迎分享交流），先用以下的临时解决办法(该问题仍会偶发):

1. 在 CMakeLists.txt 和 Thirdparty/g2o/CMakeLists.txt中删除-march=native

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

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

2. 在有定义Eigen的头文件中添加预编译命令EIGEN_MAKE_ALIGNED_OPERATOR_NEW ，分别是include路径下Converter.h、LoopClosing.h、PointCloude.h这三个文件
   Converter.h

class Converter
{
	public:
	# 添加预编译命令
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	static std::vector<cv::Mat> toDescriptorVector(const cv::Mat &Descriptors);
	
	static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT);
	static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3);

LoopClosing.h（两次修改，第一个public中添加，第二个public中的注释掉）

class LoopClosing
{

public:
	# 添加预编译命令
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	typedef pair<set<KeyFrame*>,int> ConsistentGroup;
	typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
		Eigen::aligned_allocator<std::pair<const KeyFrame*, g2o::Sim3> > > KeyFrameAndPose;
	
public:
	
	LoopClosing(Map* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale, shared_ptr<PointCloudMapping> pPointCloud);
	
	void SetTracker(Tracking* pTracker);
	void SetLocalMapper(LocalMapping* pLocalMapper);
	// Main function
	void Run();
	void InsertKeyFrame(KeyFrame *pKF);
	void RequestReset();
	// This function will run in a separate thread
	void RunGlobalBundleAdjustment(unsigned long nLoopKF);
	shared_ptr<PointCloudMapping> mpPointCloudMapping;
	bool isRunningGBA(){
		unique_lock<std::mutex> lock(mMutexGBA);
		return mbRunningGBA;
	}
	bool isFinishedGBA(){
		unique_lock<std::mutex> lock(mMutexGBA);
		return mbFinishedGBA;
	}
	
	void RequestFinish();
	
	bool isFinished();
	# 注释掉预编译命令
	// EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	int loopcount = 0;

PointCloude.h

class PointCloude
{
	typedef pcl::PointXYZRGBA PointT;
	typedef pcl::PointCloud<PointT> PointCloud;
public:
	PointCloud::Ptr pcE;
public:
	# 添加预编译命令
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	Eigen::Isometry3d T;
	int pcID;

2. 运行问题2

viewer视图中没有点云，只有坐标系

改动include文件夹中的 pointcloudmapping.h头文件

# bool loopbusy;
bool loopbusy = false;

3. 运行问题3
   确认TUM1.yaml文件里有没有点云地图的参数，如果没有，就把以下几行加在最后面

PointCloudMapping.Resolution: 0.01
meank: 50
thresh: 2.0

重新执行./build.sh，编译完成后，重新执行：

# 下载的ORB-SLAM2_RGBD_DENSE_MAP工程目录下
./bin/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml PATH/rgbd_dataset_freiburg1_desk PATH/rgbd_dataset_freiburg1_desk/associations.txt

运行结果：

median tracking time: 0.0200984
mean tracking time: 0.0206714

Saving camera trajectory to CameraTrajectory1.txt ...

trajectory saved!

Saving keyframe trajectory to CameraTrajectory2.txt ...

trajectory saved!
globalMap save finished

Current Frame 关键帧可视化界面：

Map Viewer 轨迹地图可视化界面：

viewer 稠密地图可视化界面：

保存的稠密地图文件result.pcd可用已安装的pcl打开：
pcl_viewer result.pcd

至此，成功用TUM数据包运行非ROS版本的ORB-SLAM2_RGBD_DENSE_MAP，并可视化生成的稠密地图。

3.2 build_ros.sh编译版本

本文这里就不用TUM的开源数据测试了，因为恰好有D435i相机可以实时发出深度图和rgb图来测试(录制的一小段数据ORB-SLAM2-RGBD-DENSE-MAP-data.tar)，所以以下就是以此为例。

默认已经安装realsense-ros的相机驱动，首先是相机启动文件(rs_camera.launch)的配置，保证发出与rgb图对齐的深度图：false修改成true

  <arg name="enable_depth"        default="true"/>

然后插上D435i相机后（USB3.0），新开终端启动D435i相机：

# source 激活realsense-ros的相机驱动后执行
roslaunch realsense2_camera rs_camera.launch

启动后，通过rostopic list可查的所需的rgb图和对齐的深度图的topic

# 与rgb图对齐的深度图
/camera/aligned_depth_to_color/image_raw
# rgb图
/camera/color/image_raw

然后修改代码中的两个topic，在ORB-SLAM2_RGBD_DENSE_MAP/Examples/ROS/ORB_SLAM21/src/ros_rgbd.cc文件中修改：

    // message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/kinect2/qhd/image_color_rect", 1);
    // message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/kinect2/qhd/image_depth_rect", 1);
    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/color/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/aligned_depth_to_color/image_raw", 1);

当然，还有对应的配置文件，在ORB-SLAM2_RGBD_DENSE_MAP/Examples/ROS/ORB_SLAM21/目录下新建配置文件d435i.yaml，主要是fx, fy, cx, cy、分辨率，帧率，baseline等要修改成所用的D435i相机对应的，好在D435i相机发出的rgb图已经是去畸变的，而且给出了内参，可通过rostopic echo /camera/color/camera_info命令查询，具体如下：

查询结果：其中K即是所需的内参

header: 
  seq: 0
  stamp: 
    secs: 1691576284
    nsecs: 247732162
  frame_id: "camera_color_optical_frame"
height: 540
width: 960
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [682.6236572265625, 0.0, 490.54339599609375, 0.0, 682.521240234375, 275.81976318359375, 0.0, 0.0, 
1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [682.6236572265625, 0.0, 490.54339599609375, 0.0, 0.0, 682.521240234375, 275.81976318359375, 0.0, 
0.0, 0.0, 1.0, 0.0]

d435i.yaml配置文件信息：

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 682.6236572265625
Camera.fy: 682.521240234375
Camera.cx: 490.54339599609375
Camera.cy: 275.81976318359375

Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.p3: 0.0

Camera.width: 960
Camera.height: 540
# Camera frames per second 
Camera.fps: 15.0

# IR projector baseline times fx (aprox.)
# bf = baseline (in meters) * fx, D435i的 baseline = 50 mm 
Camera.bf: 50.0

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1

# Close/Far threshold. Baseline times.
ThDepth: 40.0

# Deptmap values factor
DepthMapFactor: 1000.0

#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------

# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000

# ORB Extractor: Scale factor between levels in the scale pyramid 	
ORBextractor.scaleFactor: 1.2

# ORB Extractor: Number of levels in the scale pyramid	
ORBextractor.nLevels: 8

# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast			
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7

#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize:2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500

PointCloudMapping.Resolution: 0.01
meank: 50
thresh: 2.0

然后重新执行./build_ros.sh编译，编译成功后执行如下命令运行：

# ORB-SLAM2_RGBD_DENSE_MAP 目录下执行
rosrun ORB_SLAM21 RGBD Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM21/d435i.yaml

运行后相比原版的ORB-SLAM2多生成一个viewer的稠密建图的可视化界面：

Current Frame可视化界面：

Map Viewer可视化界面：

viewer可视化界面：

至此，成功用在线加载D435i相机发出的rgb图和对齐的深度图，运行ROS版本的ORB-SLAM2_RGBD_DENSE_MAP，并可视化生成的稠密地图。

Reference:

1. ORB-SLAM2_RGBD_DENSE_MAP
2. ORB-SLAM2算法1之Ubuntu20.04+ROS-noetic安装ORB-SLAM2及各种问题解决
3. 点云库PCL(Point Cloud Library)的学习资源汇总
4. ORB-SLAM2算法2之TUM开源数据运行ORB-SLAM2生成轨迹并用evo工具评估轨迹
5. ORB-SLAM2-RGBD-DENSE-MAP-data.tar

须知少时凌云志，曾许人间第一流。

⭐️👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍🌔