用D435i跑高翔ORB_SLAM2稠密建图

---

前言

本文写于2022年5月18日。

1.软件准备

Ubuntu18.04 + ROS melodic

2.硬件准备

InterRealSenseD435i 相机

---

一、前提准备

1.安装ROS melodic

参考链接

2.安装 InterRealSenseD435i SDK2

参考链接

3.编译高翔ORB_SLAM2稠密建图

参考链接

---

二、D435i相机跑高翔ORB_SLAM2稠密建图

1.修改一个文件

修改ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc文件

    //message_filters::Subscriber rgb_sub(nh, "/camera/rgb/image_raw", 1);
    //message_filters::Subscriber depth_sub(nh, "camera/depth_registered/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/color/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/aligned_depth_to_color/image_raw", 1);

2.重新编译 ./build_ros.sh

进入 ORB_SLAM2_modified 文件夹

cd ORB_SLAM2_modified

给 build_ros.sh 文件权限

chmod +x build_ros.sh

编译 build_ros.sh 文件

./build_ros.sh

3.查看相机内参

打开一个终端输入

roslaunch realsense2_camera rs_rgbd.launch

再打开一个终端输入

rostopic echo /camera/color/camera_info

查看到的相机内参如下

header: 
  seq: 2411
  stamp: 
    secs: 1652874980
    nsecs:   1782894
  frame_id: "camera_color_optical_frame"
height: 720
width: 1280
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [910.0997314453125, 0.0, 639.4933471679688, 0.0, 909.994873046875, 359.3774108886719, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [910.0997314453125, 0.0, 639.4933471679688, 0.0, 0.0, 909.994873046875, 359.3774108886719, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False

     |fx  0  cx|
k=|0  fy  cy|
     |0   0   1 |
Camera.fx: 910.099731
Camera.fy: 909.994873
Camera.cx: 639.493347
Camera.cy: 359.377410

4.创建配置文件

切换到ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2目录下
打开终端输入

cd ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2

新建MyD435i.yaml文件
在终端里输入

touch MyD435i.yaml

将以下内容复制到MyD435i.yaml文件里并保存

%YAML:1.0
 
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
 
# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 910.099731
Camera.fy: 909.994873
Camera.cx: 639.493347
Camera.cy: 359.377410

Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.p3: 0.0

Camera.width: 640
Camera.height: 480
# Camera frames per second 
Camera.fps: 30.0

# IR projector baseline times fx (aprox.)
# bf = baseline (in meters) * fx, D435i的 baseline = 50 mm 
Camera.bf: 50.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: 1000.0
 
#--------------------------------------------------------------------------------------------
# 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
 
PointCloudMapping.Resolution: 0.01
meank: 50
thresh: 2.0

5.运行

在ORB_SLAM2_modified文件夹下打开一个终端输入

roslaunch realsense2_camera rs_rgbd.launch

在ORB_SLAM2_modified文件夹下再打开一个终端输入

rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/MyD435i.yaml

即可运行成功