如何基于LXD用OAK-D和ROS noetic做ORB SLAM3?

如何基于LXD用OAK-D和ROS noetic做ORB SLAM3?

翻译 | OAK中国
作者 | nindanaoto

一、前言

在本文中,我将解释如何基于LXD用OAK-D和ROS noetic做ORB SLAM3。我假设读者正在使用Ubuntu作为主机操作系统,并且对ROS和LXD有一些基本知识。

OAK-D

二、一般设置

在本节中,我将解释有关非ORB-SLAM3特定设置的内容。

2.1 设置OAK-D

要在Linux上使用OAK-D,您必须设置UDEV规则,如下所示。点击这里看官方文件。

(这只需要一次。)

echo 'SUBSYSTEM=="usb", ATTRS{idVendor}=="03e7", MODE="0666"' | sudo tee /etc/udev/rules.d/80-movidius.rules
sudo udevadm control --reload-rules && sudo udevadm trigger

2.2 设置LXD

首先,我们需要启动LXC容器(本文中的“noetic”)来使用。

2.2.1 OAK-D连通LXD

要在LXD容器上使用OAK-D,你必须连通OAK-D到LXD容器。正如官方文件中提到的,我们必须通过两个设备,因为OAK-D在启动后会改变其设备ID。

lxc config device add noetic oak-d usb vendorid=03e7 productid=2485
lxc config device add noetic oak-d-loop usb vendorid=03e7 productid=f63b
lxc config device set noetic oak-d-loop mode 0666

2.2.2 OpenGL设置

因为ROS的GUI工具如RViz使用OpenGL v1.4,我们必须让LXC调用Host的OpenGL API。下面的命令来自这篇博文

(1)以下命令只用一次

echo "root:$UID:1" | sudo tee -a /etc/subuid /etc/subgid

(2)以下命令每个容器都需要

lxc config set noetic raw.idmap "both $UID 1000"
lxc restart noetic
lxc config device add noetic X0 disk path=/tmp/.X11-unix/X0 source=/tmp/.X11-unix/X0 
lxc config device add noetic Xauthority disk path=/home/ubuntu/.Xauthority source=${XAUTHORITY}
lxc config device add noetic mygpu gpu
lxc config device set noetic mygpu uid 1000
lxc config device set noetic mygpu gid 1000
lxc exec noetic -- bash -c 'echo "export DISPLAY =:0" >> /home/ubuntu/.bashrc'

对于NVIDA GPU 用户:

由于英伟达提供了额外的软件包来控制其对容器的能力管理,我们需要对配备英伟达GPU的主机进行额外的设置。

lxc config set noetic nvidia.driver.capabilities all
lxc config set noetic nvidia.runtime "true"

三、容器安装

我们应该登录到容器中去安装本节所述的程序。

lxc exec noetic -- sudo --login --user ubuntu

3.1 安装ROS noetic

按照官方的文档来安装ros-noetic-desktop。

下面的命令只是为了完整地说明问题。如果你发现与官方文件有任何矛盾,请遵循官方文件。

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
sudo apt update
sudo apt install ros-noetic-desktop
echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
source ~/.bashrc
sudo apt install python3-rosdep
sudo rosdep init
rosdep update

3.2 构建ORB SLAM3

在本文中,我们假设使用ORB SLAM3 v0.4,即写作时的最新版本。

3.2.1 安装OpenCV4.5.4

因为ORB SLAM3需要OpenCV 4.4,但Ubuntu 20.04提供了OpenCV 4.2作为官方软件包,所以我们必须手动安装OpenCV。

注意 我提出的命令将覆盖您当前的OpenCV安装。因为我假设你使用的是LXC容器,所以不会有什么危害,但如果你想在本地按照这个指令操作,请自行承担风险。

下面的说明是安装OpenCV 4.5.4,这是写作时的最新版本,但如果你想的话,也可以安装其他版本,不过我没有测试。

首先,下载非官方的bash脚本。我用这个是因为它很容易使用。

