Processing ROS2 dataset gets stuck at 3-4 %

[gfr-misha]

I managed to sucessfully run EuRoC dataset as well as HILTI 2022. However, when I try to run it on my data with IMU enabled the processing gets stuck at 3-4% of progress without any warnings or errors. Below I attach the log. Is there a way to debug it to know where exactly it gets stuck? Thanks!

[okvis_node_synchronous-1] I20241025 13:45:15.990123 456272 okvis_node_synchronous.cpp:213] Progress: 2% 
[okvis_node_synchronous-1] I20241025 13:45:16.074002 456272 Frontend.cpp:2271] RANSAC FAIL: 150 inliers, ratio = 0.322581
[okvis_node_synchronous-1] I20241025 13:45:16.149731 456272 Frontend.cpp:2271] RANSAC FAIL: 49 inliers, ratio = 0.148036
[okvis_node_synchronous-1] I20241025 13:45:16.229626 456272 Frontend.cpp:2271] RANSAC FAIL: 18 inliers, ratio = 0.219512
[okvis_node_synchronous-1] I20241025 13:45:16.297343 456272 Frontend.cpp:2271] RANSAC FAIL: 17 inliers, ratio = 0.232877
[okvis_node_synchronous-1] I20241025 13:45:16.393421 456272 Frontend.cpp:2271] RANSAC FAIL: 43 inliers, ratio = 0.623188
[okvis_node_synchronous-1] I20241025 13:45:16.481249 456272 Frontend.cpp:2271] RANSAC FAIL: 43 inliers, ratio = 0.373913
[okvis_node_synchronous-1] I20241025 13:45:16.576957 456272 Frontend.cpp:2271] RANSAC FAIL: 42 inliers, ratio = 0.242775
[okvis_node_synchronous-1] I20241025 13:45:16.663672 456272 Frontend.cpp:2271] RANSAC FAIL: 61 inliers, ratio = 0.238281
[okvis_node_synchronous-1] I20241025 13:45:16.767130 456272 Frontend.cpp:2271] RANSAC FAIL: 57 inliers, ratio = 0.276699
[okvis_node_synchronous-1] I20241025 13:45:16.900861 456272 Frontend.cpp:2271] RANSAC FAIL: 29 inliers, ratio = 0.13615
[okvis_node_synchronous-1] I20241025 13:45:17.017676 456272 Frontend.cpp:2271] RANSAC FAIL: 42 inliers, ratio = 0.33871
[okvis_node_synchronous-1] I20241025 13:45:17.124526 456272 Frontend.cpp:2271] RANSAC FAIL: 67 inliers, ratio = 0.25969
[okvis_node_synchronous-1] I20241025 13:45:17.234644 456272 Frontend.cpp:2271] RANSAC FAIL: 61 inliers, ratio = 0.222628
[okvis_node_synchronous-1] I20241025 13:45:17.356280 456272 Frontend.cpp:2271] RANSAC FAIL: 50 inliers, ratio = 0.347222
[okvis_node_synchronous-1] I20241025 13:45:17.474529 456272 Frontend.cpp:2271] RANSAC FAIL: 108 inliers, ratio = 0.366102
[okvis_node_synchronous-1] I20241025 13:45:17.600672 456272 Frontend.cpp:2271] RANSAC FAIL: 167 inliers, ratio = 0.431525
[okvis_node_synchronous-1] I20241025 13:45:17.701730 456272 Frontend.cpp:2271] RANSAC FAIL: 126 inliers, ratio = 0.529412
[okvis_node_synchronous-1] I20241025 13:45:17.795635 456272 Frontend.cpp:2271] RANSAC FAIL: 112 inliers, ratio = 0.324638
[okvis_node_synchronous-1] I20241025 13:45:17.892614 456272 Frontend.cpp:2271] RANSAC FAIL: 117 inliers, ratio = 0.354545
[okvis_node_synchronous-1] I20241025 13:45:17.984743 456272 Frontend.cpp:2271] RANSAC FAIL: 83 inliers, ratio = 0.345833
[okvis_node_synchronous-1] I20241025 13:45:18.095503 456272 Frontend.cpp:2271] RANSAC FAIL: 152 inliers, ratio = 0.493506
[okvis_node_synchronous-1] I20241025 13:45:18.200789 456272 Frontend.cpp:2271] RANSAC FAIL: 31 inliers, ratio = 0.246032
[okvis_node_synchronous-1] I20241025 13:45:18.315472 456272 Frontend.cpp:2271] RANSAC FAIL: 7 inliers, ratio = 0.28
[okvis_node_synchronous-1] W20241025 13:45:18.318428 456272 Frontend.cpp:609] 3d2d tracking weak: quality=0. Number of 3d2d-matches: 28
[okvis_node_synchronous-1] I20241025 13:45:18.407487 456272 Frontend.cpp:2271] RANSAC FAIL: 5 inliers, ratio = 0.227273
[okvis_node_synchronous-1] I20241025 13:45:18.507031 456272 Frontend.cpp:2271] RANSAC FAIL: 6 inliers, ratio = 0.3
[okvis_node_synchronous-1] W20241025 13:45:18.509940 456272 Frontend.cpp:609] 3d2d tracking weak: quality=0. Number of 3d2d-matches: 23
[okvis_node_synchronous-1] I20241025 13:45:18.604813 456272 Frontend.cpp:2271] RANSAC FAIL: 5 inliers, ratio = 0.333333
[okvis_node_synchronous-1] I20241025 13:45:18.697595 456272 Frontend.cpp:2271] RANSAC FAIL: 19 inliers, ratio = 0.59375
[okvis_node_synchronous-1] I20241025 13:45:18.773200 456272 Frontend.cpp:2271] RANSAC FAIL: 83 inliers, ratio = 0.557047
[okvis_node_synchronous-1] I20241025 13:45:18.870834 456272 Frontend.cpp:2271] RANSAC FAIL: 112 inliers, ratio = 0.390244
[okvis_node_synchronous-1] I20241025 13:45:18.979297 456272 Frontend.cpp:2271] RANSAC FAIL: 38 inliers, ratio = 0.175926
[okvis_node_synchronous-1] I20241025 13:45:19.091317 456272 Frontend.cpp:2271] RANSAC FAIL: 36 inliers, ratio = 0.305085
[okvis_node_synchronous-1] I20241025 13:45:19.188810 456272 Frontend.cpp:2271] RANSAC FAIL: 46 inliers, ratio = 0.213953
[okvis_node_synchronous-1] I20241025 13:45:19.306375 456272 Frontend.cpp:2271] RANSAC FAIL: 41 inliers, ratio = 0.163347
[okvis_node_synchronous-1] I20241025 13:45:19.418015 456272 Frontend.cpp:2271] RANSAC FAIL: 16 inliers, ratio = 0.137931
[okvis_node_synchronous-1] I20241025 13:45:19.439040 456272 okvis_node_synchronous.cpp:208] OKVIS Timing
[okvis_node_synchronous-1] -----------
[okvis_node_synchronous-1] 1 DetectAndDescribe                              100 00:00:01.298225 (00:00:00.012982 +- 00:00:00.002034)    [00:00:00.009879,00:00:00.020602]
[okvis_node_synchronous-1] 2 Match                                          100 00:00:02.189336 (00:00:00.021893 +- 00:00:00.006216)    [00:00:00.000097,00:00:00.040259]
[okvis_node_synchronous-1] 2.01 match to map                                 99 00:00:00.829172 (00:00:00.008375 +- 00:00:00.002044)    [00:00:00.000516,00:00:00.016920]
[okvis_node_synchronous-1] 2.02 match motion stereo                          99 00:00:00.810641 (00:00:00.008188 +- 00:00:00.002273)    [00:00:00.002735,00:00:00.017085]
[okvis_node_synchronous-1] 2.02.1 match motion stereo: prepare               99 00:00:00.172856 (00:00:00.001746 +- 00:00:00.000445)    [00:00:00.000003,00:00:00.002849]
[okvis_node_synchronous-1] 2.03 loop closure query                           98 00:00:00.070279 (00:00:00.000717 +- 00:00:00.000379)    [00:00:00.000157,00:00:00.001675]
[okvis_node_synchronous-1] 2.04 loop closure descriptor matching            445 00:00:00.031289 (00:00:00.000070 +- 00:00:00.000074)    [00:00:00.000006,00:00:00.000413]
[okvis_node_synchronous-1] 2.05 loop closure ransacking                     163 00:00:00.171497 (00:00:00.001052 +- 00:00:00.000342)    [00:00:00.000712,00:00:00.003098]
[okvis_node_synchronous-1] 2.07 attempt loop closure                          0
[okvis_node_synchronous-1] 2.10 match stereo                                 74 00:00:00.000100 (00:00:00.000001 +- 00:00:00.000001)    [00:00:00.000001,00:00:00.000007]
[okvis_node_synchronous-1] 3 Optimise                                        99 00:00:04.318020 (00:00:00.043616 +- 00:00:00.019355)    [00:00:00.000627,00:00:00.077714]
[okvis_node_synchronous-1] 4 Prepare publishing                              99 00:00:00.332922 (00:00:00.003363 +- 00:00:00.000763)    [00:00:00.000035,00:00:00.005791]
[okvis_node_synchronous-1] 6 Visualising                                     99 00:00:00.015485 (00:00:00.000156 +- 00:00:00.000058)    [00:00:00.000014,00:00:00.000340]
[okvis_node_synchronous-1] 6.2 Prepare visualising matches                   99 00:00:00.015225 (00:00:00.000154 +- 00:00:00.000058)    [00:00:00.000011,00:00:00.000337]
[okvis_node_synchronous-1] 7 Marginalise                                     99 00:00:00.898607 (00:00:00.009077 +- 00:00:00.003255)    [00:00:00.000003,00:00:00.021392]
[okvis_node_synchronous-1] 7.1 check/handle lost                             99 00:00:00.006199 (00:00:00.000063 +- 00:00:00.000033)    [00:00:00.000000,00:00:00.000150]
[okvis_node_synchronous-1] 7.2 eliminate IMU frames                          99 00:00:00.089824 (00:00:00.000907 +- 00:00:00.001586)    [00:00:00.000001,00:00:00.004861]
[okvis_node_synchronous-1] 7.3 convert to posegraph                          68 00:00:00.313028 (00:00:00.004603 +- 00:00:00.002488)    [00:00:00.002165,00:00:00.016386]
[okvis_node_synchronous-1] 7.4 freezing                                      98 00:00:00.000253 (00:00:00.000003 +- 00:00:00.000002)    [00:00:00.000000,00:00:00.000008]
[okvis_node_synchronous-1] 7.6 expand                                        93 00:00:00.169478 (00:00:00.001822 +- 00:00:00.000585)    [00:00:00.000515,00:00:00.005213]
[okvis_node_synchronous-1] 7.7 prune place recognition frames                98 00:00:00.114756 (00:00:00.001171 +- 00:00:00.000331)    [00:00:00.000068,00:00:00.002029]
[okvis_node_synchronous-1] I20241025 13:45:19.439124 456272 okvis_node_synchronous.cpp:213] Progress: 3% 
[okvis_node_synchronous-1] I20241025 13:45:19.536705 456272 Frontend.cpp:2271] RANSAC FAIL: 105 inliers, ratio = 0.274869
[okvis_node_synchronous-1] I20241025 13:45:19.658738 456272 Frontend.cpp:2271] RANSAC FAIL: 185 inliers, ratio = 0.364892
[okvis_node_synchronous-1] I20241025 13:45:19.784689 456272 Frontend.cpp:2271] RANSAC FAIL: 78 inliers, ratio = 0.229412
[okvis_node_synchronous-1] I20241025 13:45:19.888964 456272 Frontend.cpp:2271] RANSAC FAIL: 64 inliers, ratio = 0.216216
[okvis_node_synchronous-1] I20241025 13:45:20.001638 456272 Frontend.cpp:2271] RANSAC FAIL: 36 inliers, ratio = 0.185567
[okvis_node_synchronous-1] I20241025 13:45:20.103458 456272 Frontend.cpp:2271] RANSAC FAIL: 53 inliers, ratio = 0.236607
[okvis_node_synchronous-1] I20241025 13:45:20.217494 456272 Frontend.cpp:2271] RANSAC FAIL: 96 inliers, ratio = 0.408511
[okvis_node_synchronous-1] I20241025 13:45:20.326186 456272 Frontend.cpp:2271] RANSAC FAIL: 57 inliers, ratio = 0.311475
[okvis_node_synchronous-1] I20241025 13:45:20.429425 456272 Frontend.cpp:2271] RANSAC FAIL: 21 inliers, ratio = 0.151079
[okvis_node_synchronous-1] I20241025 13:45:20.531863 456272 Frontend.cpp:2271] RANSAC FAIL: 28 inliers, ratio = 0.229508
[okvis_node_synchronous-1] I20241025 13:45:20.641443 456272 Frontend.cpp:2271] RANSAC FAIL: 81 inliers, ratio = 0.440217

