Ubuntu14.04(indigo)实现RGBDSLAMv2（数据集和实时Kinect）

Ubuntu14.04(indigo)实现RGBDSLAMv2（数据集和实时Kinect v2）

一、在.bag数据集上跑RGBDSLAMv2

RGBDSLAMv2指的是Felix Endres大神在2014年发表论文，实现的基于RGB-D 摄像头的SLAM系统，用于创建三维点云或者八叉树地图。

安装步骤重点参考原gitbub网址：https://github.com/felixendres/rgbdslam_v2

说明本人台式机硬件配置：

Intel(R)Core(TM)i5-6500 CPU @ 3.20GHz 3.20GHz；
RAM: 16.0GB；
GPU: NVIDIA GeForce GTX 1060 6GB。

1. 在Ubuntu14.04中安装ROS Indigo，参考网址：http://wiki.ros.org/cn/indigo/Installation/Ubuntu

2. 安装opencv2.4.9，参考网址：http://www.samontab.com/web/2014/06/installing-opencv-2-4-9-in-ubuntu-14-04-lts/

　　　　　　　　　　　　　　　http://blog.csdn.net/baoke485800/article/details/51236198

系统更新

sudo apt-get update
sudo apt-get upgrade

安装相关依赖包

sudo apt-get install build-essential libgtk2.0-dev libjpeg-dev libtiff4-dev libjasper-dev libopenexr-dev cmake python-dev python-numpy python-tk libtbb-dev libeigen3-dev yasm libfaac-dev libopencore-amrnb-dev libopencore-amrwb-dev libtheora-dev libvorbis-dev libxvidcore-dev libx264-dev libqt4-dev libqt4-opengl-dev sphinx-common texlive-latex-extra libv4l-dev libdc1394-22-dev libavcodec-dev libavformat-dev libswscale-dev default-jdk ant libvtk5-qt4-dev

利用wget获得Opencv2.4.9源文件，等下载完成后解压

wget http://sourceforge.net/projects/opencvlibrary/files/opencv-unix/2.4.9/opencv-2.4.9.zip

unzip opencv-2.4.9.zip
cd opencv-2.4.9

cmake编译安装opencv源文件包

mkdir build
cd build
cmake -D WITH_TBB=ON -D BUILD_NEW_PYTHON_SUPPORT=ON -D WITH_V4L=ON -D INSTALL_C_EXAMPLES=ON -D INSTALL_PYTHON_EXAMPLES=ON -D BUILD_EXAMPLES=ON -D WITH_QT=ON -D WITH_OPENGL=ON -D WITH_VTK=ON ..
make -j4
sudo make install

配置opencv相关

sudo gedit /etc/ld.so.conf.d/opencv.conf

在打开的文件中（空文件也可）添加如下代码并保存

/usr/local/lib

执行以下代码

sudo ldconfig

打开另外一个文件

sudo gedit /etc/bash.bashrc

在文件末尾添加如下并保存退出

PKG_CONFIG_PATH=$PKG_CONFIG_PATH:/usr/local/lib/pkgconfig
export PKG_CONFIG_PATH

检查opencv是否安装成功

cd ~/opencv-2.4.9/samples/c
chmod +x build_all.sh
./build_all.sh

老版本的C语言接口

./facedetect --cascade="/usr/local/share/OpenCV/haarcascades/haarcascade_frontalface_alt.xml"--scale=1.5 lena.jpg

./facedetect --cascade="/usr/local/share/OpenCV/haarcascades/haarcascade_frontalface_alt.xml"--nested-cascade="/usr/local/share/OpenCV/haarcascades/haarcascade_eye.xml" --scale=1.5 lena.jpg

新的C++接口

~/opencv-2.4.9/build/bin/cpp-example-grabcut ~/opencv-2.4.9/samples/cpp/lena.jpg

OK，测试成功。（更多测试详见上面参考网址）

opencv官网unix版本下载地址：https://sourceforge.net/projects/opencvlibrary/files/opencv-unix/

3. 安装pcl-1.7.2，使用github源码安装，地址：https://github.com/PointCloudLibrary/pcl

4. 创建catkin工作空间：

#为rgbdslam单独创建一个catkin工作空间

mkdir rgbdslam_catkin_ws
cd rgbdslam_catkin_ws
mkdir src
cd ~/rgbdslam_catkin_ws/src

#将其初始化为catkin工作空间的源码存放文件夹

catkin_init_workspace

#进入catkin工作空间目录

cd ~/rgbdslam_catkin_ws/

#编译新建的catkin工作空间，生成build、devel文件夹，形成完整的catkin工作空间

catkin_make

#调用终端配置文件

source devel/setup.bash

5. 源码安装g2o, 参考原gitbub网址：https://github.com/felixendres/rgbdslam_v2

6. 编译安装RGBDSLAMv2

#进入catkin工作空间的源码存放文件夹