wget --no-check-certificate https://raw.githubusercontent.com/milq/milq/master/scripts/bash/install-opencv.sh

用文本编辑器(如vi)打开脚本,将其中的OPENCV_VERSION改为OPENCV_VERSION=4.5.4

vi install-opencv.sh

运行

bash install-opencv.sh

3.2.2 安装Pangolin v0.6

ORB SLAM3似乎假定已经安装了Pangolin v0.6,即写作时Pangolin的最新稳定版本。因此,我们将通过以下命令来安装它。

sudo apt install libgl1-mesa-dev libglew-dev ffmpeg libavcodec-dev libavutil-dev libavformat-dev libswscale-dev libavdevice-dev libdc1394-22-dev libraw1394-dev libjpeg-dev libtiff5-dev libopenexr-dev
git clone https://github.com/stevenlovegrove/Pangolin -b v0.6
cd Pangolin
mkdir build
cd build
cmake ..
make
sudo make install
cd ../../

3.2.3 构建ORB SLAM3库

git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git
cd ORB_SLAM3
bash build.sh

3.2.4 构建ORB SLAM3的ROS封装器

由于我们安装了OpenCV 4.5,我们必须通过文本编辑器改变相应的CMake设置以使用4.5。

vi Examples/ROS/ORB_SLAM3/CMakeLists.txt

之后,通过以下命令构建它。因为Ubuntu 20.04的默认Python解释器是Python2,我们通过安装python-is-python3包来改变它。

sudo apt install python-is-python3
source ~/.bashrc
bash build_ros.sh
cd ..

3.3 安装depthai-ros

为了在ROS上发布OAK-D的消息数据,我使用了depthai-ros。在写这篇文章的时候,它不支持IMU,但它是典型的方式。

安装命令如下。

sudo wget -qO- https://raw.githubusercontent.com/luxonis/depthai-ros/noetic-devel/install_dependencies.sh | sudo bash
sudo apt install python3-vcstool
mkdir -p catkin_ws/src
cd catkin_ws
wget https://raw.githubusercontent.com/luxonis/depthai-ros/noetic-devel/underlay.repos
vcs import src < underlay.repos
rosdep install --from-paths src --ignore-src -r -y
catkin_make
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
echo "export ROS_PACKAGE_PATH=\${ROS_PACKAGE_PATH}:~/ORB_SLAM3/Examples/ROS" >> ~/.bashrc

四、运行ORB SLAM3

在这一节中,我将解释如何用OAK-D运行已安装的ORB SLAM3。

4.1 无法找到设备

在这一节中,你可能试图将OAK-D连接到lxc容器上,但找不到设备。在这种情况下,请尝试重新启动容器。这个错误可能是由LXC的USB穿透行为引起的。我还没有解决这个问题。

4.2 相机参数

ORB SLAM3需要YAML设置文件来描述相机的参数。OAK-D的参数可以通过在容器中运行EEPROM dump来提取。

python3 -m pip install --extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-python-snapshot-local/ depthai
git clone https://github.com/luxonis/depthai-python
python3 depthai-python/examples/calibration/calibration_reader.py

下面以我的输出为例进行介绍。

RGB Camera Default intrinsics...                                                                                                                                                                                                                                                                                                                                                             
[[1570.975830078125, 0.0, 970.232666015625], [0.0, 1568.9881591796875, 540.0018310546875], [0.0, 0.0, 1.0]]                                                                                                                                                                                                                                                                                  
1920                                                                                                                                                                                                                                                                                                                                                                                         
1080                                                                                                                                                                                                                                                                                                                                                                                         
/home/nimda/sources/depthai-python/examples/calibration/calibration_reader.py:23: VisibleDeprecationWarning: Creating an ndarray from ragged nested sequences (which is a list-or-tuple of lists-or-tuples-or ndarrays with different lengths or shapes) is deprecated. If you meant to do this, you must specify 'dtype=object' when creating the ndarray                                   
  M_rgb, width, height = np.array(calibData.getDefaultIntrinsics(dai.CameraBoardSocket.RGB))                                                                                                                                                                                                                                                                                                 
