Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Infinite loop when no IMU data between two frames #37

Closed
ruffsl opened this issue Mar 10, 2020 · 4 comments
Closed

Infinite loop when no IMU data between two frames #37

ruffsl opened this issue Mar 10, 2020 · 4 comments

Comments

@ruffsl
Copy link
Contributor

ruffsl commented Mar 10, 2020

Description:

As firsted pointed out in MIT-SPARK/Kimera-VIO-ROS2#5 (comment) , it seems the Kimera-VIO is liable to fall into an infinite loop when no IMU data between two frames is received. Such can occur when IMU messages are lost or dropped by the middleware QoS, or when the sample rates from the IMU and Stereo frames happened to alias such that two Stereo frames are timestamped between two consecutively received IMU samples.

Command:

See launch command and context from linked PR:

ros2 launch kimera_vio_ros kimera_ros_realsense.launch.xml

MIT-SPARK/Kimera-VIO-ROS2#5 (comment)

Console output:

$ ros2 launch kimera_vio_ros kimera_ros_realsense.launch.xml 
[INFO] [launch]: All log files can be found below /home/ruffsl/.ros/log/2020-03-06-11-59-33-116779-cog-rw07-6169
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [kimera_vio_ros-1]: process started with pid [6180]
[kimera_vio_ros-1] I0306 11:59:33.421044  6180 CameraParams.cpp:31] Parsing camera parameters for: left_cam
[kimera_vio_ros-1] I0306 11:59:33.582267  6180 CameraParams.cpp:31] Parsing camera parameters for: right_cam
[kimera_vio_ros-1] I0306 11:59:33.582564  6180 DataProviderInterface.cpp:96] Running dataset between frame 50 and frame 10000
[kimera_vio_ros-1] I0306 11:59:33.589473  6180 RegularVioBackEnd.cpp:113] Using Regular VIO backend.
[kimera_vio_ros-1] I0306 11:59:33.596930  6194 PipelineModule.h:163] Module: Data Provider - Spinning.
[kimera_vio_ros-1] W0306 11:59:33.672981  6194 DataProviderModule.cpp:97] No IMU data from last frame timestamp: 1583524773595843072 to timestamp: 1583524773629287936
[kimera_vio_ros-1] W0306 11:59:33.673069  6194 DataProviderModule.cpp:97] No IMU data from last frame timestamp: 1583524773595843072 to timestamp: 1583524773629287936
[kimera_vio_ros-1] W0306 11:59:33.673132  6194 DataProviderModule.cpp:97] No IMU data from last frame timestamp: 1583524773595843072 to timestamp: 1583524773629287936
[kimera_vio_ros-1] W0306 11:59:33.673167  6194 DataProviderModule.cpp:97] No IMU data from last frame timestamp: 1583524773595843072 to timestamp: 1583524773629287936
[kimera_vio_ros-1] W0306 11:59:33.673197  6194 DataProviderModule.cpp:97] No IMU data from last frame timestamp: 1583524773595843072 to timestamp: 1583524773629287936
[kimera_vio_ros-1] W0306 11:59:33.673235  6194 DataProviderModule.cpp:97] No IMU data from last frame timestamp: 1583524773595843072 to timestamp: 1583524773629287936
[kimera_vio_ros-1] W0306 11:59:33.673280  6194 DataProviderModule.cpp:97] No IMU data from last frame timestamp: 1583524773595843072 to timestamp: 1583524773629287936
[kimera_vio_ros-1] W0306 11:59:33.673322  6194 DataProviderModule.cpp:97] No IMU data from last frame timestamp: 1583524773595843072 to timestamp: 1583524773629287936
[kimera_vio_ros-1] W0306 11:59:33.673369  6194 DataProviderModule.cpp:97] No IMU data from last frame timestamp: 1583524773595843072 to timestamp: 1583524773629287936
[kimera_vio_ros-1] W0306 11:59:33.673408  6194 DataProviderModule.cpp:97] No IMU data from last frame timestamp: 1583524773595843072 to timestamp: 1583524773629287936
[kimera_vio_ros-1] W0306 11:59:33.673439  6194 DataProviderModule.cpp:97] No IMU data from last frame timestamp: 1583524773595843072 to timestamp: 1583524773629287936

