寫在前面

上一篇文章，記錄了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的標定（獲取內參和畸變引數）

# 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.yaml

3.編譯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

執行成功！！！