cd ~/rgbdslam_catkin_ws/src

#下载github上对应ROS Indigo版本的rgbdslam源码

wget -q http://github.com/felixendres/rgbdslam_v2/archive/indigo.zip

#解压

unzip -q indigo.zip

#进入catkin工作空间目录

cd ~/rgbdslam_catkin_ws/

#ROS依赖包更新

rosdep update

yuanlibin@yuanlibin:~/rgbdslam_catkin_ws$ rosdep update
reading in sources list data from /etc/ros/rosdep/sources.list.d
Hit https://raw.githubusercontent.com/ros/rosdistro/master/rosdep/osx-homebrew.yaml
Hit https://raw.githubusercontent.com/ros/rosdistro/master/rosdep/base.yaml
Hit https://raw.githubusercontent.com/ros/rosdistro/master/rosdep/python.yaml
Hit https://raw.githubusercontent.com/ros/rosdistro/master/rosdep/ruby.yaml
Hit https://raw.githubusercontent.com/ros/rosdistro/master/releases/fuerte.yaml
Query rosdistro index https://raw.githubusercontent.com/ros/rosdistro/master/index.yaml
Add distro "groovy"
Add distro "hydro"
Add distro "indigo"
Add distro "jade"
Add distro "kinetic"
Add distro "lunar"
updated cache in /home/yuanlibin/.ros/rosdep/sources.cache

#安装rgbdslam依赖包

rosdep install rgbdslam

正确运行后显示：#All required rosdeps installed successfully

#编译rgbdslam

catkin_make

正确运行后显示：[100%] Built target rgbdslam

source devel/setup.bash

最后运行

roslaunch rgbdslam rgbdslam.launch

会出现错误：

NODES
  /
    rgbdslam (rgbdslam/rgbdslam)

ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found
ERROR: cannot launch node of type [rgbdslam/rgbdslam]: rgbdslam
ROS path [0]=/opt/ros/indigo/share/ros
ROS path [1]=/opt/ros/indigo/share
ROS path [2]=/opt/ros/indigo/stacks
No processes to monitor
shutting down processing monitor...
... shutting down processing monitor complete

解决方法是将工作空间的路径加到 .bashrc 文件中，如本电脑示例：：

echo "source /home/yuanlibin/rgbdslam_catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

至此，RGBDSLAMv2已编译安装完成。

7. 下载TUM的.bag数据集文件，下载地址：https://vision.in.tum.de/data/datasets/rgbd-dataset/download

例如：rgbd_dataset_freiburg1_xyz.bag

查看.bag数据集的信息:

终端1

roscore

终端2

rosbag play rgbd_dataset_freiburg1_xyz.bag

终端3

rostopic info

最后的命令不要按enter键按tab键进行查看

yuanlibin@yuanlibin:~$ rostopic info /
/camera/depth/camera_info  /cortex_marker_array
/camera/depth/image        /imu
/camera/rgb/camera_info    /rosout
/camera/rgb/image_color    /rosout_agg
/clock                     /tf
yuanlibin@yuanlibin:~$

然后修改路径：/home/yuanlibin/rgbdslam_catkin_ws/src/rgbdslam_v2-indigo/launch下的rgbdslam.launch文件

其中第8、9行的输入数据设置

   <param name="config/topic_image_mono"              value="/camera/rgb/image_color"/> 
    <param name="config/topic_image_depth"             value="/camera/depth_registered/sw_registered/image_rect_raw"/>

需要修改为上述数据集相应的信息，修改如下：

   <param name="config/topic_image_mono"              value="/camera/rgb/image_color"/> 
    <param name="config/topic_image_depth"             value="/camera/depth/image"/>

在该文件中可以修改系统使用的特征：

SIFT, SIFTGPU, SURF, SURF128 (extended SURF), ORB.

8. 在数据集上跑RGBDSLAMv2

终端1

roscore

终端2

rosbag play rgbd_dataset_freiburg1_xyz.bag

终端3

roslaunch rgbdslam rgbdslam.launch

最后，就可以看到在数据集上运行RGBDSLAMv2重建的三维点云图了。

 二、基于Kinect v1实时运行RGBDSLAMv2

1. 进行ROS indigo下Kinect v1的驱动安装与调试，可参考：http://www.cnblogs.com/yuanlibin/p/8608190.html

2. 在终端执行以下命令：

终端1

roscore

终端2

roslaunch rgbdslam openni+rgbdslam.launch

3. 移动Kinect v1，就可以看到实时重建的三维点云了。

 三、基于Kinect v2实时运行RGBDSLAMv2

1. 运行Kinect v2 查看其输出数据信息：

终端1

roslaunch kinect2_bridge kinect2_bridge.launch

终端2(输入命令rostopic info后，不要按enter，要按table键进行查看)

