[人工智能]AzureKinect+ORB-SLAM2实现实时稠密点云建图

Ubuntu18.04环境下使用AzureKinect摄像头和ORB-SLAM2实现实时稠密建图。ORB-SLAM2原始代码并没有包括稠密建图部分，高翔博士在ORB-SLAM2的基础上进行了扩展，实现了该功能。（参考高翔博士博客）

ROS收发Azure Kinect图像数据

略

ROS 工作环境

在开始编译ORB-SLAM2之前需要先创建一个ROS的工作空间。

mkdir -p ~/catkin_ws_orbslam2gao/src
cd catkin_ws_orbslam2gao/src
catkin_init_workspace
cd ~/catkin_ws_orbslam2gao/
catkin_make
source devel/setup.bash

下载ORBSLAM2_with_pointcloud_map源码

github repo中有一个文件夹ORB_SLAM2_modified和一个zip压缩包。我们使用文件夹对应的代码，压缩包是老版本代码不需要管。

cd ~/catkin_ws_orbslam2gao/src
git clone https://github.com/gaoxiang12/ORBSLAM2_with_pointcloud_map.git

# 删除作者保留的build文件夹重新编译
cd ORBSLAM2_with_pointcloud_map/ORB_SLAM2_modified
rm -rf Thirdparty/g2o/build
rm -rf Thirdparty/DBoW2/build
rm -rf Examples/ROS/ORB_SLAM2/build

# 下载Vocabulay
mkdir Vocabulary && cd Vocabulary
wget https://raw.githubusercontent.com/raulmur/ORB_SLAM2/master/Vocabulary/ORBvoc.txt.tar.gz

编译核心代码

在编译代码前先解决一些可能遇到的问题：

关于PCL库可能有两个问题。

1. 提示找不到 pcl/common/transforms.h，可以通过locate命令查找本机上pcl库的路径，并添加在CMakeList.txt。(ORBSLAM2_with_pointcloud_map/ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/CMakeLists.txt第52行)

   locate pcl/common/transforms.h
   

   修改后的CMakeLists如下

   include_directories(
   ${PROJECT_SOURCE_DIR}
   ${PROJECT_SOURCE_DIR}/../../../
   ${PROJECT_SOURCE_DIR}/../../../include
   ${Pangolin_INCLUDE_DIRS}
   /usr/include/pcl-1.8/
   )
   

2. 运行过程中出现Segmentation Fault，参考github issue, 出现该错误的一个原因是pcl库的问题。在安装ros的时候默认安装了pcl1.8，默认的库可能在编译上有点问题。解决办法是从PCL的github上下载1.8版本的源码手动编译一遍（编译过程可能遇到lib文件和anaconda冲突，解决办法是将anaconda重命名，等编译完再改回来。另一问题是关闭GPU版本，我的在编译GPU版本的时候会报错），并在CMakeList.txt中手动指定pcl位置。关于制定pcl这里参考博客。

提示找不到libboost_system和libboost_filesystem

报错信息如下：

/usr/bin/ld: CMakeFiles/RGBD.dir/src/ros_rgbd.cc.o: undefined reference to symbol '_ZN5boost6system15system_categoryEv'
/usr/lib/x86_64-linux-gnu/libboost_system.so: error adding symbols: DSO missing from command line
collect2: error: ld returned 1 exit status

定位libboost_system.so与libboost_filesystem.so

sudo updatedb
locate libboost_system.so
locate libboost_filesystem.so

将这两个文件copy到ORBSLAM2_with_pointcloud_map/ORB_SLAM2_modified/lib文件夹下
并修改ORBSLAM2_with_pointcloud_map/ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/CMakeLists.txt，加入对应的路径（如下最后两行）。

set(LIBS
${OpenCV_LIBS}
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so
${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so
${PROJECT_SOURCE_DIR}/../../../lib/libboost_filesystem.so
${PROJECT_SOURCE_DIR}/../../../lib/libboost_system.so
)

相机配置文件

使用的yaml文件如下：

%YAML:1.0

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

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 503.842
Camera.fy: 503.93
Camera.cx: 325.452
Camera.cy: 335.608

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

Camera.width: 640
Camera.height: 576

# Camera frames per second 
Camera.fps: 30.0

# IR projector baseline times fx (aprox.)
Camera.bf: 40.0

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

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

# Deptmap values factor
DepthMapFactor: 1.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

注：

1. 对Asus Kinect，参数 DepthMapFactor要设置成1.0
2. 参数Camera.RGB需设置为0
3. 参数PointCloudMapping.Resolution: 0.01, 如果忘了设置这一参数可能出现点云不显示的情况。
4. 我在实验中填入相机自带的畸变参数（Camera.k1, Camera.k2, Camera.p1, Camera.p2）后效果很差。因此这里没有进行畸变矫正，对应的参数填0。
5. 关于畸变参数，也可以参考博客

编译ORB-SLAM2

处理完以上错误后可以进行编译了。

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

运行

source ~/catkin_ws_orbslam2gao/devel/setup.bash
rosrun ORB_SLAM2_modified RGBD Asus.yaml

其他问题

• 点云颜色无法显示问题https://github.com/gaoxiang12/ORBSLAM2_with_pointcloud_map/issues/10#issuecomment-309663764