写在前面
上一篇文章，记录了Ubuntu下安装Kinect2的驱动，以及在ROS下运行bridge.现在已经迫不及待的要跑下ORB-SLAM2下的RGB-D接口。
修改ORB-SLAM2
安装好Kinect2的驱动后，执行：
roslaunch kinect_bridge kinect_bridge.launch然后执行以下命令查询ROS发布的话题
rostopic list话题如下：
/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
/kinect2/qhd/image_mono_rect/compressed
/kinect2/qhd/points
/kinect2/sd/camera_info
/kinect2/sd/image_color_rect
/kinect2/sd/image_color_rect/compressed
/kinect2/sd/image_depth
/kinect2/sd/image_depth/compressed
/kinect2/sd/image_depth_rect
/kinect2/sd/image_depth_rect/compressed
/kinect2/sd/image_ir
/kinect2/sd/image_ir/compressed
/kinect2/sd/image_ir_rect
/kinect2/sd/image_ir_rect/compressed
/kinect2/sd/points
/rosout
/rosout_agg这里使用/kinect2/qhd/image_color和/kinect2/qhd/image_depth_rect。
打开ORB_SLAM2/Example/ROS/ORB_SLAM2/src/ros_rgbd.cc，将对应语句修改为：
message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh,"/kinect2/qhd/image_color",1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh,"/kinect2/qhd/image_depth_rect",1);2.kinect2的标定（获取内参和畸变参数）
参考https://github.com/code-iai/iai_kinect2/tree/master/kinect2_calibration#calibrating-the-kinect-one标定Kinect2，获取内参和畸变系数后，在Examples/RGB-D/下新建kinect2.yaml，参照Examples/RGB-D/TUM1.yaml，修改对应参数。
# Camera calibration and distortion parameters (OpenCV)
Camera.fx: 1050.6
Camera.fy: 1051.3
Camera.cx: 938.0
Camera.cy: 554.4
Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.width: 1920
Camera.height: 1080
# Camera frames per second
Camera.fps: 30.0
最后执行以下命令即可启动ORB-SLAM2。
rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/kinect2.yaml3.编译build_ros.sh脚本文件
配置ROS_PACKAGE_PATH环境变量
source ~/.zshrc
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/xuzifeng/ORB_SLAM2/Examples/ROS
进入ORB_SLAM2主路径编译ros节点：
chmod +x build_ros.sh
./build_ros.sh
确保原程序可以编译，我在编译的阶段遇到如下问题：
/usr/bin/ld: CMakeFiles/RGBD.dir/src/ros_rgbd.cc.o: undefined reference to symbol ‘_ZN5boost6system15system_categoryEv’
- 出错原因为：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
locate boost_filesystem
//查找到目录
/usr/lib/x86_64-linux-gnu/libboost_filesystem.a
/usr/lib/x86_64-linux-gnu/libboost_filesystem.so
/usr/lib/x86_64-linux-gnu/libboost_filesystem.so.1.58.0
将libboost_system.so与libboost_filesystem.so复制到ORB_SLAM2/lib下，将ORB_SLAM2/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
)
之后加入 -lboost_system,修改之后的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
-lboost_system
)4.重新编译工程
//重新编译工程
chmod +x build_ros.sh
./build_ros.sh编译成功！！接下来就是启动kinect2的驱动和bridge，并且运行Examples/ROS/RBGD
5.运行程序
打开一个终端，运行：
roscore
//新开一个终端 我这里已经设置了ROS 的启动路径 不需要source devel/setup.sh
roslaunch kinect2_bridge kinect2_bridge.launch
接着cd 到ORB_SLAM2/Examples/ROS/ORB_SLAM2路径下 新开一个终端输入：
rosrun ORB_SLAM2 RGBD /home/xuzifeng/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/xuzifeng/ORB_SLAM2/Examples/RGB-D/kinect2.yaml
运行成功！！！