Output always delayed by maxWaitTime_

[ZacharyTaylor]

I was testing a camera IMU combo with some significant delay in the camera images. To ensure the images were used I increased the maxWaitTime in the MeasurementTimeline class of lightweight filtering. However, this was when I found that whatever this value is set to is the delay in the output of rovio. Is this behavior intended, as I was expecting it would output as each frame was received. I tried to follow the timing logic in the code, however got lost as to how the safe variables time is progressed.

[bloesch]

sorry for the late reply Zac. if I remember right maxWaitTime describes how long Rovio is waiting for a specific measurement channel before just going on with estimation. So for instance if the current time is 15s (defined by the newest measurement timestamp it received in any channel) and there is one measurement channel where the newest timestamp is 10s, the filter state will only be updated to 10s if the maxWaitTime is infinity: There might be a measurement at 11s for that specific channel, which has just not reached Rovio yet. In essence maxWaitTime should be set to the maximum delay that a measurement may require from its creation to the point it reaches Rovio. This is for ensuring that Rovio does not skip any measurement.

As long as the measurements are fairly quick in reaching Rovio the induced delay should not be a big problem. However, if the delay is too large one can always do a manual IMU based state prediction. This was originally planned to be included in Rovio and I remember someone talking about implementing it but I cannot recall who that was and whether it was implemented on a fork.

[mhkabir]

@ZacharyTaylor @bloesch Also wondering if we can bring the latency fix from maplab upstream. Not sure if this is related to the delay Zac sees. From https://github.com/ethz-asl/maplab/pull/55 :

ROVIOLI - Improve latency of pose estimation

Previous version:

The ROVIO filter update is triggered by the RovioInterface object. We have discovered that the update was triggered unnecessarily late, only after the next camera frame was feeded to ROVIO. For instance, for a framerate of 10Hz it resulted in an additional 100ms delay of the pose output. This behavior can lead to issues if someone wants to directly control an agile robot (e.g. an MAV) using the pose output of ROVIOLI.

Update:

The logic of updating the ROVIO filter has been changed and the updates are processed as soon as they leave the update queue. The filter is no longer waiting for the next image to start processing the previous updates.

Effects:

ROVIOLI pose estimates are published with a smaller latency. This is particularly important for real-time systems and control purposes.

[ZacharyTaylor]

Thanks for clarifying. When running with only one camera and IMU (NPOSE = 0, NCAM = 1) I was expecting that it would output immediately on the image updates as there was no sensor left to wait for. However, I guess there is still the velocity subscriber and other possible sensors in there that it is waiting for.

[bloesch]

This is correct, it may possibly wait for other sensor modalities. This can be avoided by setting the maxWaitTime to zero.

[araujokth]

Hi guys, I was looking at this issue again and was trying different values for maxWaitTime and was wondering if any of you had any insights into the problem. What I am observing is that there seems to be a minimum value of maxWaitTime for which maplab will crash, e.g. maxWaitTime < 0.005 in a desktop computer, and maxWaitTime < 0.05 on an Nvidia Jetson AGX, and the warning and error I get if I set maxWaitTime below this threshold is

W1125 12:15:04.090965  3524 imu-measurements-buffer.cc:63] Too few IMU measurements available between time 1403636617113555499[ns] and 1403636617113555500[ns].
F1125 12:15:04.091037  3524 rovio-localization-handler.cc:249] Check failed: lookup_result != vio_common::PoseLookupBuffer::ResultStatus::kFailed

Does anyone have any insights why this could be happening?

[aseyfi]

Here is my understanding of the root of this problem: When ROVIO is creating the filter, the assumption is that there are three sources for updating the filter. Light weight filter checks if it has received data from all of those sources by getting the number of sources here and then iteratively going through all of them here. If any of the sources is not available, the other sources have to wait maxWaitTime_.

