Queue synchronizer error

[violetteavi]

Description:
I'm working on integrating Kimera into an MAV simulator. The simulator creates rectified stereo images and an IMU stream. I unfortunately can't share launch/param files due to confidentiality reasons, but they are basically Euroc's with a different camera/IMU model. I'm running through the ROS wrapper.

The sim is very slow in wall clock time, but uses sim_time to "speed up" to 60 Hz. Any ideas as to why I'm getting the queue synchronizer error below?

Command:

roslaunch kimera_vio_ros <launchfile>

Console output:

[ INFO] [1588266720.331173496]: Initializing pose graph visualizer
I0430 13:12:00.557873 15321 RegularVioBackEnd.cpp:114] Using Regular VIO backend.
I0430 13:12:00.630167 15321 RosOnlineDataProvider.cpp:41] Waiting for ROS time to be valid... 
(Sim Time is enabled; run rosbag with --clock argument)
I0430 13:12:00.810081 15345 RosOnlineDataProvider.cpp:350] Spinning RosOnlineDataProvider.
I0430 13:12:00.810123 15346 Pipeline.h:174] Spinning Kimera-VIO.
I0430 13:12:00.810075 15321 KimeraVioRos.cpp:99] 
W0430 13:12:01.065454 15349 RosDataProviderInterface.cpp:118] Converting image...
W0430 13:12:01.336712 15346 DataProviderModule.cpp:115] Asking for data before start of IMU stream, from timestamp: 29444801089 to timestamp: 29460426094
I0430 13:12:01.346658 15345 RosOnlineDataProvider.cpp:383] Spinning RosOnlineDataProvider.
I0430 13:12:01.631211 15346 Pipeline.cpp:776] Frontend launched (parallel_run set to 1).
I0430 13:12:01.631243 15346 Pipeline.cpp:591] ------------------- Initialize Pipeline with frame k = 2--------------------
W0430 13:12:01.631251 15346 InitializationFromImu.cpp:26] InitializationFromImu: assumes that the vehicle is stationary and upright along some axis,and gravity vector is along a single axis!
I0430 13:12:01.704898 15346 Pipeline.cpp:300] Before launching threads.
I0430 13:12:01.704996 15346 Pipeline.cpp:805] Backend, mesher and visualizer launched (parallel_run set to 1).
I0430 13:12:01.705001 15346 Pipeline.cpp:302]  launching threads.
W0430 13:12:02.438673 15350 RosDataProviderInterface.cpp:118] Converting image...
W0430 13:12:03.792672 15350 RosDataProviderInterface.cpp:118] Converting image...
W0430 13:12:04.920267 15351 Tracker.cpp:578] 1-point RANSAC (voting) could not find a solution.
F0430 13:12:04.970415 15353 QueueSynchronizer.h:138] Check failed: timestamp == payload_timestamp (29678134497 vs. 29477092766) Syncing queue mesher_frontend in module Mesher failed;
 Could not retrieve exact timestamp requested: 
 - Requested timestamp: 29678134497
 - Actual timestamp:    29477092766
Reached max number of sync attempts: 10
*** Check failure stack trace: ***
    @     0x7fb145fceb0d  google::LogMessage::Fail()
    @     0x7fb145fd09b1  google::LogMessage::SendToLog()
    @     0x7fb145fce63d  google::LogMessage::Flush()
    @     0x7fb145fd1389  google::LogMessageFatal::~LogMessageFatal()
    @     0x7fb14579c81d  VIO::SimpleQueueSynchronizer<>::syncQueue()
    @     0x7fb13e97563e  VIO::MesherModule::getInputPacket()
    @     0x7fb13e97ad9f  VIO::PipelineModule<>::spin()
    @     0x7fb144c096df  (unknown)
    @     0x7fb14432b6db  start_thread
    @     0x7fb14466488f  clone

Please give also the following information:

• SparkVio branch, tag or commit used: master
• GTSAM version used:
• OpenGV version used:
• OpenCV version used:
• Operating system and version (e.g. Ubuntu 16.04 or Windows 10): Ubuntu 18.04
• Did you change the source code? (yes / no): no, except the launch file

[violetteavi]

It might be an issue with timestamps that are equal-- here's a text file with timestamps from ~ a second of output
Timestamps.txt

[violetteavi]

I ran Kimera five times on the rosbag that has ^ those timestamps and received the same error each time, but with different timestamps in the error message. Here are the timestamps it errored out on.
Round 1:

 - Requested timestamp: 247080287399
 - Actual timestamp:    246880287335

Round 2:

 - Requested timestamp: 247113620743
 - Actual timestamp:    246913620679

Round 3:

 - Requested timestamp: 247163620759
 - Actual timestamp:    246963620695

Round 4:

 - Requested timestamp: 247163620759
 - Actual timestamp:    246946954023

Round 5:

 - Requested timestamp: 247030287383
 - Actual timestamp:    246813620647

It looks like both timestamps are sensor timestamps from the IMU. The Requested timestamp can be any of the IMU messages, but the Actual timestamp is always a timestamp that both an IMU and a stereo pair are published on.

[violetteavi]

The Requested timestamp comes from the backend output queue and the Actual timestamp comes from the frontend output queue. Both should be tied to stereo frame timestamps.

Somehow, we are getting a backend output packet that has a timestamp from an IMU packet, not a stereo frame, causing this call to fail. @ToniRV Any ideas?

[violetteavi]

Also to note, the simulated camera stream has nothing on the cameras except the background (0 disparity between left/right). How does Kimera's VioBackEndModule respond in this case? It may be failing in a weird way, then passing a bad input packet to the mesher.

[violetteavi]

In the end, this error was fixed by giving the MesherModule more room to work with in its queue. This appears to be dataset dependent, but if you want to give it a shot you can pass the desired number of iterations into syncQueue in MesherModule.cpp:

  PIO::syncQueue(timestamp, &frontend_payload_queue_, &frontend_payload);

to this:

  PIO::syncQueue(timestamp, &frontend_payload_queue_, &frontend_payload, 40);