some inputs regarding D435i + vins_fusion

[FaboNo]

As you are one of the very few who were able to run VINS-FUSION on a VIO mode, I come back to you to clarify some points and ask for your feedback.

I am trying to run VINS FUSION with a D435i (like many people I guess). Using the available launch realsense_stereo_imu_config.yaml I experienced what may people reported, as soon as you touch the camera, the computed pose got crazy.

So I used Kalibr :

• to calibrate the cameras and I got the following result:

cam0:
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [0.3775752250096366, -0.08329757139021714, 0.6504570262599587, -0.4530039925701512]
  distortion_model: equidistant
  intrinsics: [395.5922288679944, 395.5957501002353, 319.88738279106286, 239.07386401539998]
  resolution: [640, 480]
  rostopic: /camera/infra1/image_rect_raw
cam1:
  T_cn_cnm1:
  - [0.9999953397168069, 9.448668958037214e-05, 0.0030514942108417595, -0.04984248508758902]
  - [-9.749429725933605e-05, 0.9999995096576434, 0.0009854843073503579, 6.189814698173665e-05]
  - [-0.0030513995994150846, -0.0009857772179980872, 0.9999948585886561, 0.00015202509092000083]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [0.31220975539351176, 0.48299676137352554, -1.4010717249687126, 2.191483852686]
  distortion_model: equidistant
  intrinsics: [396.5827833428163, 396.75838144377565, 319.1950768721882, 238.53973946284336]
  resolution: [640, 480]
  rostopic: /camera/infra2/image_rect_raw

I think the calibration is ok because when I run VINS_FUSION with these parameters (imu = 0), I got a decent behavior. I will be more than happy to share the pdf report with you if necessary because I am not an expert in that field.

• Camera IMu calibration, was not so straighforward because I do not have the noise density and randm walk values. So I used the Allan variance software and some code from matlab and I got this for the acceleration
  

accelerometer_noise_density: 0.0011060    #Noise density (continuous-time)
accelerometer_random_walk:   8.6056e-05    #Bias random walk

but for the gyro I am clearly skeptical :

because the values are the following:

gyroscope_noise_density:     50.427    #Noise density (continuous-time)
gyroscope_random_walk:       0.3480    #Bias random walk

Here I would like to get your opinion on these values?

However I was able to to do an imu-camera calibration:

cam0:
  T_cam_imu:
  - [0.999848715163262, 0.0018345448477758066, 0.017296856118190283, -0.009035083963879727]
  - [-0.001867318311452875, 0.999996491602708, 0.0018788040335944412, 0.002129300542443833]
  - [-0.01729334868368733, -0.0019108185351930207, 0.9998486329759259, -0.005593943362795322]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [0.3775752250096366, -0.08329757139021714, 0.6504570262599587,
    -0.4530039925701512]
  distortion_model: equidistant
  intrinsics: [395.5922288679944, 395.5957501002353, 319.88738279106286, 239.07386401539998]
  resolution: [640, 480]
  rostopic: /camera/infra1/image_rect_raw
  timeshift_cam_imu: -0.028184396371617668
cam1:
  T_cam_imu:
  - [0.9997911085949877, 0.0019231918046627172, 0.020347985347160178, -0.058894395640646836]
  - [-0.0019818392674274344, 0.999993939322739, 0.0028624519050287085, 0.0021865657711008357]
  - [-0.02034235698054308, -0.002902180399802527, 0.9997888606407872, -0.005416418875488448]
  - [0.0, 0.0, 0.0, 1.0]
  T_cn_cnm1:
  - [0.9999953397168102, 9.448668958037212e-05, 0.003051494210841759, -0.04984248508758902]
  - [-9.749429725933604e-05, 0.9999995096576467, 0.0009854843073503574, 6.189814698173665e-05]
  - [-0.003051399599415084, -0.0009857772179980868, 0.9999948585886594, 0.00015202509092000083]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [0.31220975539351176, 0.48299676137352554, -1.4010717249687126,
    2.191483852686]
  distortion_model: equidistant
  intrinsics: [396.5827833428163, 396.75838144377565, 319.1950768721882, 238.53973946284336]
  resolution: [640, 480]
  rostopic: /camera/infra2/image_rect_raw
  timeshift_cam_imu: -0.028184788415138273

