Kinect2+Ros跑通ORBLSAM2

一、ubuntu16.04安装kinect2以及iai-kinect2

可以参考这篇文章kinetic2安装以及iai-kinect2

iai-kinect2

二、KInetic+Ros 跑ORBSLAM2

步骤：
打开终端1

git clone http://github.com/raulmur/ORB_SLAM2.git

cd ORB_SLAM2

chmod +x build.sh 

 ./build.sh #可自动编译好g2o与DBoW2并且把Vocabulary自动解压

打开一个新的终端2

gedit ~/.bashrc
在打开的文件的最后一行写入：
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/wenbo/ORB_SLAM2/Examples/ROS
上面这个一定要做，否则会出错说找不到路径和包

回到终端1
编译ros节点：

chmod +x build_ros.sh 

确保原程序可以编译 ：

./build_ros.sh

从有iai-kinect2的工作空间中打开一个终端3，运行：

source devel/setup.sh
roslaunch kinect2_bridge kinect2_bridge.launch 

再打开一个终端4，输入：

rostopic list #查看此时Kinect2发布的topic 

然后在ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc 按照上面3的topic修改里面的ros订阅的内容

 message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/kinect2/qhd/image_color_rect", 1);  //改ros的发布  
 message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/kinect2/qhd/image_depth_rect", 1);  //改ros的发布 

回到终端1
重新编译工程：

chmod +x build_ros.sh
./build_ros.sh 

打开一个终端5，输入：

rqt_graph #查看话题是否接上

类似下图：

最后在ORB_SLAM2/Examples/Ros打开终端6，输入

rosrun ORB_SLAM2 RGBD /home/wenbo/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/wenbo/ORB_SLAM2/Examples/RGB-D/kinect2_qhd.yaml

注意：可得到实时在线轨迹(解决waiting for image，需要在修改ros_rgbd.cc中发布后重新编译ROS/ORB_SLAM2，这里就算是打开过ros_rgbd.cc，但是没有修改里面的内容，也建议重新编译一次)

在上面的编译过程中可能会遇到的问题：

问题1：libboost_system.so 与libboost_filesystem.so找不到链接目录
解决方法：

locate  boost_system

查找到目录

/usr/lib/x86_64-linux-gnu/libboost_system.a
/usr/lib/x86_64-linux-gnu/libboost_system.so
/usr/lib/x86_64-linux-gnu/libboost_system.so.1.58.0 

将libboost_system.a, libboost_system.so复制到ORB_SLAM2/lib

问题2：ORB_SLAM2编译时执行 ./build_ros.sh 出现 error: ‘usleep’ was not declared in this scope
解决方法：
在ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc中加入头文件 #include<unistd.h>

参考博客：kinect+Ros+ORBSLAM2

最后的运行结果：