7
Help us understand the problem. What are the problem?

posted at

updated at

ORB SLAM3 with OAK-D & ROS1 noetic on LXD

Preface

In this article, I will explain how to run ORB SLAM3 with OAK-D stereo camera and ROS1 noetic on LXD. I assume the reader is using Ubuntu as the host machine OS and has some primary knowledge about ROS1 and LXD.

General Settings

In this section, I will explain about not ORB-SLAM3 specific settings.

OAK-D Settings

To use OAK-D on Linux, you have to set udev rule like the following. See here for an official document.
This is needed only once.

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

LXD Settings

Firstly of all, we need to launch LXC container ("noetic" in this article) to use.

export container_name=noetic
lxc launch ubuntu:20.04 $container_name

OAK-D Pass-Through

To use the OAK-D on LXD, you have to pass through the OAK-D to the container. As mentioned in the official document, we have to pass through 2 devices because OAK-D changes its device id after the boot.

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

Open GL Settings

Because ROS1's GUI tools like RViz uses OpenGL v1.4, we have to let LXC call Host's OpenGL API. The following commands came from this blog post.

The command needed only once

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

The commands needed for each containers

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

Because NVIDIA provides additional packages for controlling its capability management for containers, we need addtional settings for the host machine equipped with NVIDA's GPUs.

lxc config set $container_name nvidia.driver.capabilities all
lxc config set $container_name nvidia.runtime "true"

In Container Installation

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

lxc exec $container_name -- sudo --login --user ubuntu

Installing ROS1 noetic

Follow the official instruction to install ros-noetic-desktop.

Following commands are just for completeness. If you find any contradiction with the official document, follow the official one.

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 python3-rosinstall python3-rosinstall-generator python3-wstool build-essential
sudo apt install python3-rosdep
sudo rosdep init
rosdep update

Building ORB SLAM3

In this article, we assume using ORB SLAM3 v0.4, the latest version at the time of writing.

Installing OpenCV4.5.5

Because ORB SLAM3 requires OpenCV 4.4 but Ubuntu 20.04 provides OpenCV 4.2 as an official package, we have to install OpenCV manually.
CAUTION Following commands I present will overwrite your current OpenCV installation. Since I assume you are using an LXC container, it is not harmful, but if you are trying to follow this instruction on your local, do so at your own risk.
Following instructions install OpenCV 4.5.5, the latest version at the time of writing but you can install another version if you want though I did not test that.

Firstly, download the unofficial bash script. I'm using this because it is easy to use.

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

Change the OPENCV_VERSION in the script to OPENCV_VERSION=4.5.5 by opening it with a text editor like vi.

vi install-opencv.sh

Just run it.

bash install-opencv.sh

Installing Pangolin v0.6

ORB SLAM3 seems to be assuming Pangolin v0.6, the latest stable version of Pangolin at the time of writing, is already installed. Therefore, we will install it by following commands.

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

Building ORB SLAM3 library

At the time of writing, official repository does not support ROS, so I made a fork with ROS1 support.

git clone https://github.com/nindanaoto/ORB_SLAM3.git
cd ORB_SLAM3
bash build.sh

Building the ROS Wrapper of ORB SLAM3

After that, build it by following commands. Because Ubuntu 20.04's default python interpreter is Python2, we change it by installing the python-is-python3 package.

sudo apt install python-is-python3
echo "export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:~/ORB_SLAM3/Examples/ROS" >> ~/.bashrc
source ~/.bashrc
bash build_ros.sh
cd ..

Installing depthai-ros

To publish OAK-D's data on ROS1 network, I used depthai-ros. It does not include IMU support at the time of writing but it is the canonical way.

Install commands are like the following.

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

Installing stereo_image_proc

To undistort the camera images, I used stereo_image_proc.

sudo apt install ros-noetic-stereo-image-proc

Running ORB SLAM3

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

When You Can't Find The Device

In this section, you may try to connect OAK-D to the lxc container and can't find the device. In this case, try to restart the container. This error may be caused by the LXC's USB pass-through behavior. I have not resolved this issue yet.

Camera Parameter

ORB SLAM3 needs YAML setting file which describes the camera parameters. OAK-D's parameter 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

My output is presented below 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]]                                                                                                                                                                                                                                                                                  
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.        ]]

The important part is here.

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 like the following.

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 to that, because the baseline of the OAK-D is 0.75m,Camera.bf is 0.75*Camera.fx.

The distortion coefficients and rectification matrices are not needed because they will be corrected by stereo_image_proc.

Therefore, the example YAML file will be like the following.

%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

Running the node

In this subsection, we try to run the ORB SLAM3. This instruction needs two terminals that are logging into the container's shell. Of course, one terminal and two windows by tmux also work;)

1st terminal

Launch the stereo node for OAK-D.

roslaunch depthai_examples stereo_inertial_node.launch

2nd terminal

Run stereo_image_proc to undistort the images.

ROS_NAMESPACE=/stereo_inertial_publisher/ rosrun stereo_image_proc stereo_image_proc

3rd terminal

I assume the name of config YAML file is ~/oak-d-params.yaml.

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

I hope you will be able to see ORB SLAM3's output now ;)

Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
Sign upLogin
7
Help us understand the problem. What are the problem?