Here I am also surprise because the timeshift is NEGATIVE! Is it possible?
Regarding the camera transformation, from a thread related to VINS_FUSION, I understood that body_T_cam0/1 is the transform from camera frame to the IMU frame

p = body_T_cam0 * q
p: point in IMU coordinates
q: point in camera coordinates

and with Kalibr T_cam_imu is the IMU extrinsics: transformation from IMU to camera coordinates (T_c_i) but I am puzzled so far to get the right format used by VINS_FUSION.

However despite all this work, I am still not be able to run VINS with the IMU. Sorry for being so long but do you see something wrong here that may explain why it is not working?

Thanks a lot

[marufino]

@FaboNo I managed to get it working with the config below, didn't need to run my own calibrations and, i'm sure it could work better with a proper calibration, but it works well and doesn't diverge

I used the left.yaml and right.yaml provided in this repo

Make sure you're turning off autoexposure and turning off the IR emitter in the realsense options,

%YAML:1.0

#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam; 
imu: 1
num_of_cam: 2

imu_topic: "/camera/imu"
image0_topic: "/camera/infra1/image_rect_raw"
image1_topic: "/camera/infra2/image_rect_raw"
output_path: "/home/dji/output/"

cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640
image_height: 480
   

# 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: [  1,0,0,0.00552,
            0,1,0,-0.0051,
            0,0,1,-0.01174,
            0., 0., 0., 1. ]

body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 1,0,0,-0.04464144,
           0,1,0,-0.0051,
           0,0,1,-0.01174,
           0., 0., 0., 1. ]

#Multiple thread support
multiple_thread: 1

#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.072                             # 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/dji/output/pose_graph/" # save and load path
save_image: 0                   # save image in pose graph for visualization prupose; you can close this function by setting 0 

[engcang]

@FaboNo Hi FaboNo. I am so sorry for late replying. I did not recognize I got a new issue.

1. First of all, I recommend you to calibration your camera model with pinhold-radtan, rather than equidistant, which you used.

2. After right camera calibrations, please try to run VINS-Fusion with 2 camera and 0 imu mode.
   It should work well first (under easy motions, without fast motion or pure rotation)

3. Make sure that set your body_T_cam is inverse matrix of T_cam_imu from your Kalibr result.
   Also check your body_T_cam0 and 1 before running only 2 cameras mode (above).

4. negative timeshift_cam_imu is normal value for d435i, which means camera is later than IMU.

5. You do not have to run Kalibr Allan for your IMU calibraiton, which was not suitable for d435i for my experience,
   rather I found the IMU model of d435i and then just used that specific model's parameter as here

6. However, I recommend to use just default imu parameters as here
   They should be enough. (or, just slightly increase them, like 3 times of default values)

7. I recommend you to run IMU calibration from D435i SDK as I explained here

[engcang]

@FaboNo
By the way, I checked the default parameter/ yaml files from original authors for D435i work quite well before.
Even with all 0 distortion values and only focal lengths/principal points values and nominal body_T_cam values.

I think your equidistant camera intrinsic calibration results with huge distortion values are the main problem.

[FaboNo]

@engcang thank you for your message, actually the images are already rectified so run Kalibr on it is almost useless no?
As they are rectified, distortion must be equal to 0 as well.

@marufino thank you very much for your message, I will give it a try tomorrow and let you know

[engcang]

@FaboNo
Any news for this issue? Did you get it work?
I recently had a chance to test VINS-Fusion (stereo) with Intel D435i and here is the result clip, if you are interested in yet.

[FaboNo]

@engcang I am sorry to reply so late. I saw the video and it is impressive. So I have few questions for you, mainly because you used the PixHawk4.

• Does it means that the IMU accuracy is better than the D435i one?
• At which frequency are you reading the IMU data 100hz, 200hz ?
• Did you use PX4 and if so, did you use Mavros to read the data? Personnally I tried PixHawk2 and it was not convincing and I was not able to increase the IMU frequency more than 100Hz
  Very nice work!

[engcang]

@FaboNo Hi.