[sleutenegger]

Could you share with us (parts of) the dataset so we can reproduce this behavior?

[gfr-misha]

@sleutenegger apologies for the very overdue reply! |

You can find a test dataset attached below. We are trying to make it work in synchronous mode, which it does if imu is disabled. It works so-so in the subscriber mode. However, when imu is enabled in synchronous mode the processing gets stuck immediately.

Any suggestions are welcome!

https://drive.google.com/file/d/1d-6may0H5PxISublkHVxHWEbP-qndKgh/view?usp=sharing

Config file:

%YAML:1.0
cameras:
     #right camera, /tracking_front
     - {T_SC:
        [-0.00666736, 0.00065237, 0.99997756, 0.0305577, 
         0.99989474, -0.01288199, 0.00667521, 0.01683913,
         0.01288606, 0.99991681, -0.00056641, -0.00439179,
         0,0,0,1.],
        image_dimension: [1280, 800],
        distortion_coefficients: [0.04521904999207441, 0.04093252080036277, -0.028516487541826886, 0.006077390594816995],
        camera_name: /tracking_front,
        distortion_type: equidistant,
        focal_length: [468.98040619359426, 468.8544998317947],
        principal_point: [637.496318100534, 369.75333131991454],
        camera_type: gray, #gray, rgb, gray+depth, rgb+depth
        slam_use: okvis}
     
     #left camera, /tracking_rear
     - {T_SC:
        [ -0.8660381,  0.0000000, -0.4999780, -0.133, 
            -0.4999780,  0.0000000, 0.8660381, 0.055,
            0.0000000, 1.0000000,  0.0000000, 0.041,
            0.0000000000000000000, 0.0000000000000000000, 0.0000000000000000000, 1.0000000000000000000],
        image_dimension: [1280, 800],
        distortion_coefficients: [0.0442, 0.0410, -0.0281, 0.0068],
        camera_name: /tracking_rear,
        distortion_type: equidistant,
        focal_length: [470.422, 470.148],
        principal_point: [652.701, 387.915],
        camera_type: gray, #gray, rgb, gray+depth, rgb+depth
        slam_use: okvis}

     #upward camera, /tracking_down
     - {T_SC:
        [ 0.8660381,  0.0000000, -0.4999780, -0.134, 
            -0.4999780,  0.0000000, -0.8660381, -0.093,
            0.0000000, 1.0000000,  0.0000000, 0.041,
            0.0000000000000000000, 0.0000000000000000000, 0.0000000000000000000, 1.0000000000000000000],
        image_dimension: [1280, 800],
        distortion_coefficients: [0.0404, 0.0575, -0.0407, 0.0101],
        camera_name: /tracking_down,
        distortion_type: equidistant,
        focal_length: [466.158, 465.881],
        principal_point: [685.518, 391.768],
        camera_type: gray, #gray, rgb, gray+depth, rgb+depth
        slam_use: okvis}


