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.

# Hardware Requirement

As far as I tested, you need a CPU stronger than Cerelon N5105 for 400p 30fps images. 4GB RAM can work but the RAM usage will grow with the running time.

# 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](https://docs.luxonis.com/projects/api/en/latest/install/#vmware) 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](https://docs.luxonis.com/projects/api/en/latest/install/#virtual-box), 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](https://docs.luxonis.com/projects/api/en/latest/install/#virtual-box).

#### 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](http://wiki.ros.org/noetic/Installation/Ubuntu) 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 -y ros-noetic-desktop
echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
source ~/.bashrc
sudo apt install -y python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential
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 -y 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 -y 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. Follow [the official install instruction](sudo wget -qO- https://raw.githubusercontent.com/luxonis/depthai-ros/main/install_dependencies.sh | sudo bash).

Install commands are like the following at the time of writing.

```
sudo wget -qO- https://raw.githubusercontent.com/luxonis/depthai-ros/main/install_dependencies.sh | sudo bash
sudo apt install -y python3-vcstool
mkdir -p catkin_ws/src
cd catkin_ws/src
git clone https://github.com/luxonis/depthai-ros.git
cd ..
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 Stereo Mode

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](https://docs.luxonis.com/en/latest/pages/faq/#what-are-the-maximum-depths-visible-by-depthai) is 7.5cm (0.075m),`Camera.bf` is `0.075*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 ;)

# Running ORB SLAM3 in Stereo_Inertial mode

Because OAK-D is equipped with an IMU (BNO085), we can use it for getting more acculate results. 

## Calibrating IMU

Because there are no factory calibration for the IMU on OAK-D, we have to calibrate it by hands. 

### Noise and bias calibration

