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 :
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.
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
@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
@FaboNo Hi FaboNo. I am so sorry for late replying. I did not recognize I got a new issue.
First of all, I recommend you to calibration your camera model with pinhold-radtan
, rather than equidistant
, which you used.
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)
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).
negative timeshift_cam_imu
is normal value for d435i, which means camera is later than IMU.
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
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)
I recommend you to run IMU calibration
from D435i SDK as I explained here
@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.
@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
@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.
@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.
@FaboNo Hi.
@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
@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.
Most helpful comment
@FaboNo Hi.