Kinect2通过ROS跑ORB_SLAM2 下RGB-D接口
写在前面
上一篇文章，记录了Ubuntu的下安装Kinect2的驱动，以及在ROS下运行的桥梁。现在已经迫不及待的要跑下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，获取内参和畸变系数后，在实施例/ 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：对符号'_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 /实施例/ 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的驱动和桥，并且运行实例/ 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运行成功！
