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

Never Initialising #313

Closed sacchinbhg closed 1 year ago

sacchinbhg commented 1 year ago

When I run openVINS with a custom dataset using OAK-D Lite camera with a external BNO055 IMU I am never able to initialise and hence it never gives odometry values.

Here is what I keep receiving:

image

These are my yaml files:


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

use_fej: true # if first-estimate Jacobians should be used (enable for good consistency)
use_imuavg: true # if using 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: true # if we have more than 1 camera, if we should try to track stereo constraints between pairs
max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking)

calib_cam_extrinsics: false # if the transform between camera and IMU should be optimized R_ItoC, p_CinI
calib_cam_intrinsics: false # if camera intrinsics should be optimized (focal, center, distortion)
calib_cam_timeoffset: false # if timeoffset between camera and IMU should be optimized

max_clones: 11 # how many clones in the sliding window
max_slam: 50 # number of features in our state vector
max_slam_in_update: 25 # update can be split into sequential updates of batches, how many in a batch
max_msckf_in_update: 40 # how many MSCKF features to use in the update
dt_slam_delay: 1 # delay before initializing (helps with stability from bad initialization...)

gravity_mag: 9.81 # magnitude of gravity in this location

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.1
zupt_noise_multiplier: 10
zupt_max_disparity: 0.5 # set to 0 for only imu-based
zupt_only_at_beginning: false

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

init_window_time: 2.0 # how many seconds to collect initialization information
init_imu_thresh: 1.5 # threshold for variance of the accelerometer to detect a "jerk" in motion
init_max_disparity: 10.0 # max disparity to consider the platform stationary (dependent on resolution)
init_max_features: 75 # how many features to track during initialization (saves on computation)

init_dyn_use: false # if dynamic initialization should be used
init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended)
init_dyn_mle_max_iter: 50 # how many iterations the MLE refinement should use (zero to skip the MLE)
init_dyn_mle_max_time: 0.05 # how many seconds the MLE should be completed in
init_dyn_mle_max_threads: 6 # how many threads the MLE should use
init_dyn_num_pose: 6 # number of poses to use within our window time (evenly spaced)
init_dyn_min_deg: 10.0 # orientation change needed to try to init

init_dyn_inflation_ori: 10 # what to inflate the recovered q_GtoI covariance by
init_dyn_inflation_vel: 100 # what to inflate the recovered v_IinG covariance by
init_dyn_inflation_bg: 10 # what to inflate the recovered bias_g covariance by
init_dyn_inflation_ba: 100 # what to inflate the recovered bias_a covariance by
init_dyn_min_rec_cond: 1e-12 # reciprocal condition number thresh for info inversion

init_dyn_bias_g: [ 0.0, 0.0, 0.0 ] # initial gyroscope bias guess
init_dyn_bias_a: [ 0.0, 0.0, 0.0 ] # initial accelerometer bias guess

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

record_timing_information: false # if we want to record timing information of the method
record_timing_filepath: "/tmp/traj_timing.txt" # https://docs.openvins.com/eval-timing.html#eval-ov-timing-flame

# if we want to save the simulation state and its diagional covariance
# use this with rosrun ov_eval error_simulation
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 # if true we will use KLT, otherwise use a ORB descriptor + robust matching
num_pts: 200 # number of points (per camera) we will extract and try to track
fast_threshold: 20 # threshold for fast extraction (warning: lower threshs can be expensive)
grid_x: 5 # extraction sub-grid count for horizontal direction (uniform tracking)
grid_y: 5 # extraction sub-grid count for vertical direction (uniform tracking)
min_px_dist: 10 # distance between features (features near each other provide less information)
knn_ratio: 0.70 # descriptor knn threshold for the top two descriptor matches
track_frequency: 21.0 # frequency we will perform feature tracking at (in frames per second / hertz)
downsample_cameras: false # will downsample image in half if true
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
up_msckf_chi2_multipler: 1
up_slam_sigma_px: 1
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"

relative_config_imu: "oakd_imu_chain.yaml"
relative_config_imucam: "oakd_imucam_chain.yaml"

IMU yaml file


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.0023
  accelerometer_random_walk: 7.1174e-05
  gyroscope_noise_density: 0.0002
  gyroscope_random_walk: 8.9858e-07
  model: calibrated
  rostopic: /imu/data
  time_offset: 0.0
  update_rate: 50.0

cam-IMU chain yaml file


cam0:
  T_imu_cam:
    - [-0.9970716643242389, 0.013025088308218503, -0.0753554462282239, 0.003055804847922268]
    - [0.07646348257562346, 0.18522322355282475, -0.9797171496350909, -0.00032903354502456406]
    - [0.0011966762715800092, -0.9826101488033099, -0.18567677139221703, -0.0027379068541272143]
    - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [0.2980222767114992, 0.5282502497507331, -1.1886119209736297, 1.3293455448793905]
  distortion_model: equidistant
  intrinsics: [461.1316186581853, 455.69760739673495, 314.6762328533272, 262.44484944397766]
  resolution: [640, 480]
  rostopic: /cam0/image_raw
  timeshift_cam_imu: -0.43357782404104284