IMHO, a better fix is setting nUpdates_ to the correct value. For example if you are using IMU and image input, then you should set this variable to 1 which means there is only one source for updating the filter (ImageUpdate). If you have ImageUpdate and VelocityUpdate, but not poseUpdate, then you may need to change the order here, so the sources that are valid are listed first or maybe come up with another solution.

Thanks, Ahmad

[araujokth]

Thanks @aseyfi! But isnt rovio already setting Updates to the size it should be according if you have input data from velocityUpdate and poseUpdate actually available? Have you tried to see what is the impact of your suggested change on one of maplab's tutorials, for example on Euroc dataset MH01?

[aseyfi]

@araujokth , (if I understand your question correctly), ROVIO is creating the filter assuming that you will have input data for all of those Updates available, and light weight filter simply reads how many different update types will be available and non of them knows that we have decided not to provide some of those updates. I am even using the default CmakeList file that sets the number of external poses to zero and I still see nUpdates_ values is 3. So it seems the value of this variable is simply based on the number of various Update sources provided when the filter was defined.

I have not tested on MH01, however, I tested it on my private data that includes IMU and Image and I saw significant improvement.

[araujokth]

Ok! I just tried your suggestion to set nUpdates=1 and keep maxWaitTime = 0.1 and I get a larger error on MH01 than the original case. See the plots below where left is with nUpdates=1 and the right is with default nUpdates_=4. I am using the evo tool and the "trajectory_ros" is the error assuming no delay between image input to pose estimate (using the ros timestamp, as its done in maplab by default) and "trajectory_rtc" is using the real maplab time to save the poses.

So it looks like the other "updates" are also being used by the filter, so they must be being computed and fed to it, or? Otherwise the results should have been the same for the trajectory_ros where the delay does not matter.

[aseyfi]

@araujokth Thank for testing this. I didn't not think to go that far. My suggestion was based on printing measMap_.empty() and seeing this waitTime() function being called 3 times fro each single image and only in the first call measMap.empty() was false and for the other two it was true, so I wrongly made the assumption that only the first call was important. You mentioned "default nUpdates=4", did you print the value? Do you know why I see 3 and you see 4?

[araujokth]

For me it printed = 4 when I printed it in the cout in lines 135 and 144 here https://bitbucket.org/bloesch/lightweight_filtering/src/0c8517a5eb19a00a4f8d36dac4ffdc45dac6a363/include/lightweight_filtering/FilterBase.hpp#lines-135. What I had understood was that poseUpdate and velocityUpdate were things that were being computed also from the IMU and image data... but I never really digged into the code to understand this.

Btw I am running the "original" maplab from the march_2018 release branch which is the latest released version https://github.com/ethz-asl/maplab/tree/pre_release_public/march-2018, I am not running bare Rovio.. so there could be some differences.

[aseyfi]

@araujokth , can you send me a link to "evo tool" you are using? I can try it with bare ROVIO that I'm using and if I see the same problem, then we can dig deeper and find another solution.

[araujokth]

ah sorry, here it is https://github.com/MichaelGrupp/evo. Sounds great!

[aseyfi]

@araujokth , I am not familiar with evo tool and the process you used for running this tool, so this is what I did using MH_01_easy.bag: I compiled rovio code without any changes and then ran rovio and recorded a rosbag:

$ rosbag record /rovio/odometry -O update3

$ roslaunch rovio rovio_rosbag_node.launch

Next converted the rosbag to kitti format:

$ evo_traj bag update3.bag /rovio/odometry  --save_as_kitti
$ mv odometry.kitti update3.kitti

Then I modified nUpdates_ to 1 and repeated the same process of generated another kitti file and compared the results:

$ evo_ape kitti  update1.kitti update3.kitti -p --plot_mode=xz
APE w.r.t. translation part (m)
(not aligned)

       max  0.000000
      mean  0.000000
    median  0.000000
       min  0.000000
      rmse  0.000000
       sse  0.000000
       std  0.000000

If you think the way I am generating the kitti files is correct, then I think changing nUpdates_ to 1 has no side effects in bare rovio.