LEFT Camera resized intrinsics...                                                                                                                                                                                                                                                                                                                                                            
[[836.39874268   0.         636.00115967]                                                                                                                                                                                                                                                                                                                                                    
 [  0.         836.66235352 357.10168457]                                                                                                                                                                                                                                                                                                                                                    
 [  0.           0.           1.        ]]                                                                                                                                                                                                                                                                                                                                                   
LEFT Distortion Coefficients...                                                                                                                                                                                                                                                                                                                                                              
k1: 12.44894027709961                                                                                                                                                                                                                                                                                                                                                                        
k2: 20.618579864501953                                                                                                                                                                                                                                                                                                                                                                       
p1: 0.0032805176451802254                                                                                                                                                                                                                                                                                                                                                                    
p2: -0.0014607576886191964                                                                                                                                                                                                                                                                                                                                                                   
k3: 261.510986328125                                                                                                                                                                                                                                                                                                                                                                         
k4: 12.51024341583252                                                                                                                                                                                                                                                                                                                                                                        
k5: 19.012231826782227                                                                                                                                                                                                                                                                                                                                                                       
k6: 260.66668701171875                                                                                                                                                                                                                                                                                                                                                                       
s1: 0.0                                                                                                                                                                                                                                                                                                                                                                                      
s2: 0.0                                                                                                                                                                                                                                                                                                                                                                                      
s3: 0.0                                                                                                                                                                                                                                                                                                                                                                                      
s4: 0.0                                                                                                                                                                                                                                                                                                                                                                                      
τx: 0.0                                                                                                                                                                                                                                                                                                                                                                                      
τy: 0.0                                                                                                                                                                                                                                                                                                                                                                                      
RIGHT Camera resized intrinsics...                                                                                                                                                                                                                                                                                                                                                           
[[843.39074707   0.         635.90380859]                                                                                                                                                                                                                                                                                                                                                    
 [  0.         843.87249756 359.52798462]                                                                                                                                                                                                                                                                                                                                                    
 [  0.           0.           1.        ]]                                                                                                                                                                                                                                                                                                                                                   
RIGHT Distortion Coefficients...                                                                                                                                                                                                                                                                                                                                                             
k1: 8.136334419250488                                                                                                                                                                                                                                                                                                                                                                        
k2: 15.044692039489746                                                                                                                                                                                                                                                                                                                                                                       
p1: 0.0011016841745004058                                                                                                                                                                                                                                                                                                                                                                    
p2: -0.001027329359203577                                                                                                                                                                                                                                                                                                                                                                    
k3: 176.60096740722656                                                                                                                                                                                                                                                                                                                                                                       
k4: 8.13979721069336                                                                                                                                                                                                                                                                                                                                                                         
k5: 13.856715202331543                                                                                                                                                                                                                                                                                                                                                                       
k6: 176.6233367919922                                                                                                                                                                                                                                                                                                                                                                        
s1: 0.0                                                                                                                                                                                                                                                                                                                                                                                      
s2: 0.0                                                                                                                                                                                                                                                                                                                                                                                      
s3: 0.0                                                                                                                                                                                                                                                                                                                                                                                      
s4: 0.0                                                                                                                                                                                                                                                                                                                                                                                      
τx: 0.0
τy: 0.0
RGB FOV 68.7938003540039, Mono FOV 71.86000061035156
LEFT Camera stereo rectification matrix...
[[ 1.01874362e+00 -2.81472561e-02 -1.40969483e+01]
 [ 3.21303086e-02  1.00703708e+00 -1.84303370e+01]
 [ 1.70172454e-05 -3.45508922e-06  9.90305330e-01]]
RIGHT Camera stereo rectification matrix...
[[ 1.01029788e+00 -2.79067627e-02 -8.64526404e+00]
 [ 3.18639371e-02  9.98432838e-01 -1.76077311e+01]
 [ 1.68761665e-05 -3.42556854e-06  9.90394469e-01]]
