Sliding window optimizer

[jakemclaughlin6]

I am interested in implementing an optimizer that works similarly to the fixed lag smoother however rather than marginalizing out variables that are outside of the time window, it'll instead keep a fixed # of the most recent "states". States in this case would any variables associated to the set of unique timestamps in the graph. This model is specifically useful in the case of visual odometry since new states arent necessarily created using time but motion so two states could be arbitrarily far apart temporally if there hasn't been much motion. I am wondering if this optimizer would be something that could be merged into the main devel branch?

[svwilliams]

I'll happily include another optimizer implementation, assuming it can be implemented in a reasonably generic way. Also you can release your optimizer implementation in your own public repo. I could add a section to the documentation somewhere pointing to external packages that may be of interest to others. Either way works for me.

That said, your proposed "sliding window" optimizer should be able to share a bunch of code with the existing fixed-lag smoother. In fact, I think "all" you need to do is implement different "variables to marginalize" selection logic here: https://github.com/locusrobotics/fuse/blob/devel/fuse_optimizers/src/fixed_lag_smoother.cpp#L240-L245

Unfortunately, I didn't setup the fixed-lag smoother to be extended nicely. I'll be happy to work with you to refactor that implementation to make deriving different "windowed" smoothers easier. Perhaps there should be a base class that handles all the tedious logic for windowed smoothers. And then both the fixed-lag and sliding-window implementations can inherit from that. All they need to do is select which variables need to be removed at each timestep.

Let me know if you think that makes sense, and I can take an initial stab at it next weekend.

[jakemclaughlin6]

Thats exactly what I was thinking, I can start to implement it this week. Something like a WindowedSmoother base class, with FixedLagSmoother and SlidingWindowSmoother subclasses

[svwilliams]

I have a ton of cleanup, testing, documentation, and minor tweaks to make. But this branch contains the basic idea we discussed above.
https://github.com/locusrobotics/fuse/tree/issue258-windowed-optimizer-base-class

Hopefully that will give you something to work from until I have time to finish it up. And let me know if you run into any issues with the interface. There's still plenty of time to change things up if something is missing.

[jakemclaughlin6]

I've made an initial implementation of the fixed size smoother in. Most of the actual capability ended up being implemented in the variable stamp index, I've opened a PR here: #261