# additional camera parameters
camera_parameters:
    timestamp_tolerance: 0.0035068605940155686 # [s] stereo frame out-of-sync tolerance
    image_delay: 0.0035068605940155686  # [s] timestamp_camera_correct = timestamp_camera - image_delay
    sync_cameras: [0,1,2] # the cameras that will be synchronised
    online_calibration: # some parameters to set the online 
        do_extrinsics: true # Do we online-calibrate extrinsics?
        do_extrinsics_final_ba: true # Do we calibrate extrinsics in final BA?
        sigma_r: 0.001 # T_SCi position prior stdev [m]
        sigma_alpha: 0.01 # T_SCi orientation prior stdev [rad]


imu_parameters:
    name: /imu_apps 
    use: true
    a_max: 176.0 # acceleration saturation [m/s^2]
    g_max: 10.0 # gyro saturation [rad/s]
    sigma_g_c: 0.0002444351257197984 # gyro noise density [rad/s/sqrt(Hz)]
    sigma_a_c: 0.02855544931738909 # accelerometer noise density [m/s^2/sqrt(Hz)]
    sigma_bg: 0.03 # gyro bias prior [rad/s]
    sigma_ba: 0.0252 # accelerometer bias prior [m/s^2]
    sigma_gw_c: 3.337348319684385e-05 # gyro drift noise density [rad/s^s/sqrt(Hz)]
    sigma_aw_c: 0.00032369994691723954 # accelerometer drift noise density [m/s^2/sqrt(Hz)]
    # Local gravity at 85748 Garching, Germany (latitude: 48.262768, elevation: 481m), according to https://www.sensorsone.com/local-gravity-calculator/
    g: 9.81007 # Earth's acceleration due to gravity [m/s^2]
    g0: [ 0.0, 0.0, 0.0 ] # initial gyro bias [rad/s]
    a0: [0.237891675418656, 0.167477259386443, 0.242558311983621]  # initial accelerometer bias [m/s^2]
    # tranform Body-Sensor (IMU)
    T_BS:
        [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]