First of all, we have to calibrate the noise and bias in the IMU to filtering them in ORB SLAM3. 
I used [imu_utils](https://github.com/gaowenliang/imu_utils) for this purpose. 
The usage is relatively simple. 

First of all, we have to build the library.

```
cd ~/catkin_ws/src
git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_make
```


Secondly, we need the launch file like the below as `imu.launch`:

```
<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/oakd/imu"/>
        <param name="imu_name" type="string" value= "BNO085"/>
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <param name="max_time_min" type="int" value= "120"/>
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>
```

Lastly, run it. This calibration collects the noise from a stationary IMU, so we have to put your camera in a quiet place.

```
roslaunch imu.launch
```

After this calibration was finished, you can find the output at `catkin_ws/src/imu_utils/data/BNO085_imu_param.yaml`. For example, below is my output.

```
%YAML:1.0
---
type: IMU
name: BNO085
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 6.1812048014273056e-03
      gyr_w: 5.3543409844795362e-05
   x-axis:
      gyr_n: 4.5418794718006189e-03
      gyr_w: 3.8935636505378507e-05
   y-axis:
      gyr_n: 9.4460641028299049e-03
      gyr_w: 5.1407042612016025e-05
   z-axis:
      gyr_n: 4.5556708296513940e-03
      gyr_w: 7.0287550416991553e-05
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 4.4782638307960559e-02
      acc_w: 1.5676203442719888e-03
   x-axis:
      acc_n: 7.7544942126337599e-02
      acc_w: 3.7845627827887817e-03
   y-axis:
      acc_n: 2.6254504179465756e-02
      acc_w: 3.5277636750032727e-04
   z-axis:
      acc_n: 3.0548468618078330e-02
      acc_w: 5.6552188252685773e-04
```

Based on this output, we have to write YAML configuration file for the kalibr's calibration like below. As you can see, we only need avg-axis values. I assume that this configuration file is saved as `imu.yaml`.

```
#Accelerometers
accelerometer_noise_density: 2.8138017235431984e-02   #Noise density (continuous-time)
accelerometer_random_walk:   7.1952095806215616e-04   #Bias random walk

#Gyroscopes
gyroscope_noise_density:     6.4190421491705540e-04   #Noise density (continuous-time)
gyroscope_random_walk:       2.3553045282732546e-06   #Bias random walk

rostopic:                    /oakd/imu      #the IMU ROS topic
update_rate:                 400.0      #Hz (for discretization of the values above)
```

### Joint Calibration

Because the factory calibration of OAK-D does not include IMU, we have to calibrate Camera-IMU joint calibration to get the position of the IMU in the camera's coordinate.
We can use [Kalibr](https://github.com/ethz-asl/kalibr) for this purpose. 
Though I describe the procedure of this calibration, if you are using OAK-D, you may be able to just copy my configuration because the position of IMU may not vary much between cameras.

### Preparing calibration target

Kalibr needs a calibration target. The generation of the target is described in the [official wiki](https://github.com/ethz-asl/kalibr/wiki/calibration-targets). 
I used my display for displaying the calibration targets although the official procedure seems to be recommending to print it on a large piece of paper.
As far as I tested, a 13.3inch display may work, but the viewing angle of the display can affect the calibration quality because it limits the possible movement of the camera during the calibration.
For example, I used the following configuration for my 4K TN 27.9 inch display, [HP V28](https://jp.ext.hp.com/monitors/personal/hpv28_4k/kakaku/?jumpid=st_cn_p_af_kkc). 
Please change the parameters to make each tag visible enough and the number of tags are as many as possible. (This is my criteria for determining the parameters but I'm not sure whether this is the best one or not.)


```
rosrun kalibr kalibr_create_target_pdf --type apriltag --nx 10 --ny 5 --tspace 0.3
```

To use the generated target, we have to write the configuration YAML file which matches the parameters used in the generation like the following. `tagSize` is the actual size of the tag on the display measured by a physical measure or something like that.


```
target_type: 'aprilgrid' #gridtype
tagCols: 10               #number of apriltags
tagRows: 5               #number of apriltags
tagSize: 0.0456           #size of apriltag, edge to edge [m]
tagSpacing: 0.3          #ratio of space between tags to tagSize
```

In this post, I assume that the configuration file is saved as `april.yaml`, 

### Write Factory calibrated YAML

Camera-IMU joint calibration generally needs camera-only calibration before the joint calibration however, because the OAK-D has factory calibration data, just formatting it in the kalibr's format is fine, like the following. I assume that this file is saved as `factory-camchain-oak-d-stereo.yaml`.
Because the distortion model used in the factory calibration is not supported in kalibr, I assumed using `stereo_image_proc` to distort the images, so `distrotion_coeffs` are set to zero.

```
cam0:
  cam_overlaps:
  - 1
  camera_model: pinhole
  distortion_coeffs:
  - 0
  - 0
  - 0
  - 0
  distortion_model: radtan
  intrinsics:
  - 836.39874268
  - 836.66235352
  - 636.00115967
  - 357.10168457
  resolution:
  - 1280
  - 720
  rostopic: /oakd/stereo_ir/left/image_rect
cam1:
  T_cn_cnm1:
  - -  9.99938488e-01
    -  7.27824634e-03
    - -8.37180577e-03
    - -7.47539711e-02
  - - -7.23146135e-03
    -  9.99958158e-01 
    -  5.60518634e-03
    -  2.47205094e-03
  - -  8.41225125e-03
    - -5.54430112e-03
    -  9.99949217e-01
    -  4.30144593e-04
  - - 0.0
    - 0.0
    - 0.0
    - 1.0
  cam_overlaps:
  - 0
  camera_model: pinhole
  distortion_coeffs:
  - 0
  - 0
  - 0
  - 0
  distortion_model: radtan
  intrinsics:
  - 843.39074707
  - 843.87249756
  - 635.90380859
  - 359.52798462
  resolution:
  - 1280
  - 720
  rostopic: /oakd/stereo_ir/right/image_rect
```

### Recording Camera-IMU joint data

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

Record data as `oak-d-stereo.bag`. See [the official video](https://www.youtube.com/watch?app=desktop&v=puNXsnrYWTY) for recommended recording movements.


```
rosbag record -O ./oak-d-stereo.bag /oakd/stereo_ir/left/image_rect /oakd/stereo_ir/right/image_rect /oakd/imu
```

### Run calibration 

```
rosrun kalibr kalibr_calibrate_imu_camera --target april.yaml --bag oak-d-stereo.bag --cam factory-camchain-oak-d-stereo.yaml --imu imu.yaml
```

After calibration, you will get output as `camchain-imucam-oak-d-stereo.yaml`. If your calibration is good enough, the reprojection error that will be displayed at the end of the calibration will be within about 20, but this is just a rough indication　to reject a bad calibration.
The output will be like the following:

```
cam0:
  T_cam_imu:
  - - -0.004297644870309092
    - -0.9999379633526877
    - 0.010276171205606692
    - 0.057355539285231474
  - - 0.9999890070827118
    - -0.004278141289016657
    - 0.0019191719158318053
    - 0.014038544265156515
  - - -0.001875089944512822
    - 0.010284306159845909
    - 0.9999453570493289
    - -0.019419965178863116
  - - 0.0
    - 0.0
    - 0.0
    - 1.0
  cam_overlaps:
  - 1
  camera_model: pinhole
  distortion_coeffs:
  - 0
  - 0
  - 0
  - 0
  distortion_model: radtan
  intrinsics:
  - 836.39874268
  - 836.66235352
  - 636.00115967
  - 357.10168457
  resolution:
  - 1280
  - 720
  rostopic: /oakd/stereo_ir/left/image_rect
  timeshift_cam_imu: -0.005743440329743733
cam1:
  T_cam_imu:
  - - 0.0029964838942637684
    - -0.9999936708554896
    - 0.0019181588134633415
    - -0.017137204848541913
  - - 0.9999677186176349
    - 0.0030106958496116665
    - 0.007449659934745395
    - 0.01598639071112978
  - - -0.007455387777549706
    - 0.0018957741066330407
    - 0.9999704111790622
    - -0.018584179655013494
  - - 0.0
    - 0.0
    - 0.0
    - 1.0
  T_cn_cnm1:
  - - 0.9999384691844168
    - 0.007278246450583977
    - -0.0083718057552318
    - -0.0747539711
  - - -0.007231461239416023
    - 0.9999581441300118
    - 0.005605186373246936
    - 0.00247205094
  - - 0.008412251264768203
    - -0.005544301086753065
    - 0.9999492471680708
    - 0.000430144593
  - - 0.0
    - 0.0
    - 0.0
    - 1.0
  cam_overlaps:
  - 0
  camera_model: pinhole
  distortion_coeffs:
  - 0
  - 0
  - 0
  - 0
  distortion_model: radtan
  intrinsics:
  - 843.39074707
  - 843.87249756
  - 635.90380859
  - 359.52798462
  resolution:
  - 1280
  - 720
  rostopic: /oakd/stereo_ir/right/image_rect
  timeshift_cam_imu: -0.007189075332096728
```

What we need is 'T_cam_imu'.

```
  T_cn_cnm1:
  - - 0.9999384691844168
    - 0.007278246450583977
    - -0.0083718057552318
    - -0.0747539711
  - - -0.007231461239416023
    - 0.9999581441300118
    - 0.005605186373246936
    - 0.00247205094
  - - 0.008412251264768203
    - -0.005544301086753065
    - 0.9999492471680708
    - 0.000430144593
  - - 0.0
    - 0.0
    - 0.0
    - 1.0
```

### Write configuration file for Stero_Inertial mode

I assume the configuration file is saved as 'oak-d-param.yaml' The following is my configuration file. As you can see, the only difference from the one for Stereo mode is the addition of `Tbc` and `IMU` sections. 
The transformation part of 'Tbc' is from the output of kalibr. Because the rotation part seems to not match with ORB_SLAM3's axis, I used a fixed value for the rotation part. 
The values for 'IMU' section is the one from 'imu.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
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

Stereo.T_c1_c2: !!opencv-matrix
  rows: 4
  cols: 4
  dt: f
  data: [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 from camera 0 to body-frame (imu)
Tbc: !!opencv-matrix
   rows: 4
   cols: 4
   dt: f
   data: [0, 1, 0, 0.01744779136481356,
        -1, 0, 0, 0.012407593809577674,
        0, 0, 1, -0.026713744779204896,
        0.,        0.,        0.,        1.      ]


# IMU noise
IMU.NoiseGyro: 6.4190421491705540e-04
IMU.NoiseAcc:  2.8138017235431984e-02
IMU.GyroWalk: 2.3553045282732546e-06
IMU.AccWalk:  7.1952095806215616e-04
IMU.Frequency: 400


#--------------------------------------------------------------------------------------------
# Stereo Rectification. Only if you need to pre-rectify the images.
#
# NOT NEEDED ON OAK-D. We extract rectified images directly from the OAK-D
# itself, so no need for these.
#
# Camera.fx, .fy, etc must be the same as in LEFT.P
#--------------------------------------------------------------------------------------------

#--------------------------------------------------------------------------------------------
# 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.0
Viewer.GraphLineWidth: 0.9
Viewer.PointSize: 2.0
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3.0
Viewer.ViewpointX: 0.0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500.0
Viewer.imageViewScale: 1.0
```

## Run Stero_Inertial mode

Finally, we can run it!

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

```
rosrun ORB_SLAM3 Stereo_Inertial 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 /imu:=/stereo_inertial_publisher/imu
```