Reset SLAM if tracking is lost during georeferencing initialization

[zthorson]

Description

This introduces a method to reset SLAM tracking in the case that tracking is lost before georeferencing has been initialized. The exact number of lost frames that trigger this reset can be tuned by a new parameter to avoid false resets.

Reason

On particularily challenging datasets, there have been cases where tracking was lost before the georeferencer fully initialized. When this happens, two problems occur.

1) The frames that are in the initialization buffer are stuck and never written out (unless tracking somehow recovers) 2) VSLAM typically doesn't recover (especially in cases where the loss of tracking was due to poor initialization)

By resetting the SLAM module, we can reinitialize both SLAM and the georeferencer to give another attempt at creating a good map. We don't reset the module if the georeferencer has been initialized, mostly because we want to avoid a reset in cases where we may lose tracking for a while, but regain it during a loop closure event.

Method / Design

• Added parameter init_lost_frames_reset_count to set the number of lost frames that are allowed during georeferencing before a reset is triggered
• Resets are only triggered if georeferencing has not yet been initialized
• The VSLAM reset will trigger code that dumps the init frames to the publish queue to avoid gaps in the map due to failed initialization

Testing

Compiled and run on:

• Ubuntu 20.04 - Desktop
• Custom OpenREALM wrapper (non-ROS)

Other Notes

• Also fixes a bug in reset() where frames were being set to inaccurate pose while still containing the VSLAM world-camera matrices. When they were projected, they would be in visual coordinates, rather than world coordinates. Resetting the camera model pose fixes this.
• Added display of min_nrof_frames_georef to log

[laxnpander]

Hmm, I recently added the reset callback to the openvslam interface here: https://github.com/laxnpander/OpenREALM/blob/684d27b2549ba96a169a43c0b328384b3f1958a5/modules/realm_vslam/realm_vslam_base/src/open_vslam.cpp#L144 Which is registered in the pose estimation stage here: https://github.com/laxnpander/OpenREALM/blob/684d27b2549ba96a169a43c0b328384b3f1958a5/modules/realm_stages/src/pose_estimation.cpp#L56 And triggers the reset https://github.com/laxnpander/OpenREALM/blob/684d27b2549ba96a169a43c0b328384b3f1958a5/modules/realm_stages/src/pose_estimation.cpp#L296

I think this might already do what you are wanting to achieve? Or is there an advantage to your approach?

[zthorson]

@laxnpander

I saw your reset, and the code takes advantage of it (the vslam reset propagates through to the georeferencer through those callbacks).

What this is intended to solve is for very challenging tracking cases. For us, that usually means bare fields, or fields where the growth is still very young. In these cases, we end up relying more on fallback modes than tracking due to the challenging nature of the images.

• The system begins tracking, but does not get accurate tracking to finish georeferencing (either due to scale fluctuations or just no keypoint matching after a given point)
• The system loses tracking, and typically does not regain it. When it loses tracking, it enters fallback mode and starts creating a map (good)
• The frames that are in the initialization section never get cleaned out, since we aren't able to reinitialize tracking for the remainder of the flight. This leaves a small gap in the map.

This is admittedly an edge case mostly having to do with flying over bare fields. However, I have seen it also help in the case where VSLAM settles on a solution that is incorrect during georeferencing (large attitude shifts, bad altitudes, etc). The reset gives it another shot to get a good georeference.

I'm open to other solutions as well, this seemed to work well for my particular edge case, but a more generalized form would be good.

Some examples:

Gap left after initialization failed and was never recovered (all fallback frames):

With reset on loss of frames:

[laxnpander]

@zthorson Ah okay, so this covers the case when tracking is lost but visual SLAM is not triggering reset. Sounds legit. I always had the idea of implementing an „observer“ resetting the SLAM when it is either lost and will likely not recover, or when certain quality metrics indicate a drift / invalid visual pose. But I don’t see myself implementing this in the foreseeable future. So your solution could at least solve a part of the problem. Will test it in the next few days and merge accordingly!

[zthorson]

@laxnpander Yeah, I thought about digging into VSLAM and looking for other metrics to decide when to trigger a reset. However a lack of time and other pressing needs resulting in this temporary solution.

[zthorson]

@laxnpander Just pinging on this pull request to clean up some branches I have on my fork.

[laxnpander]

@zthorson Sorry, took a while.