search

rpng / open_vins

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

Pose diverging on Raspberry Pi 4 using EUROC dataset #435

Open mzahana opened 8 months ago

mzahana commented 8 months ago

Hi @goldbattle

I am trying to run open_vins on a Raspberry Pi 4 with 4GB RAM. I installed ROS2 humble from source and it works fine. To compile open_vins. I first installed deps

sudo apt-get update && sudo apt-get install -y \
    libeigen3-dev \
    libboost-all-dev \
    libceres-dev \

Then I compiled the ros workspace using

MAKEFLAGS="-j1 -l1" colcon build --executor sequential   --cmake-args -DCMAKE_BUILD_TYPE=Release

The compilation went fine.

My OpenCV version is 4.6.0

I tries using the default euroc_mav config file, the ran the ros2 launch ov_msckf subscribe.launch.py config:=euroc_mav, but the pose diverges quickly. I played the V1_01_easy bag.

I updated the config file as follows

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

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

use_fej: true # if first-estimate Jacobians should be used (enable for good consistency)
integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used)
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: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI
calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion)
calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized
calib_imu_intrinsics: false # if imu intrinsics should be calibrated (rotation and skew-scale matrix)
calib_imu_g_sensitivity: false # if gyroscope gravity sensitivity (Tg) should be calibrated

max_clones: 11 # how many clones in the sliding window
max_slam: 25 # 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: 20 # 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: 50 # 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: 50 # 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: 1 # -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"

Then, I ran the node gain, but it still diverges as I see it in RViz. I don't run RViz on the Raspberry Pi, I run it on a separate laptop on the same network, and I only visualize the imu path topic.

In DEBUG mode, here is the node output just after I stopped node when the pose started to diverge.