# frontend: detection etc. 
frontend_parameters:
    detection_threshold: 38.0 #42.0 # detection threshold. By default the uniformity radius in pixels
    absolute_threshold: 150.0 # absolute Harris corner threshold (noise floor)
    matching_threshold: 60.0 # BRISK descriptor matching threshold
    octaves: 0 # number of octaves for detection. 0 means single-scale at highest resolution
    max_num_keypoints: 700 # restrict to a maximum of this many keypoints per image (strongest ones)
    keyframe_overlap: 0.59 # minimum field-of-view overlap
    use_cnn: false # use the CNN (if available) to filter out dynamic content / sky
    parallelise_detection: true # Run parallel detect & describe.
    num_matching_threads: 4 # Parallelise matching with this number of threads.

# estimator parameters
estimator_parameters:
    num_keyframes: 5 # number of keyframes in optimisation window
    num_loop_closure_frames: 5 # number of loop closure frames in optimisation window
    num_imu_frames: 3 # number of frames linked by most recent nonlinear IMU error terms
    do_loop_closures: true # whether to do VI-SLAM or VIO
    do_final_ba: true # Whether to run a full final BA
    enforce_realtime: false # whether to limit the time budget for optimisation 
    realtime_min_iterations: 3 # minimum number of iterations always performed
    realtime_max_iterations: 10 # never do more than these, even if not converged
    realtime_time_limit: 0.035 # time budget for realtime optimisation [s]
    realtime_num_threads: 4 # number of threads for the realtime optimisation
    full_graph_iterations: 15 # don't do more than these for the full (background) optimisation
    full_graph_num_threads: 2 # number of threads for the full (background) optimisation

# some options for how and what to output
output_parameters:
    display_matches: true  # displays debug video and matches. May be slow.
    display_overhead: true # debug overhead image. Is slow.

[bochsim]

@gfr-misha sorry for our late response on this matter - I just tried to reproduce the issue but unfortunately, I am not able to process the provided bag file... (neither playing nor reindexing seems to properly work)

Could you maybe double-check that you uploaded the correct, non-corrupted bag file? Which ROS2 distribution are you using?

[gfr-misha]

I think the root cause of the issue I was facing was irregular imu sampling rates. When I collected the data in a different mode and ran okvis in the synchronous mode (ROS-free), it performed very well. So, the ROS driver on my acquisition device is cooked :)

I'm now trying to make it work with asynchronous point clouds and perform some kind of lidar odometry in addition to visual one. Great project!