The infinite looping is encountered most of the time, but something one can get lucky with the race condition and avoid it for a few seconds:

$ ros2 launch kimera_vio_ros kimera_ros_realsense.launch.xml 
[INFO] [launch]: All log files can be found below /home/ruffsl/.ros/log/2020-03-06-11-44-36-116764-cog-rw07-4477
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [kimera_vio_ros-1]: process started with pid [4490]
[kimera_vio_ros-1] I0306 11:44:36.403184  4490 CameraParams.cpp:31] Parsing camera parameters for: left_cam
[kimera_vio_ros-1] I0306 11:44:36.537226  4490 CameraParams.cpp:31] Parsing camera parameters for: right_cam
[kimera_vio_ros-1] I0306 11:44:36.537469  4490 DataProviderInterface.cpp:96] Running dataset between frame 50 and frame 10000
[kimera_vio_ros-1] I0306 11:44:36.542948  4490 RegularVioBackEnd.cpp:113] Using Regular VIO backend.
[kimera_vio_ros-1] I0306 11:44:36.551772  4504 PipelineModule.h:163] Module: Data Provider - Spinning.
[kimera_vio_ros-1] I0306 11:44:36.733357  4504 Pipeline.cpp:780] Frontend launched (parallel_run set to 1).
[kimera_vio_ros-1] I0306 11:44:36.733377  4544 PipelineModule.h:163] Module: VioFrontEnd - Spinning.
[kimera_vio_ros-1] I0306 11:44:36.733757  4504 Pipeline.cpp:511] ------------------- Initialize Pipeline with frame k = 1--------------------
[kimera_vio_ros-1] W0306 11:44:36.733768  4504 InitializationFromImu.cpp:26] InitializationFromImu: assumes that the vehicle is stationary and upright along some axis,and gravity vector is along a single axis!
[kimera_vio_ros-1] I0306 11:44:36.802474  4504 VioBackEnd.cpp:217] Initial state seed: 
[kimera_vio_ros-1]  - Initial pose: 
[kimera_vio_ros-1] |0.999984, -0.0027797, 0.0049191|
[kimera_vio_ros-1] |-0.0027797, 0.515936, 0.856622|
[kimera_vio_ros-1] |-0.0049191, -0.856622, 0.51592|
[kimera_vio_ros-1] 
[kimera_vio_ros-1] [0, 0, 0]';
[kimera_vio_ros-1] 
[kimera_vio_ros-1]  - Initial vel: 0 0 0
[kimera_vio_ros-1]  - Initial IMU bias: acc = [-0.00123735, -0.215474, 0.129774]' gyro = [-0.00703574, -2.63912e-05, 6.80661e-06]'
[kimera_vio_ros-1] I0306 11:44:36.807591  4504 Pipeline.cpp:288] Before launching threads.
[kimera_vio_ros-1] I0306 11:44:36.807698  4504 Pipeline.cpp:809] Backend, mesher and visualizer launched (parallel_run set to 1).
[kimera_vio_ros-1] I0306 11:44:36.807705  4504 Pipeline.cpp:290]  launching threads.
[kimera_vio_ros-1] I0306 11:44:36.807698  4564 PipelineModule.h:163] Module: VioBackEnd - Spinning.
[kimera_vio_ros-1] I0306 11:44:36.807870  4565 PipelineModule.h:163] Module: MesherModule - Spinning.
[kimera_vio_ros-1] I0306 11:44:36.814316  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 2--------------------
[kimera_vio_ros-1] W0306 11:44:36.824456  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 100 Hz. (10 ms).
[kimera_vio_ros-1] I0306 11:44:36.824506  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 3--------------------
[kimera_vio_ros-1] W0306 11:44:36.831315  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 166.667 Hz. (6 ms).
[kimera_vio_ros-1] I0306 11:44:36.831351  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 4--------------------
[kimera_vio_ros-1] W0306 11:44:36.835386  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 250 Hz. (4 ms).
[kimera_vio_ros-1] I0306 11:44:36.960853  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 5--------------------
[kimera_vio_ros-1] I0306 11:44:37.008589  4544 StereoVisionFrontEnd.cpp:124] Keyframe 5 with: 443 smart measurements
[kimera_vio_ros-1] W0306 11:44:37.008661  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 21.2766 Hz. (47 ms).
[kimera_vio_ros-1] I0306 11:44:37.008683  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 6--------------------
[kimera_vio_ros-1] I0306 11:44:37.008906  4564 RegularVioBackEnd.cpp:211] Tracker has a LOW_DISPARITY status.
[kimera_vio_ros-1] W0306 11:44:37.010509  4565 Mesher.cpp:1283] Missing landmark information to build 3D Mesh.
[kimera_vio_ros-1] W0306 11:44:37.010524  4564 PipelineModule.h:186] Module: VioBackEnd - frequency: 1000 Hz. (1 ms).
[kimera_vio_ros-1] W0306 11:44:37.013517  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 250 Hz. (4 ms).
[kimera_vio_ros-1] W0306 11:44:37.014544  4565 PipelineModule.h:186] Module: MesherModule - frequency: 250 Hz. (4 ms).
[kimera_vio_ros-1] I0306 11:44:37.026594  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 7--------------------
[kimera_vio_ros-1] W0306 11:44:37.033161  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 166.667 Hz. (6 ms).
[kimera_vio_ros-1] I0306 11:44:37.061677  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 8--------------------
[kimera_vio_ros-1] W0306 11:44:37.068816  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 142.857 Hz. (7 ms).
[kimera_vio_ros-1] I0306 11:44:37.126309  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 9--------------------
[kimera_vio_ros-1] W0306 11:44:37.130944  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 250 Hz. (4 ms).
[kimera_vio_ros-1] I0306 11:44:37.156752  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 10--------------------
[kimera_vio_ros-1] W0306 11:44:37.161860  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 200 Hz. (5 ms).
[kimera_vio_ros-1] I0306 11:44:37.191395  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 11--------------------
[kimera_vio_ros-1] I0306 11:44:37.227880  4544 StereoVisionFrontEnd.cpp:124] Keyframe 11 with: 507 smart measurements
[kimera_vio_ros-1] W0306 11:44:37.227944  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 27.7778 Hz. (36 ms).
[kimera_vio_ros-1] I0306 11:44:37.227962  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 12--------------------
[kimera_vio_ros-1] I0306 11:44:37.228159  4564 RegularVioBackEnd.cpp:211] Tracker has a LOW_DISPARITY status.
[kimera_vio_ros-1] W0306 11:44:37.230064  4565 Mesher.cpp:1283] Missing landmark information to build 3D Mesh.
[kimera_vio_ros-1] W0306 11:44:37.230144  4564 PipelineModule.h:186] Module: VioBackEnd - frequency: 500 Hz. (2 ms).
[kimera_vio_ros-1] W0306 11:44:37.232029  4565 PipelineModule.h:186] Module: MesherModule - frequency: 1000 Hz. (1 ms).
[kimera_vio_ros-1] W0306 11:44:37.232172  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 250 Hz. (4 ms).
[kimera_vio_ros-1] I0306 11:44:37.256656  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 13--------------------
[kimera_vio_ros-1] W0306 11:44:37.262290  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 200 Hz. (5 ms).
[kimera_vio_ros-1] I0306 11:44:37.292416  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 14--------------------
[kimera_vio_ros-1] W0306 11:44:37.297888  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 200 Hz. (5 ms).
[kimera_vio_ros-1] I0306 11:44:37.327052  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 15--------------------
[kimera_vio_ros-1] W0306 11:44:37.332326  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 200 Hz. (5 ms).
[kimera_vio_ros-1] I0306 11:44:37.361037  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 16--------------------
[kimera_vio_ros-1] W0306 11:44:37.366055  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 200 Hz. (5 ms).
[kimera_vio_ros-1] I0306 11:44:37.395197  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 17--------------------
[kimera_vio_ros-1] W0306 11:44:37.402889  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 142.857 Hz. (7 ms).
[kimera_vio_ros-1] I0306 11:44:37.463069  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 18--------------------
[kimera_vio_ros-1] I0306 11:44:37.509714  4544 StereoVisionFrontEnd.cpp:124] Keyframe 18 with: 467 smart measurements
[kimera_vio_ros-1] W0306 11:44:37.509785  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 21.7391 Hz. (46 ms).
[kimera_vio_ros-1] I0306 11:44:37.510071  4564 RegularVioBackEnd.cpp:211] Tracker has a LOW_DISPARITY status.
[kimera_vio_ros-1] W0306 11:44:37.512203  4565 Mesher.cpp:1283] Missing landmark information to build 3D Mesh.
[kimera_vio_ros-1] W0306 11:44:37.512255  4564 PipelineModule.h:186] Module: VioBackEnd - frequency: 500 Hz. (2 ms).
[kimera_vio_ros-1] W0306 11:44:37.515733  4565 PipelineModule.h:186] Module: MesherModule - frequency: 333.333 Hz. (3 ms).
[kimera_vio_ros-1] I0306 11:44:37.526648  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 19--------------------
[kimera_vio_ros-1] W0306 11:44:37.531821  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 200 Hz. (5 ms).
[kimera_vio_ros-1] I0306 11:44:37.560017  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 20--------------------
[kimera_vio_ros-1] W0306 11:44:37.565753  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 200 Hz. (5 ms).
[kimera_vio_ros-1] I0306 11:44:37.592141  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 21--------------------
[kimera_vio_ros-1] W0306 11:44:37.597502  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 200 Hz. (5 ms).
[kimera_vio_ros-1] I0306 11:44:37.628166  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 22--------------------
[kimera_vio_ros-1] W0306 11:44:37.634687  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 166.667 Hz. (6 ms).
[kimera_vio_ros-1] I0306 11:44:37.656633  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 23--------------------
[kimera_vio_ros-1] W0306 11:44:37.661800  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 200 Hz. (5 ms).
[kimera_vio_ros-1] I0306 11:44:37.689841  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 24--------------------
[kimera_vio_ros-1] I0306 11:44:37.730131  4544 StereoVisionFrontEnd.cpp:124] Keyframe 24 with: 461 smart measurements
[kimera_vio_ros-1] W0306 11:44:37.730195  4544 PipelineModule.h:186] Module: VioFrontEnd - frequency: 25 Hz. (40 ms).
[kimera_vio_ros-1] I0306 11:44:37.730213  4544 StereoVisionFrontEnd.cpp:62] ------------------- Processing frame k = 25--------------------

