Question regarding frames - quick VIO divergence

[RotorBoy2020]

Hello,

I am getting ready to calibrate my D435i stereo camera and I had a question: what frames are used for this VIO? I used the default D435i frames and my result diverged to 3000 m within a minute. Either I'm cruising or I misconfigured something!

When holding the D435i camera in front of you, the +Z direction is out from the camera (away from you), and +Y is pointing down towards the ground, and +X is pointing to the right. The IMU on the D435i shares these same frames. Both frames are shown here.

I ran the calibration on kalibr and received the following yaml file for the stereo camera and IMU using the frames above:

cam0:
  T_cam_imu:
    [0.9998199170516484, 0.01643527217215919, 0.009487639087880625, 8.613401914494543e-05,
    -0.01616638966451235, 0.9994840651453857, -0.027753402776407157, -0.00010688708046965506,
    -0.009938878812520997, 0.027595023991317455, 0.9995697741222813, 1.2810958498957763e-05,
    0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [0.3312112694754906, 0.2301353059085136, -0.26520851036543713, 0.3157811122644503]
  distortion_model: equidistant
  intrinsics: [382.49905884077964, 381.6457570726868, 322.48867508369136, 242.55604218005752]
  resolution: [640, 480]
  rostopic: /realsense/cam0_image
cam1:
  T_cam_imu:
    [0.9998058089699385, 0.016526150605438206, 0.010734556168454843, -0.048245448646605266,
    -0.016226807860865586, 0.9994928721538184, -0.027398708370246112, -6.396721266264149e-05,
    -0.011181907557026586, 0.027219200206428144, 0.9995669462739897, -4.1064145030144116e-05,
    0.0, 0.0, 0.0, 1.0]
  T_cn_cnm1:
    [0.9999992183699322, 5.645343070813724e-05, 0.0012490286356398044, -0.048331592565536895,
    -5.689735797354274e-05, 0.999999935231977, 0.00035538532618945056, 4.2920208855604844e-05,
    -0.0012490084920218037, -0.00035545611483899555, 0.9999991568140121, -5.380550427206741e-05,
    0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [0.3466355723934432, 0.05010249547871596, 0.35580888463187466, -0.3349500418257732]
  distortion_model: equidistant
  intrinsics: [382.45925212800483, 381.70131579955574, 322.13326249159, 242.38976085988756]
  resolution: [640, 480]
  rostopic: /realsense/cam1_image
  timeshift_cam_imu: 0.0
T_imu_body:
  [1.0000, 0.0000, 0.0000, 0.0000,
  0.0000, 1.0000, 0.0000, 0.0000,
  0.0000, 0.0000, 1.0000, 0.0000,
  0.0000, 0.0000, 0.0000, 1.0000]

However, when I used this config with msckf_vio - it quickly diverged when it was stationary. I thought one possible reason is because maybe the frames are misaligned between the D435i and msckf_vio, possibly? I can flip the IMU frame on the D435i if needed, but to what?

Any help on the problem would be greatly appreciated! Thank you

[ke-sun]

Did you happen to take a look at the output at terminal? It should report the number of features detected, matched, and removed, which should serve as a good hint whether the calibration is sufficiently accurate.

[RotorBoy2020]

Thank you for taking the time to reply to my question, @ke-sun, much appreciated!

I ran my roslaunch file again today and I get the following outputs:

[ INFO] [1611849126.366688754]:  candidates: 61; track: 61; match: 61; ransac: 61/61=1.000000
[ INFO] [1611849126.868917797]:  candidates: 61; track: 61; match: 61; ransac: 61/61=1.000000
[ INFO] [1611849127.377193731]:  candidates: 61; track: 61; match: 61; ransac: 61/61=1.000000
[ INFO] [1611849127.570016453]: Total processing time 0.052120/88...

I'm not sure what I changed (or a good night of sleep helped) but the msckf_vio algorithm seems to be working correctly now. When I move the camera around, I get reasonable changes in position on the /vio/odom topic. I had to turn the noise parameters up a little bit, but things seem to be working now

However, my question regarding frames is still valid, as it seems that when I move the camera around the changes in XYZ do not correlate with the D435i's IMU frame. What frames does the algorithm use (something like this?), and do I need to change my IMU directions in order to accommodate this?

Thank you!

[RotorBoy2020]

I noticed that when I translate the camera in place, it gives reasonable changes in position, however the XYZ are inconsistent. For example, moving the camera forward results in a +Y change

New discovery: When I rotate the camera in place, the VIO position diverges to incorrect values quickly (ie > 1 m error after only a few rotations).

[ke-sun]

I assume you were using rviz for visualization. The moving frame in the rviz is actually the IMU frame of the senor. Depending on the relative rotation between the IMU and CAM frames, it could happen that "moving the camera forward results in a +Y change".

However, this contradicts with the IMU and CAM frames in the calibration file, where they are perfectly aligned. There might be some inaccurate calibration. The issue can also lead to the result "When I rotate the camera in place, the VIO position diverges to incorrect values quickly".

Another point to keep in mind is to let the sensor rest for a few seconds once the software starts. The software takes 200 IMU messages for initialization.

[jingshaojing]

how can i use D435i with this project , I guess I need to write ".launch" file to compile with this project?

[jingshaojing]

I have the same question for D435i, my calibrayion file is similar as above, but the result is not good. The pictures will be shown the path of euroc bag (left)and D435i bag(right) ,the latter is recorded by D435i with same frequency. The first question is the path( this part code is added by subscribe vio/odom）seems not so good as euorc like. I guess the purple circle represents the stablitity? I want to know the pirple circle means in picture. And it seems the params is not calibrated correctly? The second question is that I want to show all path ,but as follows, it is always a period will be appear, I want the path in all process. Can I trouble you to solve these two @ke-sun @RotorBoy2020

[jingshaojing]

I want to ask for the D435i cordinate, as your link explains, the IMU frame is aligned with the depth, and the picture in the link of coordinate is depth frame, I think, however, it is not consistent with the calibration file as you konwn, so is there some new process in this part?thanks. @RotorBoy2020

[ke-sun]

I may not have answers for all the questions. But the purple ellipsoid is the position covariance. If you would like to show the whole path, a simple solution is to increase the buffer size for the odom msg in RViz.

[jingshaojing]

I may not have answers for all the questions. But the purple ellipsoid is the position covariance. If you would like to show the whole path, a simple solution is to increase the buffer size for the odom msg in RViz.

@ke-sun ，thanks for your reply!