search

rpng / open_vins

An open source platform for visual-inertial navigation research.
https://docs.openvins.com
GNU General Public License v3.0
2.06k stars 616 forks source link

Drifting odometry on physical drone with kakute h7 v2 (BMI270 IMU) and arducam imx 219 #315

Closed flaviopinzarrone closed 1 year ago

flaviopinzarrone commented 1 year ago

Hello, first of all thanks for your amazing work and the effort you put in maintaining it! I am trying to run openVINS on a real drone with a kakute h7 v2 flight controller (containing a BMI270 IMU, running at approximately 900 Hz) and an arducam imx 219 camera, running at 30 fps and 640x480 resolution.

I performed the calibration following your tutorial using kalibr and these are the resulting files.

%YAML:1.0

imu0:
  T_i_b:
    - [1.0, 0.0, 0.0, 0.0]
    - [0.0, 1.0, 0.0, 0.0]
    - [0.0, 0.0, 1.0, 0.0]
    - [0.0, 0.0, 0.0, 1.0]
  accelerometer_noise_density: 0.1
  accelerometer_random_walk: 0.01
  gyroscope_noise_density: 0.001
  gyroscope_random_walk: 0.0001
  model: calibrated
  rostopic: /sensors/imu
  time_offset: 0.0
  update_rate: 900.0
%YAML:1.0

cam0:
  T_cam_imu:
    - [0.05451904521553491, -0.99831541208361, 0.019849728086765273, 0.0018482671070304117]
    - [0.6290914954515545, 0.018904062509603692, -0.7771013619671243, 0.007770338994264448]
    - [0.7754170259023934, 0.05485411941654933, 0.6290623669587314, 0.03486991192516528]
    - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: []
  camera_model: pinhole
  distortion_coeffs: [0.046157353433677537, -0.042233733507246775, 0.05245768451793893, -0.022187016630998244]
  distortion_model: equidistant
  intrinsics: [277.0839475135466, 368.83353527406734, 309.7544238276891, 228.20693144082884]
  resolution: [640, 480]
  rostopic: /camera/image_raw
  timeshift_cam_imu: -0.03522099226646433

I also report here the estimator_config.yaml

%YAML:1.0 # need to specify the file type at the top!

verbosity: "DEBUG" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT

use_fej: true # if we should use first-estimate Jacobians (enable for good consistency)
use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it
use_rk4int: true # if rk4 integration should be used (overrides imu averaging)

use_stereo: false # if we have more than 1 camera, if we should try to track stereo constraints
max_cameras: 1

calib_cam_extrinsics: true
calib_cam_intrinsics: true
calib_cam_timeoffset: true

max_clones: 11
max_slam: 50
max_slam_in_update: 25
max_msckf_in_update: 40
dt_slam_delay: 2

gravity_mag: 9.8065 # kalibr calibration

feat_rep_msckf: "GLOBAL_3D"
feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH"
feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH"

# zero velocity update parameters we can use
# we support either IMU-based or disparity detection.
try_zupt: false
zupt_chi2_multipler: 0 # set to 0 for only disp-based
zupt_max_velocity: 0.5
zupt_noise_multiplier: 20
zupt_max_disparity: 0.5 # set to 0 for only imu-based
zupt_only_at_beginning: false

# ==================================================================
# ==================================================================

init_window_time: 1.0
init_imu_thresh: 0.30
init_max_disparity: 2.0
init_max_features: 75

init_dyn_use: false
init_dyn_mle_opt_calib: false
init_dyn_mle_max_iter: 50
init_dyn_mle_max_time: 0.05
init_dyn_mle_max_threads: 6
init_dyn_num_pose: 6
init_dyn_min_deg: 20.0

init_dyn_inflation_ori: 10
init_dyn_inflation_vel: 100
init_dyn_inflation_bg: 10
init_dyn_inflation_ba: 100
init_dyn_min_rec_cond: 1e-20

init_dyn_bias_g: [0.0, 0.0, 0.0]
init_dyn_bias_a: [0.0, 0.0, 0.0]

# ==================================================================
# ==================================================================

record_timing_information: false
record_timing_filepath: "/tmp/traj_timing.txt"

save_total_state: false
filepath_est: "/tmp/ov_estimate.txt"
filepath_std: "/tmp/ov_estimate_std.txt"
filepath_gt: "/tmp/ov_groundtruth.txt"

# ==================================================================
# ==================================================================

# our front-end feature tracking parameters
# we have a KLT and descriptor based (KLT is better implemented...)
use_klt: true
num_pts: 200
fast_threshold: 50
grid_x: 5
grid_y: 5
min_px_dist: 15
knn_ratio: 0.70
track_frequency: 31.0
downsample_cameras: false
num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE

# aruco tag tracker for the system
# DICT_6X6_1000 from https://chev.me/arucogen/
use_aruco: false
num_aruco: 1024
downsize_aruco: true