Additional files:

For reference, I'm using an updated realsense package for ROS2:
https://github.com/ruffsl/ros2_intel_realsense/tree/devel
(The repo's devel branch includes a Dockerfile for how to build)
And the Kimera-VIO-ROS2 branch for the current work in progress PR:
MIT-SPARK/Kimera-VIO-ROS2#5

  • OS: Ubuntu 18.04
  • Kimera-VIO: bceed72
  • Downstream Libraries: See .repo files in respective fork/repo
  • Source code changed: yes
  • Just trivial build system fixes
  • master...ruffsl:patch-3
@ToniRV
Copy link
Collaborator

ToniRV commented Mar 10, 2020

About to merge fix

@ToniRV
Copy link
Collaborator

ToniRV commented Mar 11, 2020

Ok, fix merged. Let us know if it works for you @ruffsl
Also, this issue is related: MIT-SPARK/Kimera-VIO-ROS#49

@ruffsl
Copy link
Contributor Author

ruffsl commented Mar 13, 2020

I've rebased off the latest push to the public repo, and no longer encounter this issue. Though the diff from the recent set of commits pushed to the public repo is rather large so I couldn't easily grok which exact changes may have fixed this. I'd might suggest making more regular steady syncs to the open repos, as large infrequent pushes of commits can dissuade external collaboration.

@ruffsl ruffsl closed this as completed Mar 13, 2020
@ToniRV
Copy link
Collaborator

ToniRV commented Mar 13, 2020

Thank you @ruffsl, I totally agree! We should make smaller PRs and encourage regular syncs.
The semester is turning out to be a bit hectic.

@ruffsl ruffsl mentioned this issue Mar 16, 2020
Draft
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
2 participants
@ruffsl @ToniRV