Incremental vs Batch FixedLagSmoother

[Joel Truher]

Hi everybody,

I'm making progress on using GTSAM in Java for the FRC 2d localization problem.

I've been using the BatchFixedLagSmoother, using factors for odometry, vision, and gyro, and it works fine in simulation.

This morning I tried swapping in the IncrementalFixedLagSmoother, because I imagined it would be a faster, drop-in replacement.  It did not work well at all.  It produces incorrect, uncertain estimates for awhile, and then aborts with IndeterminantLinearSystemException.  I can make it "work" by supplying the ground-truth pose as the initial value.

Does anyone have advice about the use of these two smoothers?  Is the incremental one supposed to be so sensitive to initial values?  Are there other limitations or best-practices I should be aware of?  Should I just use the batch one?

Thanks!

Joel

[Dellaert, Frank]

Hmmm, would be good to get to the bottom of that. 

One thing you could try when it runs into an indeterminate linear system is to do a batch optimization with Gauss-Newton on exactly the same measurements. If that fails, you have a truly underdetermined system. 

FD

--
You received this message because you are subscribed to the Google Groups "gtsam users" group.
To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
To view this discussion visit https://groups.google.com/d/msgid/gtsam-users/d57f99d0-914d-4219-93f6-1b5534a4d0a2n%40googlegroups.com.

[Joel Truher]

OK, I've dug a little deeper.  It seems that I completely do not understand ISAM2.

If I set up a simple one-shot problem with GenericProjectionFactor, with a few (tight-prior) landmarks and a single (loose-prior) pose, and appropriate pixel measurements, the Gauss Newton solver works fine.

GaussNewtonOptimizer optimizer(graph(), initial());

Values result = optimizer.optimize();

Giving the same problem to ISAM2 yields nothing useful, it just returns the initial values.

ISAM2 isam2;

isam2.update(graph(), initial());

Values result = isam2.calculateBestEstimate();

I must be doing something terribly wrong here.  Can you advise?

[José Luis Blanco-Claraco]

Hi Joel:

iSAM2 is tricky to use :-)

Try to add additional update() calls before getting the estimate:

ISAM2 isam2;
isam2.update(graph(), initial());
for (int i=0;i<3;i++) {
isam2.update();
}
Values result = isam2.calculateBestEstimate();

Or at least one update().
Also, try reducing the relinearization thresholds:

ISAM2Params params;
params.relinearizeThreshold = 0.01;
params.relinearizeSkip = 1;
ISAM2 isam2(params);

Hope it helps...
JL

> To view this discussion visit https://groups.google.com/d/msgid/gtsam-users/119d8799-061d-46ab-a255-92af8cfb633en%40googlegroups.com.



--

/**
* Jose Luis Blanco-Claraco
* Universidad de Almería - Departamento de Ingeniería
* [Homepage]( https://w3.ual.es/~jlblanco/ )
* [GH profile]( https://github.com/jlblancoc )
*/

[Joel Truher]

Oh!  That does indeed make my example work, thank you!

For the larger real case (many poses, many landmarks, many cameras), it still misbehaves in fascinating ways.  I'll come back to it after the batch version is in prod.

Thank you for your help!

[Roman Stadlhuber]

Hi Joel!

A bit late to the party, but I made a short writeup on exactly this issue. Basically the "IncrementalFixedLagSmoother" is a very thin wrapper around iSAM2 and it has this huge pitfall which will easily lock your system in a degenerate state if not properly taken care of at the factor construction level.

When a variable in the smoother leaves the sliding window it is marginalized, but it does something a bit counter-intuitive.

from iSAM2::margnializeLeaves:

> Marginalization leaves a linear approximation of the marginal in the system,

> and the linearization points of any variables involved in this linear marginal become fixed.

> Fixed variables will include any key involved with the marginalized variables in the original factors […]

so basically if your to-be-marginalized variable is connected to any other variables in the system at that point in time, they will become fixed too which very quickly makes them useless for computing jacobians that poperly represent the system state.

I suggest that you try rebuilding factors every tick to remove any constraints pointing to the oldest state that will be marginalized, this is what I did in my system and I guess it's also why projects like LIO-SAM and Kimera-VIO work - because they do the data association itself in a sliding window fashion.

If you are interested, you can read up on my thoughts on the matter here: https://stadlhuber.io/me/blog/using-isam2

Hope that helps to clear things up!

Kind regards,

Roman