yuanlibin@yuanlibin:~$ rostopic info /
/kinect2/bond
/kinect2/hd/camera_info
/kinect2/hd/image_color
/kinect2/hd/image_color/compressed
/kinect2/hd/image_color_rect
/kinect2/hd/image_color_rect/compressed
/kinect2/hd/image_depth_rect
/kinect2/hd/image_depth_rect/compressed
/kinect2/hd/image_mono
/kinect2/hd/image_mono/compressed
/kinect2/hd/image_mono_rect
/kinect2/hd/image_mono_rect/compressed
/kinect2/hd/points
/kinect2/qhd/camera_info
/kinect2/qhd/image_color
/kinect2/qhd/image_color/compressed
/kinect2/qhd/image_color_rect
/kinect2/qhd/image_color_rect/compressed
/kinect2/qhd/image_depth_rect
/kinect2/qhd/image_depth_rect/compressed
/kinect2/qhd/image_mono
/kinect2/qhd/image_mono/compressed
/kinect2/qhd/image_mono_rect
--More--

2. 在路径/home/yuanlibin/rgbdslam_catkin_ws/src/rgbdslam_v2-indigo/launch下新建一个rgbdslam_kinect2.launch文件，内容如下：

<launch>
<node pkg="rgbdslam" type="rgbdslam" name="rgbdslam" cwd="node" required="true" output="screen"> 
<!-- Input data settings-->
<param name="config/topic_image_mono"              value="/kinect2/qhd/image_color_rect"/>  
<param name="config/camera_info_topic"             value="/kinect2/qhd/camera_info"/>

<param name="config/topic_image_depth"             value="/kinect2/qhd/image_depth_rect"/>

<param name="config/topic_points"                  value=""/> <!--if empty, poincloud will be reconstructed from image and depth -->

<!-- These are the default values of some important parameters -->
<param name="config/feature_extractor_type"        value="ORB"/><!-- also available: SIFT, SIFTGPU, SURF, SURF128 (extended SURF), ORB. -->
<param name="config/feature_detector_type"         value="ORB"/><!-- also available: SIFT, SURF, GFTT (good features to track), ORB. -->
<param name="config/detector_grid_resolution"      value="3"/><!-- detect on a 3x3 grid (to spread ORB keypoints and parallelize SIFT and SURF) -->

<param name="config/optimizer_skip_step"           value="15"/><!-- optimize only every n-th frame -->
<param name="config/cloud_creation_skip_step"      value="2"/><!-- subsample the images' pixels (in both, width and height), when creating the cloud (and therefore reduce memory consumption) -->

<param name="config/backend_solver"                value="csparse"/><!-- pcg is faster and good for continuous online optimization, cholmod and csparse are better for offline optimization (without good initial guess)-->

<param name="config/pose_relative_to"              value="first"/><!-- optimize only a subset of the graph: "largest_loop" = Everything from the earliest matched frame to the current one. Use "first" to optimize the full graph, "inaffected" to optimize only the frames that were matched (not those inbetween for loops) -->

<param name="config/maximum_depth"           value="2"/>
<param name="config/subscriber_queue_size"         value="20"/>

<param name="config/min_sampled_candidates"        value="30"/><!-- Frame-to-frame comparisons to random frames (big loop closures) -->
<param name="config/predecessor_candidates"        value="20"/><!-- Frame-to-frame comparisons to sequential frames-->
<param name="config/neighbor_candidates"           value="20"/><!-- Frame-to-frame comparisons to graph neighbor frames-->
<param name="config/ransac_iterations"             value="140"/>

<param name="config/g2o_transformation_refinement"           value="1"/>
<param name="config/icp_method"           value="gicp"/>  <!-- icp, gicp ... -->

<!--
<param name="config/max_rotation_degree"           value="20"/>
<param name="config/max_translation_meter"           value="0.5"/>

<param name="config/min_matches"           value="30"/>   

<param name="config/min_translation_meter"           value="0.05"/>
<param name="config/min_rotation_degree"           value="3"/>
<param name="config/g2o_transformation_refinement"           value="2"/>
<param name="config/min_rotation_degree"           value="10"/>

<param name="config/matcher_type"         value="ORB"/>
 -->
</node>
</launch>

注意第3、4、5、7行的输入数据设置，应与上面查看到的信息一致。

在该文件中可以修改系统使用的特征：

SIFT, SIFTGPU, SURF, SURF128 (extended SURF), ORB.

3. 最后基于Kinect v2的实时运行RGBDSLAMv2

终端1

roslaunch rgbdslam rgbdslam_kinect2.launch

终端2

roslaunch kinect2_bridge kinect2_bridge.launch

缓慢移动Kinect v2，就可以看到实时重建的三维点云了。自己实现的三维点云截图如下：

图1所示为实验室工位的全景三维点云图；

图2所示为全景图中红点处的侧视图。

图1. 实验室工位的全景三维点云图

图2. 全景图中红点处的侧视图