ubuntu16.04+ROS kinect+DELL一体机单目摄像头运行ORB_SLAM2

首先，我们已经完成了相机标定，生成了head_camera.yaml

image_width: 640
image_height: 480
camera_name: head_camera
camera_matrix:
  rows: 3
  cols: 3
  data: [634.9179818076392, 0, 329.2437180764049, 0, 639.320700174626, 251.6448538152812, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [0.1309044967838789, -0.190156630074109, 0.002588424412321755, 0.001212400848318653, 0]
rectification_matrix:
  rows: 3
  cols: 3
  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
  rows: 3
  cols: 4
  data: [650.2191162109375, 0, 329.9299229989629, 0, 0, 654.1919555664062, 252.713960915602, 0, 0, 0, 1, 0]

接着我们将其写成ORB_SLAM2/Example/ROS/ORB_SLAM2中的Asus.yaml的格式，如下所示

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 634.917981
Camera.fy: 639.320700
Camera.cx: 329.243718
Camera.cy: 251.644853

Camera.k1: 0.130904
Camera.k2: -0.190156
Camera.p1: 0.002588
Camera.p2: 0.001212
Camera.k3: 0

Camera.width: 640
Camera.height: 480

# Camera frames per second 
Camera.fps: 30.0

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1

#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------

# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000

# ORB Extractor: Scale factor between levels in the scale pyramid 	
ORBextractor.scaleFactor: 1.2

# ORB Extractor: Number of levels in the scale pyramid	
ORBextractor.nLevels: 8

# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast			
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7

#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize:2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500

即head_camera.yaml中的camera_matrix为 $\begin{vmatrix} f_x &0 &c_x\\ 0& f_y &c_y \\ 0& 0 & 1 \end{vmatrix}$ 。畸变参数也为一一对应。之后将自己的相机参数保存为mycam.yaml。之后移动到工作空间/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2 中。

这里我们ORB_SLAM2的ROS包已经编译完成，相关ROS路径也以及配置完成。

之后将src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src中的ros_mono.cc

ros::Subscriber sub = nodeHandler.subscribe("/usb_cam/image_raw", 1, &ImageGrabber::GrabImage,&igb);

/usb_cam/image_raw改为自己的摄像头在ROS中发布的节点。重新编译，生成可执行文件Mono

之后就是运行命令啦

rosrun ORB_SLAM2 Mono /home/lee/SLAM/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/lee/SLAM/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/mycam.yaml

这里的路径改为自己的路径就OK啦

ubuntu16.04+ROS kinect+DELL一体机单目摄像头运行ORB_SLAM2

猜你喜欢