• Yes. I think Pixhawk4 mini has better IMU than D435i. Cause Pixhawk 4 mini uses two 6DOF IMU and one magnetometer and then filters them to output one ROS topic (not 100% sure for filtering, but they do have two IMU).
• I used 100Hz.
• Yes I used PX4 and MAVROS. I remember I could get around 150Hz as maximum, but from somewhere I saw that 3:1 ratio for IMU and camera FPS is optimal for VINS. We usually use 30Hz camera, so I thought 100Hz is proper.

[FaboNo]

@engcang thank you for your response. I am wondering did you change the imu parameters values in the realsense_stereo_imu_config.yaml or keep them unchanged?
I will try your setup anyway

[engcang]

@FaboNo
Hi. Usually, I do not change IMU parameters in yaml file. I also tried to set IMU parameters as I got from Kalibr_allan,
howevery I could not get better performance using them.

Only for crazy cases like quadruped robots contacting ground toughly, I try to set parameters 10 times bigger or less, and it works.

[mzahana]

@engcang I have a question on how to use the transformation matrices (imu-cam extrinsic) reported by Kalibr (e.g. T_cam_imu) to update the transformation matrices in vins yaml file (e.g. body_T_cam0 ). Can, for example, T_cam_imu of cam0 be directly used to fill the values of body_T_cam0, or it has to be transposed?
How did you handle this in your setup ?

Thanks.

[engcang]

@mzahana Hi. I usually use results-imu-cam-blahblah.txt file and use the T_ic value directly. That is cam0 to imu0 in the file. And yes, that is the transpose of the T_cam_imu in the camchain-imucam-blahblah.yaml file.

[mzahana]

@engcang Thank you!

[bhaskar-glitch]

@mzahana Hi. I usually use results-imu-cam-blahblah.txt file and use the T_ic value directly. That is cam0 to imu0 in the file. And yes, that is the transpose of the T_cam_imu in the camchain-imucam-blahblah.yaml file.

Hi @engcang,
as you said you directly used T_ic value of the txt file i.e also the cam0 to imu0
then how you can you say that its transpose of T_cam_imu because if we transpose the T_cam_imu the result is
Cam0
cam0 to imu0
[[0.9999030027522889 0.011827835688961745 -0.007354412952148072 0.0]
[-0.012013755968760182 0.9995956566559134 -0.025771939433346106 0.0]
[0.007046612979217716 0.025857793748653494 0.9996407953599998 0.0]
[-0.045385211124978744 -0.016261584254448104 -0.02162864296886392 1.0]]

Cam1
cam0 to imu0
[[0.9999169796521159 0.012045246767503607 -0.004576661851199417 0.0]
[-0.012154670671535089 0.9996213832574352 -0.024685099055070103 0.0]
[0.0042775909407989044 0.0247386775071353 0.999684800350061 0.0]
[0.004213622719277034 -0.016244560726680984 -0.021603954642788217 1.0]]

and the T_ic value is like this
Cam0
T_ic: (cam0 to imu0):
[[ 0.999903 0.01182784 -0.00735441 0.04541408]
[-0.01201376 0.99959566 -0.02577194 0.01515235]
[ 0.00704661 0.02585779 0.9996408 0.02236117]
[ 0. 0. 0. 1. ]]
Cam1
T_ic: (cam1 to imu0):
[[ 0.99991698 0.01204525 -0.00457666 -0.00411648]
[-0.01215467 0.99962138 -0.0246851 0.01575633]
[ 0.00427759 0.02473868 0.9996848 0.02198099]
[ 0. 0. 0. 1. ]]

Please don't confuse

[engcang]

@bhaskar-glitch

Check these out.

cam-chain.yaml - here
results.txt - here

T_cam_imu and T_ci are exactly same.
But we need to use T_ic in config.yaml file of VINS-Mono or VINS-Fusion.

That is it. You do not have to get confused any further and I did not confuse you. Just use T_ic for VINS.

---

Below is optional information you would refer.
Since transformation matrix is in the format of Special Euclidean Group, namely SE(3), transpose of this should be close with inverse of that (almost same, yeah, but not same). I just tried to explain easily that camchain.yaml files shows the imu to cam calibration result, but we rather have to use cam to imu, namely T_ic for VINS.
I did not expect that readers will calculate the inverse or transpose of matrices to use.
Please just use T_ic of Kalibr for VINS.