Transformation matrix of where left Camera is W.R.T right Camera's optical center
[[ 9.99938488e-01  7.27824634e-03 -8.37180577e-03 -7.47539711e+00]
 [-7.23146135e-03  9.99958158e-01  5.60518634e-03  2.47205094e-01]
 [ 8.41225125e-03 -5.54430112e-03  9.99949217e-01  4.30144593e-02]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]
Transformation matrix of where left Camera is W.R.T RGB Camera's optical center
[[ 0.9997673  -0.01337837 -0.0169198  -3.73851228]
 [ 0.01365407  0.99977416  0.01628564  0.16693963]
 [ 0.0166981  -0.01651287  0.99972415 -0.08542535]
 [ 0.          0.          0.          1.        ]]

重要的部分如下:

LEFT Camera resized intrinsics...
[[836.39874268   0.         636.00115967]
 [  0.         836.66235352 357.10168457]
 [  0.           0.           1.        ]]
RIGHT Camera resized intrinsics...                                                                                                                                                                                                                                                                                                                                                           
[[843.39074707   0.         635.90380859]                                                                                                                                                                                                                                                                                                                                                    
 [  0.         843.87249756 359.52798462]                                                                                                                                                                                                                                                                                                                                                    
 [  0.           0.           1.        ]]

这些矩阵的意义就像下面这样:

LEFT Camera resized intrinsics...
[[  Camera.fx    0.            Camera.cx]
 [  0.         Camera.fy       Camera.cy]
 [  0.           0.           1.        ]]
RIGHT Camera resized intrinsics...                                                                                                                                                                                                                                                                                                                                                           
[[  Camera2.fx   0.           Camera2.cx]
 [  0.         Camera2.fy     Camera2.cy]
 [  0.           0.           1.        ]]

另外,因为OAK-D的基线是 0.75m,Camera.bf设为0.75*Camera.fx

畸变系数(distortion coefficients)和矫正矩阵(rectification matrices)是不需要的,因为它们似乎在设备上被纠正了。

因此,YAML文件的例子将像下面这样:

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
Camera.type: "PinHole"

# Camera calibration and distortion parameters (OpenCV) 
# Copied from the EEPROM data dump in depthai_demo.py.
Camera.fx: 836.39874268
Camera.fy: 836.66235352
Camera.cx: 636.00115967
Camera.cy: 357.10168457

Camera.k1: 0.0
Camera.k2: 0.0
Camera.k3: 0.0
Camera.k4: 0.0
Camera.p1: 0.0
Camera.p2: 0.0

Camera2.fx: 843.39074707
Camera2.fy: 843.87249756
Camera2.cx: 635.90380859
Camera2.cy: 359.52798462

Camera2.k1: 0.0
Camera2.k2: 0.0
Camera2.k3: 0.0
Camera2.k4: 0.0
Camera2.p1: 0.0
Camera2.p2: 0.0

Camera.bFishEye: 0

Camera.width: 1280
Camera.height: 720

# Camera frames per second 
Camera.fps: 30.0

# stereo baseline times fx
Camera.bf: 62.729905701

# 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: 37.5

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

4.3 运行节点

在本小节中,我们尝试运行ORB SLAM3。这个指令需要两个终端登录到容器的shell程序。当然,一个终端和两个窗口通过tmux也可以工作。

第一个终端

启动OAK-D的深度节点。

roslaunch depthai_examples stereo_node.launch

第二个终端

我假定配置的YAML文件名是~/oak-d-params.yaml

rosrun ORB_SLAM3 Stereo ORB_SLAM3/Vocabulary/ORBvoc.txt oak-d-params.yaml false /camera/left/image_raw:=/stereo_publisher/left/image /camera/right/image_raw:=/stereo_publisher/right/image 

我希望你现在能够看到ORB SLAM3的输出。