Hessian factor for oldest state marginalization in a sliding-window LM approach.

[jan m]

Hello,

I use GTSAM for iteratively solving a LM problem in a sliding window fashion. 
In this setting I need to marginalize out the oldest pose which is about to drop out of the sliding window and use it to build a new prior. I am faced with a problem of how to do it in GTSAM. Namely, according to the theory I shall build a Gauss-Newton system involving the states to be marginalized out and the states connected to the states to be marginalized out. Then, using the Schur Complement operation I can obtain the new Gradient and Hessian (corresponding to the information matrix with the marginalized states' information absorbed in them).

Now, I wonder how to realize this in the GTSAM API. Namely, what I do is - after solving one window I take the solution and linearize the solved graph about it into the Hessian factor.
This operation gives me the Hessian, Gradient and Constant terms. Now I can perform the Schur Complement operation on the Hessian and Gradient in order to obtain the reduced Gauss-Newton system which is independent of the marginalized states.

Now we come to the problematic part - namely, how do I use this newly obtained Hessian and Gradient (which contain the information from the marginalized states) to build the new prior? I tried to use the obtained matrices to build a `HessianFactor` and place it in the graph, nevertheless I obtain errors saying that the graph is a `NonlinearFactorGraph` and hence cannot accept the `HessianFactor` as a prior. Is there a way to use these obtained matrices to build a `PriorFactor`? If so, how to do it?

I believe there is a simple way to perform this operation in GTSAM, yet I am unable to find it - any information and help from more expereinced users is extremely welcome!

Thanks you,
Jan

[Amjad Hussain]

Hi Jan

Thanks for your message. I am also stuck with this. If someone has done sliding windows with this technique, it can help.

Can I ask a favor? How did you get up to the Hessian matrix? Can you share your code?

Thanks in advance.

Regards, 

Amjad

[Matías Mattamala]

Hi Jan and Amjad,

You might be interested in taking a look at the gtsam/nonlinear/BatchFixedLagSmoother as well as  gtsam_unstable/nonlinear/IncrementalFixedLagSmoother files. Here there is also an example.

You'll find they implement the logic you described, but as specialized classes that integrate the optimizer and the factor graph data structures instead of the standard API from the tutorials, in order to handle all the marginalization procedures.

Good luck!

Matias

[Amjad Hussain]

Hi there, 

I am employing two factors in my system: an IMU (Inertial Measurement Unit) factor defined as a BetweenFactor, and GNSS (Global Navigation Satellite System) factors defined as PriorFactors. I am attempting to implement a sliding window technique using a Levenberg-Marquardt (LM) optimizer. While batch optimization works correctly, I am encountering issues with window-based optimization. Could someone provide assistance or share code examples demonstrating how to achieve window-based optimization with these two factors and using LM as the optimizer? Any code snippets, samples, or examples would be highly appreciated. Thank you.

[Amjad Hussain]

Hi Jan

I'm utilizing the Levenberg-Marquardt (LM) optimizer for a sliding window-based approach. Once I attain an optimized solution, I apply it to linearize the graph using the following code: 'linearizedGraph = optimizer.linearize(parameters, initialEstimate);'. Subsequently, the next step involves performing the Schur Complement to obtain the Reduced Hessian and Gradient. However, during this process, an error is encountered, indicating that the 'linearizeGraph' method is unrecognized for the class 'gtsam.LevenbergMarquardtOptimizer' in Matlab. I would appreciate any guidance or assistance in resolving this issue.

Thanks