Using oak-D pro W to do VIO with Vins-Fusion ROS pkg

YousefGamal

Hello everyone,
I manged to run and install and run Depth ai & Vins-Fusion to run on Oak-D pro W camera but when the code starts every thing seems to be fine but once I start moving the camera the Tf starts to get crazy and drift very far.
I need some help on what I need to configure or edit.

This is my config file:

%YAML:1.0

#common parameters

#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;

imu: 1

num_of_cam: 2

imu_topic: "/stereo_inertial_publisher/imu"

image0_topic: "/stereo_inertial_publisher/left/image_rect"

image1_topic: "/stereo_inertial_publisher/right/image_rect"

output_path: "/home/pi/Documents"

cam0_calib: "left.yaml"

cam1_calib: "right.yaml"

image_width: 1280

image_height: 800

# Extrinsic parameter between IMU and Camera.

estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.

body_T_cam0: !!opencv-matrix

rows: 4

cols: 4

dt: d

data: [ 0, 0, 1, 0,

-1, 0, 0, 0.0375,

0, -1, 0, 0,

0, 0, 0, 1 ]

body_T_cam1: !!opencv-matrix

rows: 4

cols: 4

dt: d

data: [ 0, 0, 1, 0,

-1, 0, 0, -0.0375,

0, -1, 0, 0,

0, 0, 0, 1 ]

#Multiple thread support

multiple_thread: 0

#feature traker paprameters

max_cnt: 150 # max feature number in feature tracking

min_dist: 30 # min distance between two features

freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image

F_threshold: 1.0 # ransac threshold (pixel)

show_track: 1 # publish tracking image as topic

flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy

#optimization parameters

max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time

max_num_iterations: 8 # max solver itrations, to guarantee real time

keyframe_parallax: 10.0 # keyframe selection threshold (pixel)

#imu parameters The more accurate parameters you provide, the better performance

acc_n: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04

gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 0.004

acc_w: 0.001 # accelerometer bias random work noise standard deviation. #0.002

gyr_w: 0.0001 # gyroscope bias random work noise standard deviation. #4.0e-5

g_norm: 9.805 # gravity magnitude

#unsynchronization parameters

estimate_td: 1 # online estimate time offset between camera and imu

td: 0.00 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

#loop closure parameters

load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'

pose_graph_save_path: "/home/pi/Documents" # save and load path

save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0

