Objective modification for Theseus factor graph implementation

[pculbertson]

First, want to say thanks so much for building + sharing this package, I think it's a great community resource.

I am currently working on building a factor graph-based state estimation pipeline in Theseus, and was hoping to perform incremental re-solves as more measurements come in (i.e., to replicate GTSAM's incremental workflow in Theseus).

A key feature I need is to update the set of decision variables / costs online (i.e., drop old states, add new measurements). Naively my plan for this was to just modify the Objective in-place using erase(), and to add() costs for new states, but this requires me to re-initialize the optimizer/linearization for each solve, which is super costly.

Any thoughts on how to structure my project to get around rebuilding the optimizer at each time?

[mhmukadam]

Hi @pculbertson, thank you for the kind words!

If your graph structure and factors remain the same between steps then you would only need to shift the values within a sliding window (fixed lag style) without needed to erase/add (you would essentially avoid redoing the linearization part). This won't work if you get different factors/measurements at different steps and so the graph looks different between steps.

The longer route would be bring in support for variable re-elimination taking inspiration from GTSAM. This would we be a huge undertaking which unfortunately our team would not be able to prioritize right now. We welcome community contributions though and iSAM based on Givens Rotation could potentially be made PyTorch compatible. I am not sure if iSAM2 is possible.

[pculbertson]

Thanks @mhmukadam for the great answer -- I didn't realize you were computing the linearization's sparsity structure up top basically, but that makes sense.

I'll see if we can get away with a fixed graph structure, and otherwise might look into implementing an incremental update scheme for the linearization like Givens rotations. Thanks!