# ==================================================================
# ==================================================================

# camera noises and chi-squared threshold multipliers
up_msckf_sigma_px: 1.5
up_msckf_chi2_multipler: 1
up_slam_sigma_px: 1.5
up_slam_chi2_multipler: 1
up_aruco_sigma_px: 1
up_aruco_chi2_multipler: 1

# masks for our images
use_mask: false

# imu and camera spacial-temporal
# imu config should also have the correct noise values
relative_config_imu: "kalibr_imu_chain.yaml"
relative_config_imucam: "kalibr_imucam_chain.yaml"

I calibrated the init_window_time and the init_imu_thresh so that the static initialization works fine. Indeed when I manually lift the drone it triggers the initialization and the state estimation begins. The problem is that it instantly starts drifting away even if I put it back down on a flat surface.

Here you find an output example log.txt

goldbattle commented 1 year ago

To handle the stationary case, you need to enable the zero velocity update (e.g. try_zupt). But it shouldn't fly away as soon as you pick it up. Looking at your IMU noise, it seems to be very large and seems a bit suspicious. Have you calibrated these noises? I recommend the following tool: https://github.com/ori-drs/allan_variance_ros

EDIT: you can also try directly using the EuRoC mav IMU noises to see how it performs then. Otherwise it might be your camera-IMU calibration. Can you post your Kalibr resulting PDF file?

flaviopinzarrone commented 1 year ago

Sure, here you find the calibration report. In the meantime I'm looking into your suggestions, thanks a lot!

goldbattle commented 1 year ago

It looks like the angular velocity has very large errors. image

Also with this reprojection errors, I think there is no hope to run VINS with this calibration. I recommend re-calibrating with Kalibr. image

flaviopinzarrone commented 1 year ago

Thanks, I’ll try recalibrating as you suggest. About calibration, I followed your guide and I have one question. You suggest to record the camera-imu sequence at 20-30 Hz for the camera and 200-500 Hz for the IMU, is this the case even if at runtime the desired frequencies are higher? Or should the calibration and runtime frequencies match?

goldbattle commented 1 year ago

I am not sure if there will be an accuracy difference, but the higher frequency will cause Kalibr to take longer. You can try both and see if the results vary.

On Mon, Mar 13, 2023 at 12:50 AM Flavio Pinzarrone @.***> wrote:

Thanks, I’ll try recalibrating as you suggest. About calibration, I followed your guide and I have one question. You suggest to record the camera-imu sequence at 20-30 Hz for the camera and 200-500 Hz for the IMU, is this the case even if at runtime the desired frequencies are higher? Or should the calibration and runtime frequencies match?

— Reply to this email directly, view it on GitHub https://github.com/rpng/open_vins/issues/315#issuecomment-1465505842, or unsubscribe https://github.com/notifications/unsubscribe-auth/AAQ6TYURXSSKS62PNVVLXNLW32RS7ANCNFSM6AAAAAAVTY7EJ4 . You are receiving this because you commented.Message ID: @.***>

flaviopinzarrone commented 1 year ago

Hi, I recalibrated the system using the maximum frequencies, the --scale-misalignment as the imu model and --perform-synchronization flag and these are the results of the calibration: imu_cam_calib-report-imucam.pdf The odometry still drifts and the linear velocities estimated on the odomimu topic grow very high (in the order of hundreds of m/s) after 20/30 seconds of execution. Output: output.txt

I noticed from the output high values of ms between each update with respect to supported dataset benchmarks, could this be the problem (poor synchronization)?

goldbattle commented 1 year ago

The image reprojection in that calibration still is very bad (3-4 pixels is very hard to use). You can also see that there might be something wrong with the IMU timestamps. If you have a 900Hz IMU, then you should get a "flat line" at 1.1ms, but you have a very very large range there. image

Your IMU might be "batching" the IMU readings and thus be providing bad timestamps. Sometimes sequential IMU readings have a 10ms difference which equates to a 100Hz IMU, while it seems that other times it is even faster then 1.1ms. You might also try a lower IMU rate to see if this fixes the problem here.

flaviopinzarrone commented 1 year ago

I performed the calibration again and the results of the camera calibration seem quite good. You can find them here.

I then tried to fix the problem of the IMU sampling variance reducing its rate to 500 Hz and I recalibrated them. Here you can see the results.

The mean reprojection error is low but in the last graph seems quite large. The odometry is still drifting as soon I lift the drone and I'm struggling to understand if it is a problem of synchronization or still a poor calibration.

goldbattle commented 1 year ago

The time dt of the IMU seem more reasonable now. But the pattern on the reprojection error hits that there is still an issue. Are you able to perform calibration with an april-tag grid? Maybe there is issues with the checkerboard flipping. Sorry that I can't help that much more on this.

image

goldbattle commented 1 year ago

Feel free to reopen once you are able to re-calibrate things a bit better with your hardware. Good luck!