```

The Left eye config:

%YAML:1.0

---

model_type: PINHOLE

camera_name: camera

image_width: 1280

image_height: 800

distortion_parameters:

k1: 0.0

k2: 0.0

p1: 0.0

p2: 0.0

projection_parameters:

fx: 832.2688598632812

fy: 835.7731323242188

cx: 625.9611206054688

cy: 354.8282775878906

The Right eye config:

%YAML:1.0

---

model_type: PINHOLE

camera_name: camera

image_width: 1280

image_height: 800

distortion_parameters:

k1: 0.0

k2: 0.0

p1: 0.0

p2: 0.0

projection_parameters:

fx: 841.0867309570312

fy: 844.6397705078125

cx: 617.4251708984375

cy: 354.2491760253906

Thank you.

jakaskerl

Hi @YousefGamal
Sorry, can you send a demo of this happening so we get a better idea?

Thanks,
Jaka

YousefGamal

Ok sure,
here is how the behavior looks like if i just move the camera little bit ,

jakaskerl

cc @Luxonis-Adam

Luxonis-Adam

Hi, are the calibration values taken from CameraInfo messages? This behavior could be caused by for example bad baseline or instrinsics parameters.
For OAK-D-PRO W you could try following parameters for translation:
In body_T_cam0:
0.0, -0.06635, -0.00565

In body_T_cam1:
0.0, 0.00641, -0.00565

YousefGamal

Hi Mr.Adam,
thank you for your replay.
I changed the translation matrices but still drifting very far.

this is my new config file:

%YAML:1.0

#common parameters

imu: 1

num_of_cam: 2

imu_topic: "/stereo_inertial_publisher/imu"

image0_topic: "/stereo_inertial_publisher/left/image_rect"

image1_topic: "/stereo_inertial_publisher/right/image_rect"

output_path: "/home/pi/Documents"

cam0_calib: "left.yaml"

cam1_calib: "right.yaml"

image_width: 4056

image_height: 3040

# Extrinsic parameter between IMU and Camera.

estimate_extrinsic: 0 # If you have a good estimate of the extrinsic parameters, it's best to keep this disabled

body_T_cam0: !!opencv-matrix

rows: 4

cols: 4

dt: d

data: [ 0, 0, 1, 0.0,

-1, 0, 0, -0.06635,

0, -1, 0, -0.00565,

0, 0, 0, 1 ]

body_T_cam1: !!opencv-matrix

rows: 4

cols: 4

dt: d

data: [ 0, 0, 1, 0.0,

-1, 0, 0, 0.00641,

0, -1, 0, -0.00565,

0, 0, 0, 1 ]

#Multiple thread support

multiple_thread: 2 # Adjust based on your system's capabilities

#feature tracker parameters

max_cnt: 150 # Reduced further for stability, experiment with values between 100-200

min_dist: 30 # Increased for higher resolution and to avoid feature clustering

freq: 100 # Reduced to avoid overloading VINS, OAK-D Pro W can output up to 120FPS, but processing all frames might be too much

F_threshold: 1.0

show_track: 1

flow_back: 1

#optimization parameters

max_solver_time: 0.05 # Further increased for better convergence

max_num_iterations: 8 # Further increased for better convergence

keyframe_parallax: 5.0 # Reduced to create keyframes more frequently, helping with drift

#imu parameters - THESE NEED TO BE ADJUSTED BASED ON ACTUAL SENSOR SPECS

acc_n: 0.1 # Consult the datasheet of the OV9782 for the noise values

gyr_n: 0.005 # Consult the datasheet of the OV9782 for the noise values

acc_w: 0.001 # Consult the datasheet of the OV9782 for the random walk values

gyr_w: 5.0e-06 # Consult the datasheet of the OV9782 for the random walk values

g_norm: 9.805

#unsynchronization parameters

estimate_td: 0 # Keep disabled unless you are certain there is a significant time offset

td: 0.0

#loop closure parameters

load_previous_pose_graph: 0

pose_graph_save_path: "/home/pi/Documents"

save_image: 0

YousefGamal

this is my left camera info

how can I change the config file:

header:

seq: 197

stamp:

secs: 1715737960

nsecs: 228837530

frame_id: "oak_left_camera_optical_frame"

height: 720

width: 1280

distortion_model: "rational_polynomial"

D: [0.3481566309928894, -0.036945320665836334, -0.00011779300984926522, 1.3023264955336344e-06, -0.0014405992114916444, 0.6816200017929077, -0.0038426206447184086, -0.009061750955879688]

K: [572.9392700195312, 0.0, 614.5557250976562, 0.0, 573.3082885742188, 366.3216857910156, 0.0, 0.0, 1.0]

R: [0.9998615384101868, 0.009426194243133068, -0.013713574036955833, -0.009395874105393887, 0.9999532699584961, 0.002273713471367955, 0.013734365813434124, -0.0021445476450026035, 0.9999033808708191]

P: [574.8320922851562, 0.0, 618.0574340820312, -43.09019101091398, 0.0, 574.68701171875, 333.6612854003906, 0.0, 0.0, 0.0, 1.0, 0.0]

binning_x: 0

binning_y: 0

roi:

x_offset: 0

y_offset: 0

height: 0

width: 0

do_rectify: False

---

and this this is my right camera info:

header:

seq: 22

stamp:

secs: 1715738046

nsecs:  21175646

frame_id: "oak_right_camera_optical_frame"

height: 720

width: 1280

distortion_model: "rational_polynomial"

D: [3.436379909515381, 0.9962846040725708, -4.901837655779673e-06, 0.0001083303868654184, 0.017696253955364227, 3.792675018310547, 2.0209763050079346, 0.1410459578037262]

K: [574.8320922851562, 0.0, 618.0574340820312, 0.0, 574.68701171875, 333.6612854003906, 0.0, 0.0, 1.0]

R: [0.9998916387557983, 0.013972730375826359, -0.004630759358406067, -0.013982926495373249, 0.9998998641967773, -0.0021768894512206316, 0.004599878564476967, 0.0022414049599319696, 0.9999868869781494]

P: [574.8320922851562, 0.0, 618.0574340820312, 0.0, 0.0, 574.68701171875, 333.6612854003906, 0.0, 0.0, 0.0, 1.0, 0.0]

binning_x: 0

binning_y: 0

roi:

x_offset: 0

y_offset: 0

height: 0

width: 0

do_rectify: False

---

YousefGamal

when I changed the left/right files based on the msg data and based on what i understood i get this:

Residuals: -nan -nan

Parameter Block 0, size: 7

-0.266094 | -493.652 1064.36

0\.444213 |      885.918     -52.3169 

 3\.44292 |      54.2393      873.857 

0\.999594 |     -58.3219      236.449

-0.00155955 | 32.5227 198.563

-0.00504969 | -400.446 281.625

-0.0279982 | 0 0

Parameter Block 1, size: 7

-0.304939 | 493.652 -1064.36

0\.497097 |     -885.918      52.3169 

 3\.91966 |     -54.2393     -873.857 

0\.999599 |     -360.447     -166.273

-0.00157635 | 202.107 -736.208

-0.00497586 | 399.898 -198.147

-0.0278192 | 0 0

Parameter Block 2, size: 7

       0 | Not Computed  Not Computed  

-0.06635 | Not Computed  Not Computed  

-0.00565 | Not Computed  Not Computed  

     0\.5 | Not Computed  Not Computed  

    -0.5 | Not Computed  Not Computed  

     0\.5 | Not Computed  Not Computed  

    -0.5 | Not Computed  Not Computed

Parameter Block 3, size: 7

       0 | Not Computed  Not Computed  

 0\.00641 | Not Computed  Not Computed  

-0.00565 | Not Computed  Not Computed  

     0\.5 | Not Computed  Not Computed  

    -0.5 | Not Computed  Not Computed  

     0\.5 | Not Computed  Not Computed  

    -0.5 | Not Computed  Not Computed

Parameter Block 4, size: 1

 3\.30692 |     -8.29503     -112.634

Parameter Block 5, size: 1

       0 | Not Computed  Not Computed

jakaskerl

@Luxonis-Adam for ping

YousefGamal

While I was doing a research I found this
https://www.cnblogs.com/gary-guo/p/16567435.html

it seems that it is a common issue with model W (Wide cameras ) it was not only my camera that is facing this issue.

I hope Mr.@Luxonis-Adam can provide me with a proper configuration files that is suitable for W camera models.

Luxonis-Adam

Hi,
I would suggest two things as follow-up:

Instead of using stereo_inertial_node I would opt for depthai_ros_driver as it is more up-to-date and contains more parameters for customization.
I noticed that you are using pinhole camera model, which generally works only for small FOV cameras. For wide sensors I would opt to use Kannala-Brandt (Equidistant fish-eye) model and pass unrectified images to the node. Seems like parameter naming is a bit different, but it should work with that.
Also, in your new config file I see that image size is set to 4K, I'm not sure where it is used but it might also cause some issues.

magikEye

once I start moving the camera the Tf starts to get crazy and drift very far.

We observed similar behavior. Possibly related: IMU axes different from /tf_static?