Development v2.1 - Secondary Pose Graph Support

[goldbattle]

Key Changes:

• Introduce saving logic to allow for publishing of marginalized feature information
• Now all triangulated features are temporarily stored till they are lost completely
• When the last clone is marginalized from the sliding window, all features seen from that time are published. Their best 3d position in the global and uv measurement information is published.
• An example repository ov_secondary has been created to show how one could leverage this information to have a secondary pose graph bound drift through relative pose loop closures.

Secondary Pose Graph (see ov_secondary):

• This example is based on the VINS-Fusion secondary local pose graph thread.
• The code has been modified to allow for subscribing to intrinsic camera parameter topics
• Additional parameters have been exposed with some example results.
• Please see the ov_secondary repository readme for more details on this.

Discussion of Impact of Pose Graph:

While the secondary pose graph works well in some cases, in many other cases it fails to improve performance or even hurt it. For example on the EurocMav dataset there isn't a clear winner in terms of ATE/RPE accuracy of the OpenVINS odometry poses being fed into the loop closure module and its published states. On these small room datasets, many loop closure candidates are rejected, and thus there are maybe 2-4 loop closures, with V1_02_medium dataset having none over the whole trajectory in most cases. Also to ensure there are enough points for PnP, the number of tracked features in ov_msckf needed to be increased to around 300.

On the other hand, there are cases where the loop closure clearly helps. For example, on a long dataset such as the corridor1 from the TUM-VI dataset the system running in monocular mode drifts towards the end of the dataset (blue). The loop closure (red) is able to correct this and ensure that the ending pose is correct relative to the start (the sensor system returns to the same location in the bottom left of this trajectory).

Looking at some more quantitative results on the TUM-VI dataset, a few runs on the room datasets can clearly show the advantage of loop closure. These room datasets are limited to the same vicon room environment, and get very frequent loop closures due to this. From the below table, it is very clear that using the secondary loop closure thread in most cases has a clear performance gain as compared to the standard odometry.

[WoosikLee2510]

This is just publishing additional historical keyframes and the corresponding features. So no major change in OpenVins main algorithm.
The name 'keyframe' is not my taste, because we use the term often what's in our state while the keyframe here is just a copied value of historical clones and features.