Reincorporating marginalized keyframes to optimization window
Related to #69
For the Monado's fork of Basalt we are trying to reintroduce marginalized keyframes into the optimization window at arbitrary points in time. In particular we save the lmdb data and keyframe poses for later reintroduction into the optimization when we see fit. For example, when the current frame is seeing a previously seen view of the room, we would like to recover those marginalized keyframes and their corresponding landmarks and observations so that we can recover their tracks in the current frame. For more context, we are following the STS map idea introduced in this paper: Accurate Visual-Inertial SLAM by Feature Re-identification, see Fig 4. for a quick overview.
After bringing back keyframes, landmarks, and observations and having the ability to recall landmarks in the frontend implemented (see #69). Besides updating the landmark database and keyframe state in the VIO, the only other piece of state we needed to update was the marginalization prior (marg_data), in a way that made sense. My initial approach to this after reading the ICCV21 paper Square Root Marginalization for Sliding-Window Bundle Adjustment was to just add zero rows and columns to the marginalization prior (see the Hbmarg_after image). See the images below showing the different matrices in frame 292 (before reintroducing the keyframes) and frame 293 (after reintroducing them).
- I think the Jr matrices make sense, but do the Hb matrices, Hbmarg (marginalization prior) matrices make sense?
- Is adding zeroes to the Hbmarg marg prior a reasonable approach?
- Do you have any inputs on this problem, any intuition or recommendation on how to approach it?
Thank you!