手把手教你基于LXD用OAK-D和ROS noetic做ORB SLAM3
编辑：OAK中国
首发：oakchina.cn
喜欢的话，请多多⭐️✍
Hello，大家好，这里是OAK中国，我是助手君。
本文内容来自nindanaoto，由OAK中国整理。
一、前言
在本文中，我将解释如何基于LXD用OAK-D和ROS noetic做ORB SLAM3。我假设读者正在使用Ubuntu作为主机操作系统，并且对ROS和LXD有一些基本知识。
二、一般设置
在本节中，我将解释有关非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的输出。
参考资料
https://docs.oakchina.cn/en/latest/
https://www.oakchina.cn/selection-guide/
OAK中国
| OpenCV AI Kit在中国区的官方代理商和技术服务商
| 追踪AI技术和产品新动态
戳「+关注」获取最新资讯↗↗