SLAM：Kinect2 運行 ORB-SLAM2

1.安裝

1.1 kinect2 驅動

https://blog.csdn.net/learning_tortosie/article/details/81151128

1.2 ORB-SLAM2

https://blog.csdn.net/learning_tortosie/article/details/79881165

1.3 RGB-D SLAM測試

https://github.com/felixendres/rgbdslam_v2/tree/kinetic

2. 準備工作

2.1 修改 topic

 roslaunch kinect2_bridge kinect2_bridge.launch
 rostopic list

使用/kinect2/qhd/image_color和/kinect2/qhd/image_depth_rect。
打開ORB-SLAM2/Example/ROS/ORBSLAM2/src/ros_rgbd.cc，將對應語句修改爲：

(注意這裏訂閱的是qhd質量的圖像)

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);

重新編譯：

./build_ros.bash

重新編譯。

2.2 Kinect2 標定

2.2.1 標定原理

目的：確定相機內參、外參
基本過程：先保存一些圖片(record)，然後計算標定（calibrate）。
標定： color（彩色圖像） 、ir（紅外圖像）、 sync（幀同步）、 depth（深度圖）。

2.2.2 過程

1、創建文件夾保存

mkdir ~/kinect_cal_data
cd ~/kinect_cal_data

2、調整幀率

  rosrun kinect2_bridge kinect2_bridge _fps_limit:=2

查看設備串口號

[ INFO] [Kinect2Bridge::initDevice] device serial: 019654365047

3、創建文件夾（設備串口號爲文件名）
在~/catkin_ws/src/iai_kinect2/kinect2_bridge/data的文件夾裏建立一個 019654365047 文件夾

mkdir ~/catkin_ws/src/iai_kinect2/kinect2_bridge/data/019654365047

4、標定

標定圖（5x7x0.0335）下載鏈接

0.0335爲黑長度（米）
標定彩色攝像頭

rosrun kinect2_calibration kinect2_calibration chess5x7x0.0335 record color

多按幾次 空格 保存數據

rosrun kinect2_calibration kinect2_calibration chess5x7x0.0335 calibrate color

生成 calib_color.yaml 文件

標定紅外 (有些打印機墨水能反射紅外線，導致無法標定)

rosrun kinect2_calibration kinect2_calibration chess5x7x0.0335 record ir
rosrun kinect2_calibration kinect2_calibration chess5x7x0.0335 calibrate ir

生成 calib_ir.yaml 文件

幀同步

rosrun kinect2_calibration kinect2_calibration chess5x7x0.0335 record sync
rosrun kinect2_calibration kinect2_calibration chess5x7x0.0335 calibrate sync

生成 calib_pose.yaml 文件

深度圖

rosrun kinect2_calibration kinect2_calibration chess5x7x0.0335 record depth
rosrun kinect2_calibration kinect2_calibration chess5x7x0.0335 calibrate depth

會生成calib_depth.yaml 文件

然後再把calib_color.yaml calib_ir.yaml calib_pose.yaml calib_depth.yaml拷貝到/home/robot/catkin_ws/src/iai_kinect2/kinect2_bridge/data/019654365047文件夾中

完成標定

查看效果：

 roslaunch kinect2_bridge kinect2_bridge.launch
 rosrun kinect2_viewer kinect2_viewer

3.修改內參、畸變係數

3.1 概念

相機模型:https://blog.csdn.net/qq_16481211/article/details/79464786

3.2 修改方法

標定後，獲取內參和畸變係數
新建kinect2.yaml，參照Examples/RGB-D/TUM1.yaml，修改對應參數。
可參考 ：https://blog.csdn.net/qingsong1001/article/details/81779236

完成的 kinect2.yaml 文件

%YAML:1.0
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 1.1386177735304464e+03
Camera.fy: 1.1398606109373777e+03
Camera.cx: 9.2864000235608137e+02
Camera.cy: 5.1427995771772953e+02

Camera.k1: 1.5146562244145928e-01
Camera.k2: -4.6187737186969491e-01
Camera.p1: 2.7885197431450999e-03
Camera.p2: -3.5390807028563040e-03
Camera.k3: 3.4976857429131902e-01

Camera.width: 960
Camera.height: 540
# Camera frames per second 
Camera.fps: 30.0
# IR projector baseline times fx (aprox.)
Camera.bf: 40.0
# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1
# Close/Far threshold. Baseline times.
ThDepth: 40.0
# Deptmap values factor 
DepthMapFactor: 5000.0

4. 啓動ORB-SLAM2

最後執行以下命令即可啓動ORB-SLAM2。
(ORB-SLAM2文件夾下)

roslaunch kinect2_bridge kinect2_bridge.launch
rosrun ORB_SLAM2 RGBD  Vocabulary/ORBvoc.txt Examples/RGB-D/kinect2.yaml