Teach you how to do ORB SLAM3 with OAK-D and ROS noetic based on LXD

Translation | OAK China
Author| nindanaoto
This article was first published in oakchina.cn

1, Foreword

In this article, I will explain how to use based on LXD OAK-D ORB SLAM3 with ROS noetic. I assume that the reader is using Ubuntu as the host operating system and has some basic knowledge of ROS and LXD.

2, General settings

In this section, I will explain the non ORB-SLAM3 specific settings.

2.1 setting OAK-D

To use OAK-D on Linux, you must set UDEV rules, as shown below. Click here to see the official documents.

(this only takes one time.)

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 setting LXD

First, we need to start the LXC container ("noetic" in this article) to use.

2.2.1 OAK-D connection LXD

To use OAK-D on an LXD container, you must connect OAK-D to the LXD container. just as official document As mentioned in, we must pass through two devices, because OAK-D will change its device ID after startup.

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 settings

Because ROS GUI tools such as RViz use OpenGL v1 4. We must let LXC call the OpenGL API of the Host. The following command comes from This blog post.

(1) The following command is used only once

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

(2) The following commands are required for each container

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'

For NVIDA GPU users:

Since NVIDIA provides additional software packages to control its ability to manage containers, we need to make additional settings for hosts equipped with NVIDIA GPU.

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

3, Vessel installation

We should log in to the container to install the program described in this section.

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

3.1 installing ROS noetic

according to Official documents To install ROS noetic desktop.

The following command is only for complete explanation. If you find any contradiction with the official documents, please follow the official documents.

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 build ORB SLAM3

In this article, we assume to use orb slam3 v0 4, the latest version at the time of writing.

3.2.1 install opencv4 five point four

Because ORB SLAM3 requires OpenCV 4.4, but Ubuntu 20.04 provides OpenCV 4.2 as the official software package, so we must install OpenCV manually.

Note that my command will overwrite your current OpenCV installation. Because I assume that you use LXC container, there will be no harm, but if you want to follow this instruction locally, please bear your own risk.

The following instructions are to install OpenCV 4.5.4, which is the latest version at the time of writing, but you can also install other versions if you want, but I didn't test it.

First, download the unofficial bash script. I use this because it's easy to use.

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

Open the script with a text editor (such as vi) and open opencv in it_ Version changed to OPENCV_VERSION=4.5.4

vi install-opencv.sh


bash install-opencv.sh

3.2.2 installing pangolin v0 six

ORB SLAM3 seems to assume that Pangolin v0.0 is already installed 6, the latest stable version of Pangolin at the time of writing. Therefore, we will install it with the following command.

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 ..
sudo make install
cd ../../

3.2.3 build ORB SLAM3 Library

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

3.2.4 build ROS wrapper of ORB SLAM3

Since we installed OpenCV 4.5, we must change the corresponding CMake settings through the text editor to use 4.5.

vi Examples/ROS/ORB_SLAM3/CMakeLists.txt

After that, build it with the following command. Because the default Python interpreter for Ubuntu 20.04 is Python 2, we can change it by installing the python-is-python 3 package.

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

3.3 installation of depthai ROS

In order to publish the message data of OAK-D on ROS, I use depthai ros. At the time of writing this article, it does not support IMU, but it is a typical way.

The installation command is as follows.

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
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
echo "export ROS_PACKAGE_PATH=\${ROS_PACKAGE_PATH}:~/ORB_SLAM3/Examples/ROS" >> ~/.bashrc

4, Run ORB SLAM3

In this section, I will explain how to run the installed ORB SLAM3 with OAK-D.

4.1 unable to find device

In this section, you may try to connect OAK-D to the LxC container, but you can't find the device. In this case, try restarting the container. This error may be caused by LxC's USB penetration behavior. I haven't solved the problem yet.

4.2 camera parameters

ORB SLAM3 requires YAML setting files to describe camera parameters. The parameters of OAK-D can be extracted by running EEPROM dump in the container.

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

Let's take my output as an example.

RGB Camera Default intrinsics...                                                                                                                                                                                                                                                                                                                                                             
[[1570.975830078125, 0.0, 970.232666015625], [0.0, 1568.9881591796875, 540.0018310546875], [0.0, 0.0, 1.0]]                                                                                                                                                                                                                                                                                  
/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.        ]]

The important parts are as follows:

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.        ]]

The meaning of these matrices is as follows:

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.        ]]

In addition, because the baseline of OAK-D is 0.75m, camera BF is set to 0.75 * camera fx.

They don't seem to be corrected on the distortion matrix.

Therefore, the example of YAML file will look like the following:


# 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.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500

4.3 operation node

In this section, we try to run ORB SLAM3. This instruction requires two terminals to log into the shell program of the container. Of course, one terminal and two windows can also work through tmux.

First terminal

Start the depth node of OAK-D.

roslaunch depthai_examples stereo_node.launch

Second terminal

I assume that the configured YAML file name is ~ / 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 

I hope you can see the output of ORB SLAM3 now.

OAK China |Track new developments in AI technology and products
official account |OAK visual artificial intelligence development
Click here "Add wechat friends (note the purpose)
Stamp "+ follow" for the latest information ↗↗

Keywords: OpenCV AI Computer Vision

Added by thinguy on Tue, 08 Feb 2022 10:23:11 +0200