使用oak-d-pro相机跑ORB-SLAM2以及稠密建图

一.编译算法.

安装过程全部来自于官方开源代码，具体安装过程不再阐述：

GitHub - raulmur/ORB_SLAM2: Real-Time SLAM for Monocular, Stereo and RGB-D Cameras, with Loop Detection and Relocalization CapabilitiesReal-Time SLAM for Monocular, Stereo and RGB-D Cameras, with Loop Detection and Relocalization Capabilities - GitHub - raulmur/ORB_SLAM2: Real-Time SLAM for Monocular, Stereo and RGB-D Cameras, with Loop Detection and Relocalization Capabilitieshttps://github.com/raulmur/ORB_SLAM2

二.编译时遇到的问题

1.compile error：usleep was not declared in this scope

这是缺少头文件导致的。有10几个文件都缺，通过报错信息找到这些文件，添加头文件。

#include <unistd.h>

2.Pangolin could not be found because dependency Eigen3 could not be found.

Pangolin版本的问题，不需要重新安装Pangolin及Eigen，修改CMakeLists.txt如下：在find_package(Eigen3 REQUIRED)后加NO_MUDULE

find_package(Eigen3 REQUIRED NO_MODULE)

总的来说没有遇见太多问题。

三.

运行orb-slam2算法需要标定相机参数。我按照一位博主的方法进行标定，然后修改参数文件。参考：

ORB-SLAM2调用OAK-D双目摄像头进行点云建图_A. hyh的博客-优快云博客

用oak-D双目相机实现ORB-SLAM2双目实例-优快云博客

以下是我的运行参数：

%YAML:1.0
 
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
 
# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 804.9215698242188
Camera.fy: 804.9215698242188
Camera.cx: 640.6663818359375
Camera.cy: 362.4100036621094
 
Camera.k1: 0
Camera.k2: 0
Camera.p1: 0
Camera.p2: 0
 
Camera.width: 1280
Camera.height: 720
 
# Camera frames per second 
Camera.fps: 20.0
 
# stereo baseline times fx
Camera.bf: 60.438729354619866
 
# 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: 35
 
#--------------------------------------------------------------------------------------------
# Stereo Rectification. Only if you need to pre-rectify the images.
# Camera.fx, .fy, etc must be the same as in LEFT.P
#--------------------------------------------------------------------------------------------
LEFT.height: 720
LEFT.width: 1280
LEFT.D: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [-9.900439262390137, 96.50916290283203, 0.0009822448482736945, -0.00023291604884434491]
LEFT.K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [804.9215698242188, 0.0, 640.6663818359375, 0.0, 804.9215698242188, 362.4100036621094, 0.0, 0.0, 1.0]
LEFT.R:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [0.9999611377716064, -0.006486319005489349, -0.005967340432107449, 0.006491821724921465, 0.9999785423278809, 0.0009032952948473394, 0.005961353424936533, -0.0009419990819878876, 0.9999817609786987]
LEFT.P:  !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [804.9215698242188, 0.0, 640.6663818359375, 60.438729354619866, 0.0, 804.9215698242188, 362.4100036621094, 0.0, 0.0, 0.0, 1.0, 0.0]

RIGHT.height: 720
RIGHT.width: 1280
RIGHT.D: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [-9.900439262390137, 96.50916290283203, 0.0009822448482736945, -0.00023291604884434491]
RIGHT.K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [806.0142211914062, 0.0, 664.4835205078125, 0.0, 806.0142211914062, 370.78265380859375, 0.0, 0.0, 1.0]
RIGHT.R:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [0.9999005794525146, -0.005074762273579836, 0.013157090172171593, 0.005086900200694799, 0.9999866485595703, -0.0008892129990272224, -0.013152401894330978, 0.0009560533799231052, 0.9999130368232727]
RIGHT.P:  !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [806.0142211914062, 0.0, 664.4835205078125, 60.438729354619866, 0.0, 806.0142211914062, 370.78265380859375, 0.0, 0.0, 0.0, 1.0, 0.0]
 
 
#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------
 
# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1200
 
# 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

四. 运行

1.运行相机，使用depthai-ros。具体教程也参考OAK中国的官方。

cd your_ws
source devel/setup.bash
roslaunch depthai_examples stereo_node.launch

2.运行orb-slam2算法，这里我先运行双目建图模式。

3.运行效果

总结：

运行效果一般，还是存在特征点容易丢失的问题。感觉还是标定有问题，标定应该有待改进。

五.另外一个改进后的orb-slam2算法

这位的算法效果要好一些，并且不容易丢失，带有保存地图的功能。安装过程和运行过程和官方都是一样的。

GitHub - BoomFan/ORB_SLAM2: Real-Time SLAM for Monocular, Stereo and RGB-D Cameras, with Loop Detection and Relocalization Capabilities

运行效果

六.稠密建图

使用高翔博士的算法，具体安装过程参考这位

高翔ORB-SLAM2稠密建图编译（添加实时彩色点云地图+保存点云地图）_高翔orb_slam2稠密建图编译-优快云博客

安装算法后跑数据集没有问题，运行效果如下：

但是使用oak相机跑rgb模式，没有点云，还在解决中.......