cam1:
  T_imu_cam:
    - [-0.9972140318375561, 0.007711737652202295, -0.0741936911648817, -0.31511978961949566]
    - [0.07437895261370472, 0.17816520680020032, -0.9811854720153222, -0.003223253456911574]
    - [0.005652089381326893, -0.9839703695682233, -0.17824243517580207, 0.02268432212565773]
    - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [0.3426083145251802, 0.15469528822193696, -0.01767950297364661, 0.06393128082662597]
  distortion_model: equidistant
  intrinsics: [449.0117191202954, 448.241171729035, 314.55665353989355, 259.39888167346913]
  resolution: [640, 480]
  rostopic: /cam1/image_raw

Is the problem the rate at which the camera images are coming? Or is it something to do with synchronisation?

EDIT: I think the problem is in how behind my data is. It shows that I am constantly behind by 50ms. Is there a way to reduce that? Should I be doing synchronisation?

sacchinbhg commented 1 year ago

@goldbattle I tried software synchronisation that ROS Gives and even with that I get 50ms time delay. Is that the reason why this seems to not be working?

goldbattle commented 1 year ago

From the looks of it, you have having very large disparity. If the camera is stationary, then the disparity should be very small (not the 50-70 pixels your screenshot shows). Likely camera intrinsic are messed up, or there is something wrong with the tracking.

On Mon, Mar 6, 2023 at 7:45 AM Sacchin Sundar @.***> wrote:

@goldbattle https://github.com/goldbattle I tried software synchronisation that ROS Gives and even with that I get 50ms time delay. Is that the reason why this seems to not be working?

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

sacchinbhg commented 1 year ago

From the looks of it, you have having very large disparity. If the camera is stationary, then the disparity should be very small (not the 50-70 pixels your screenshot shows). Likely camera intrinsic are messed up, or there is something wrong with the tracking. … On Mon, Mar 6, 2023 at 7:45 AM Sacchin Sundar @.> wrote: @goldbattle https://github.com/goldbattle I tried software synchronisation that ROS Gives and even with that I get 50ms time delay. Is that the reason why this seems to not be working? — Reply to this email directly, view it on GitHub <#313 (comment)>, or unsubscribe https://github.com/notifications/unsubscribe-auth/AAQ6TYXIYBOIE4OI6LQB3VDW2XL63ANCNFSM6AAAAAAVQVS2C4 . You are receiving this because you were mentioned.Message ID: @.>

This was due to the dataset not being stationary. After making it stationary this is the disparity I get

github_issues

I dont think it is because of the high disparity that it never initialises. Should I make the rosbag in such a way that it initially is stationary and after some seconds starts moving? As of now I have a 10 min stationary camera + IMU dataset

PS: Thank you for such a prompt reply, really helps out when you are trying out stuff in a time crunch!

tianlong312 commented 1 year ago

I encountered the same problem when using self-made Realsense T265 data. Wondering why it won't init successfully.

goldbattle commented 1 year ago

Enable a more verbosity and see what it is reporting. When initializing without dynamic initialization it relies on detecting a "jump" / "jerk" after it is picked up. This requires a threshold, which likely you need to tune to your specific IMU.

Take a look at the following if you are interested:

goldbattle commented 1 year ago

I have added some more clear messages here: https://github.com/rpng/open_vins/pull/318/commits/0145057f37dd54209906ff6ff985c971ac1bf516 Let me know if this provides insight / clarifies your problem.

sacchinbhg commented 1 year ago

Thank you so much, facing a bit of a hardware trouble right now. I will fix it and get back to you within the weekend.

Thank you for the prompt responses once again!!

sacchinbhg commented 1 year ago

Hi, so after a lot of tinkering along while using verbosity:-all I was finally able to initialise my setup. But now I get this error:

image

Does this mean I should run the Kallibr calibration again?

goldbattle commented 1 year ago

Do you have the bag that caused this? It seems maybe the messages are coming in out of order.

On Sun, Mar 19, 2023 at 8:08 AM Sacchin Sundar @.***> wrote:

Hi, so after a lot of tinkering along while using verbosity:-all I was finally able to initialise my setup. But now I get this error:

[image: image] https://user-images.githubusercontent.com/61612220/226174088-45a15ca7-deb6-4313-8bfb-06c01c5e6a20.png

Does this mean I should run the Kallibr calibration again?

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

sacchinbhg commented 1 year ago

Do you have the bag that caused this? It seems maybe the messages are coming in out of order. … On Sun, Mar 19, 2023 at 8:08 AM Sacchin Sundar @.> wrote: Hi, so after a lot of tinkering along while using verbosity:-all I was finally able to initialise my setup. But now I get this error: [image: image] https://user-images.githubusercontent.com/61612220/226174088-45a15ca7-deb6-4313-8bfb-06c01c5e6a20.png Does this mean I should run the Kallibr calibration again? — Reply to this email directly, view it on GitHub <#313 (comment)>, or unsubscribe https://github.com/notifications/unsubscribe-auth/AAQ6TYVI33ZDXTYWDIYXOLLW43ZMDANCNFSM6AAAAAAVQVS2C4 . You are receiving this because you were mentioned.Message ID: @.>

I do this is the bag.

Is this because of synchronisation issues maybe?

sacchinbhg commented 1 year ago

Thank you for all the help @goldbattle I was able to finally fix the issue after doing software synchronisation. For anyone using a Camera + Extrenal IMU, I suggest either do hardware or software synchronisation properly and then follow the Kalibr/Allan camchain calibration, OpenVINS etc. Otherwise you will keep facing many different issues.