[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.529,-0.638,0.375,0.415 | p_IinG = -1.102,-1.335,2.076 | dist = 3.46 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0038,0.0201,0.0721 | ba = -0.0938,-0.0201,0.1073
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = 0.00083
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 457.820,455.711,367.955,247.863 | -0.282,0.070,0.003,-0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.619,455.068,378.594,255.537 | -0.284,0.071,0.002,-0.004
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.011,0.013,0.699,0.715 | 0.009,-0.052,0.052
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = -0.005,0.017,0.699,0.715 | -0.088,-0.054,0.052
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0285 seconds total (35.1 hz, 2.42 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0124 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0005 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0000 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0002 seconds for SLAM update (3 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0148 seconds for SLAM delayed init (1 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0111 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.03896 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.532,-0.638,0.370,0.415 | p_IinG = -0.792,-1.758,2.118 | dist = 3.55 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0039,0.0204,0.0720 | ba = -0.0971,0.0150,0.0979
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = 0.00086
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 457.792,455.738,367.928,247.859 | -0.282,0.070,0.003,-0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.561,455.045,378.612,255.547 | -0.284,0.070,0.002,-0.004
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.011,0.013,0.699,0.715 | 0.007,-0.052,0.053
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = -0.005,0.017,0.699,0.715 | -0.090,-0.054,0.053
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0400 seconds total (25.0 hz, 2.42 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0126 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0005 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0000 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0004 seconds for SLAM update (3 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0029 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0101 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.02649 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.533,-0.638,0.369,0.416 | p_IinG = -0.808,-1.766,2.153 | dist = 3.63 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0039,0.0204,0.0720 | ba = -0.0951,0.0163,0.1015
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = 0.00087
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 457.795,455.727,367.925,247.872 | -0.282,0.070,0.003,-0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.560,455.036,378.608,255.556 | -0.284,0.071,0.002,-0.004
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.011,0.013,0.699,0.715 | 0.007,-0.052,0.053
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = -0.005,0.017,0.699,0.714 | -0.090,-0.054,0.053
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0274 seconds total (36.6 hz, 7.41 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0133 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0004 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0002 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0003 seconds for SLAM update (3 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0012 seconds for SLAM delayed init (2 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0111 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.02646 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.532,-0.640,0.369,0.414 | p_IinG = -0.816,-1.769,2.190 | dist = 3.71 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0039,0.0204,0.0720 | ba = -0.0929,0.0178,0.1056
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = 0.00088
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 457.802,455.719,367.922,247.878 | -0.282,0.070,0.003,-0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.561,455.024,378.606,255.565 | -0.284,0.071,0.002,-0.004
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.011,0.013,0.699,0.715 | 0.007,-0.052,0.052
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = -0.005,0.017,0.700,0.714 | -0.090,-0.054,0.052
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0273 seconds total (36.7 hz, 2.41 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0124 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0005 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0000 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0004 seconds for SLAM update (3 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0101 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.02342 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.531,-0.641,0.369,0.413 | p_IinG = -0.806,-1.754,2.226 | dist = 3.79 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0039,0.0205,0.0720 | ba = -0.0899,0.0197,0.1112
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = 0.00089
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 457.815,455.706,367.923,247.883 | -0.282,0.070,0.003,-0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.567,455.011,378.601,255.567 | -0.284,0.071,0.002,-0.004
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.011,0.013,0.699,0.715 | 0.007,-0.052,0.052
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = -0.005,0.017,0.700,0.714 | -0.090,-0.053,0.052
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0243 seconds total (41.2 hz, 2.41 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0132 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0005 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0000 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0004 seconds for SLAM update (3 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0100 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.02414 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.531,-0.642,0.369,0.411 | p_IinG = -0.772,-1.726,2.262 | dist = 3.87 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0039,0.0205,0.0721 | ba = -0.0864,0.0222,0.1183
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = 0.00089
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 457.843,455.690,367.931,247.881 | -0.282,0.070,0.003,-0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.583,454.992,378.598,255.560 | -0.284,0.071,0.002,-0.004
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.011,0.013,0.699,0.715 | 0.007,-0.052,0.052
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = -0.005,0.017,0.700,0.714 | -0.090,-0.053,0.052
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0253 seconds total (39.5 hz, 7.41 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0132 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0005 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0002 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0001 seconds for SLAM update (3 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0008 seconds for SLAM delayed init (2 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0110 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.02587 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.532,-0.642,0.369,0.412 | p_IinG = -0.829,-1.765,2.305 | dist = 3.95 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0039,0.0205,0.0721 | ba = -0.0864,0.0222,0.1183
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = 0.00089
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 457.843,455.690,367.932,247.879 | -0.282,0.070,0.003,-0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.583,454.992,378.598,255.562 | -0.284,0.071,0.002,-0.004
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.011,0.013,0.699,0.715 | 0.007,-0.052,0.052
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = -0.005,0.017,0.700,0.714 | -0.090,-0.053,0.052
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0266 seconds total (37.6 hz, 2.41 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0137 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0005 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0000 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0002 seconds for SLAM update (3 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0100 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.02447 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.532,-0.641,0.368,0.413 | p_IinG = -0.886,-1.806,2.345 | dist = 4.03 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0039,0.0205,0.0721 | ba = -0.0864,0.0222,0.1183
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = 0.00089
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 457.843,455.690,367.932,247.879 | -0.282,0.070,0.003,-0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.583,454.992,378.598,255.562 | -0.284,0.071,0.002,-0.004
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.011,0.013,0.699,0.715 | 0.007,-0.052,0.052
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = -0.005,0.017,0.700,0.714 | -0.090,-0.053,0.052
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0254 seconds total (39.3 hz, 9.91 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0133 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0005 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0001 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0002 seconds for SLAM update (2 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0106 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.02473 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.532,-0.641,0.368,0.413 | p_IinG = -0.943,-1.848,2.385 | dist = 4.11 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0039,0.0205,0.0721 | ba = -0.0864,0.0222,0.1183
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = 0.00089
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 457.843,455.690,367.932,247.879 | -0.282,0.070,0.003,-0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.583,454.992,378.598,255.562 | -0.284,0.071,0.002,-0.004
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.011,0.013,0.699,0.715 | 0.007,-0.052,0.052
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = -0.005,0.017,0.700,0.714 | -0.090,-0.053,0.052
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0258 seconds total (38.7 hz, 4.91 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0142 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0005 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0002 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0000 seconds for SLAM update (2 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0009 seconds for SLAM delayed init (2 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0099 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.02569 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.532,-0.640,0.367,0.415 | p_IinG = -1.001,-1.891,2.426 | dist = 4.20 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0039,0.0205,0.0721 | ba = -0.0864,0.0222,0.1183
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = 0.00089
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 457.842,455.690,367.932,247.877 | -0.282,0.070,0.003,-0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.584,454.992,378.598,255.564 | -0.284,0.071,0.002,-0.004
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.011,0.013,0.699,0.715 | 0.007,-0.052,0.052
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = -0.005,0.017,0.700,0.714 | -0.090,-0.053,0.052
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0265 seconds total (37.7 hz, 7.41 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0151 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0005 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0017 seconds for MSCKF update (1 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0004 seconds for SLAM update (2 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0113 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.02903 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.533,-0.639,0.365,0.417 | p_IinG = -0.812,-1.753,2.608 | dist = 4.28 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0038,0.0205,0.0721 | ba = -0.0906,0.0322,0.1398
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = 0.00078
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 457.969,455.704,368.041,247.874 | -0.282,0.070,0.003,-0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.671,454.983,378.619,255.548 | -0.284,0.070,0.002,-0.004
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.011,0.013,0.699,0.715 | 0.006,-0.053,0.051
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = -0.005,0.016,0.699,0.715 | -0.091,-0.054,0.050
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0299 seconds total (33.5 hz, 4.41 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0123 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0005 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0007 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0002 seconds for SLAM update (2 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0125 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.0262 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.533,-0.639,0.365,0.419 | p_IinG = -0.864,-1.794,2.654 | dist = 4.36 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0038,0.0205,0.0721 | ba = -0.0906,0.0322,0.1398
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = 0.00078
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 457.969,455.704,368.041,247.874 | -0.282,0.070,0.003,-0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.671,454.983,378.619,255.548 | -0.284,0.070,0.002,-0.004
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.011,0.013,0.699,0.715 | 0.006,-0.053,0.051
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = -0.005,0.016,0.699,0.715 | -0.091,-0.054,0.050
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0271 seconds total (36.9 hz, 3.42 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0147 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0005 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0011 seconds for MSCKF update (1 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0000 seconds for SLAM update (10 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0142 seconds for SLAM delayed init (10 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0107 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.04117 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.531,-0.639,0.365,0.421 | p_IinG = -0.551,-1.825,2.938 | dist = 4.44 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0038,0.0205,0.0723 | ba = -0.1048,0.0559,0.1557
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = 0.00062
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.296,455.658,368.328,247.781 | -0.281,0.070,0.003,-0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.957,454.916,378.639,255.415 | -0.283,0.070,0.002,-0.005
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.011,0.011,0.698,0.716 | 0.003,-0.053,0.050
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = -0.005,0.015,0.699,0.715 | -0.094,-0.055,0.050
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0421 seconds total (23.8 hz, 2.42 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0131 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0006 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0000 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0013 seconds for SLAM update (10 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0151 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0111 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.04121 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.527,-0.639,0.365,0.424 | p_IinG = -0.569,-1.848,2.949 | dist = 4.52 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0038,0.0206,0.0723 | ba = -0.1012,0.0558,0.1563
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = 0.00056
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.346,455.611,368.408,247.741 | -0.281,0.070,0.003,-0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.975,454.833,378.698,255.397 | -0.283,0.070,0.002,-0.005
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.011,0.011,0.698,0.716 | 0.002,-0.053,0.049
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = -0.005,0.015,0.699,0.715 | -0.095,-0.054,0.049
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0424 seconds total (23.6 hz, 2.44 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0161 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0006 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0003 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0010 seconds for SLAM update (8 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0020 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0100 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.02998 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.522,-0.640,0.365,0.429 | p_IinG = -0.588,-1.890,2.960 | dist = 4.59 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0038,0.0206,0.0723 | ba = -0.0981,0.0563,0.1559
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = 0.00049
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.390,455.567,368.459,247.698 | -0.281,0.070,0.003,-0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.980,454.742,378.716,255.376 | -0.282,0.070,0.002,-0.005
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.011,0.011,0.699,0.715 | 0.001,-0.052,0.049
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = -0.005,0.014,0.699,0.715 | -0.096,-0.053,0.049
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0310 seconds total (32.2 hz, 4.94 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0165 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0006 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0005 seconds for MSCKF update (1 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0008 seconds for SLAM update (8 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0005 seconds for SLAM delayed init (1 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0116 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.03044 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.517,-0.642,0.364,0.434 | p_IinG = -0.608,-1.937,2.977 | dist = 4.67 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0038,0.0206,0.0724 | ba = -0.0956,0.0573,0.1553
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = 0.00044
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.414,455.518,368.477,247.644 | -0.280,0.070,0.003,-0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.964,454.651,378.714,255.348 | -0.282,0.070,0.002,-0.005
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.011,0.011,0.699,0.715 | 0.001,-0.051,0.049
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = -0.005,0.014,0.699,0.715 | -0.096,-0.052,0.048
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0317 seconds total (31.6 hz, 4.95 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0365 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0006 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0010 seconds for MSCKF update (1 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0005 seconds for SLAM update (7 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0099 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.04851 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.513,-0.645,0.361,0.437 | p_IinG = -0.644,-1.986,2.997 | dist = 4.75 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0038,0.0206,0.0724 | ba = -0.0937,0.0583,0.1544
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = 0.00051
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.414,455.454,368.500,247.608 | -0.280,0.070,0.003,-0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.972,454.603,378.721,255.312 | -0.282,0.070,0.002,-0.005
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.011,0.011,0.699,0.715 | 0.001,-0.051,0.049
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = -0.005,0.014,0.699,0.715 | -0.096,-0.052,0.048
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0495 seconds total (20.2 hz, 3.96 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0223 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0006 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0000 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0005 seconds for SLAM update (7 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0114 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.03483 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.510,-0.649,0.356,0.437 | p_IinG = -0.685,-2.013,3.041 | dist = 4.82 (meters)
[run_subscribe_msckf-1] VioManager.cpp:657 bg = -0.0038,0.0206,0.0724 | ba = -0.0932,0.0586,0.1553
[run_subscribe_msckf-1] VioManager.cpp:662 camera-imu timeoffset = 0.00055
[run_subscribe_msckf-1] VioManager.cpp:669 cam0 intrinsics = 458.412,455.446,368.494,247.604 | -0.280,0.070,0.003,-0.005
[run_subscribe_msckf-1] VioManager.cpp:669 cam1 intrinsics = 457.960,454.594,378.722,255.312 | -0.282,0.070,0.002,-0.005
[run_subscribe_msckf-1] VioManager.cpp:678 cam0 extrinsics = -0.011,0.011,0.699,0.715 | 0.001,-0.051,0.049
[run_subscribe_msckf-1] VioManager.cpp:678 cam1 extrinsics = -0.005,0.014,0.699,0.715 | -0.096,-0.052,0.049
[run_subscribe_msckf-1] ROS2Visualizer.cpp:483 [TIME]: 0.0358 seconds total (27.9 hz, 4.95 ms behind)
[run_subscribe_msckf-1] VioManager.cpp:613 [TIME]: 0.0188 seconds for tracking
[run_subscribe_msckf-1] VioManager.cpp:614 [TIME]: 0.0006 seconds for propagation
[run_subscribe_msckf-1] VioManager.cpp:615 [TIME]: 0.0007 seconds for MSCKF update (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:617 [TIME]: 0.0003 seconds for SLAM update (3 feats)
[run_subscribe_msckf-1] VioManager.cpp:618 [TIME]: 0.0000 seconds for SLAM delayed init (0 feats)
[run_subscribe_msckf-1] VioManager.cpp:620 [TIME]: 0.0106 seconds for re-tri & marg (11 clones in state)
[run_subscribe_msckf-1] VioManager.cpp:628 [TIME]: 0.03106 seconds for total (camera 0 1)
[run_subscribe_msckf-1] VioManager.cpp:654 q_GtoI = 0.509,-0.654,0.351,0.437 | p_IinG = -0.732,-2.057,3.079 | dist = 4.90 (meters)

The frequency of the poseimu is between 10 and 15 hz

Here is the output of ros2 run ov_core test_profile

Setting printing level to: INFO
OPENCV: RANDOM BIG IMAGE: 21.333 +- 0.381ms
OPENCV: HISTOGRAM EQUALIZATION: 1.114 +- 0.431ms
OPENCV: BUILD OPTICAL FLOW PYRAMID: 5.240 +- 0.495ms
OPENCV: FAST FEATURE EXTRACTION: 44.130 +- 0.329 ms (90402.12 avg feats)
OPENCV: KLT OPTICAL FLOW: 24.274 +- 0.791 ms (415.51 avg feats)
EIGEN3(double): RANDOM BIG MATRIX: 13.439 +- 0.096ms
EIGEN3(double): MATRIX MULTIPLY: 15.839 +- 0.138ms
EIGEN3(double): MATRIX INVERSION: 28.371 +- 0.105ms
EIGEN3(double): HOUSEHOLDER QR FULL: 65.858 +- 0.399ms
EIGEN3(double): HOUSEHOLDER QR PIV: 272.822 +- 1.414ms
EIGEN3(double): HOUSEHOLDER QR CUSTOM: 260.112 +- 0.872ms
EIGEN3(float): RANDOM BIG MATRIX: 10.829 +- 0.212ms
EIGEN3(float): MATRIX MULTIPLY: 7.732 +- 0.034ms
EIGEN3(float): MATRIX INVERSION: 13.859 +- 0.086ms
EIGEN3(float): HOUSEHOLDER QR FULL: 215.403 +- 0.222ms
EIGEN3(float): HOUSEHOLDER QR PIV: 74.850 +- 0.736ms
EIGEN3(float): HOUSEHOLDER QR CUSTOM: 67.022 +- 0.581ms

Is there any optimization that can be done for the filter to work OK on Raspberry Pi 4 with 4GB RAM?

Thank you for your time.

Hohyeong commented 7 months ago

I have the same issue on RPI 4B 4GB RAM, using ROS2 galactic. I'm using opencv 4.5.0.

mzahana commented 7 months ago

@Hohyeong I am not sure if it is related to the computation power of RPi 4 or the dataset, but I managed to run the estimator using real-time stereo images on the same RPi 4 and MPU6050 IMU.

Hohyeong commented 7 months ago

@mzahana When I applied the EuroC dataset V1_01 easy to OpenVINS, I checked that the state diverges. Was the state converging properly when do you use real-time sensor? As I check with other issues, I saw that ROS2 often has problems in multi-threading environment. So I'm considering about using OpenVINS based on ROS1.

mzahana commented 7 months ago

Yes. It was not diverging (at least not quickly) for real time sensor. However, it didn't work properly on Euroc dataset.

I am also interested to know how it performs on ros1. Do you gave a way to setup ros1 on Raspberry Pi 4 OS?

goldbattle commented 4 months ago

Maybe this is related to playback and reading the ROS bag from disk in realtime. Not sure if this could be a bottleneck. If you were able to get it working with live sensor data, but not the rosbag, then this would be my suspicion.