Problem with iSAM2 + stereo smart factors (no IMU)

[José Luis Blanco-Claraco]

Hi all,

I've been playing around with your great smart factors for stereo observations and created a new example for stereo factors only (without IMU), based on the existing ISAM2_SmartFactorStereo_IMU.

It's on a new branch on my fork here, together with a new TXT data file from a simulator in a 2D world for 20 frames.

All good... except that when running iSAM2, it always complains about the problem being ill-posed (!). Despite the data points are all stereo (no parallax problem) and there are plenty of them, well-distributed, etc. In fact, a batch optimization with Lev. Marq. (even with the same smart factor classes) seems to work fine, so the issue seems to me to lie only on something that iSAM2 does.

If I understood iSAM2 instructions, the "new_graph" and "new_initialvalues" must be cleared for each iteration, and so I did and found the exception on ill-posed problem.

Interestingly, if one removes the "graph.clear()" call, in fact, NOT removing the factors from one iteration to the next one, the numerical problem goes away. (Although it seems the numerical results are not 100% accurate, must check it later)

So my question is: Should we still honor the instructions to "clear the factor graph container for each iteration" when working with smart factors?

If the answer is "yes", then I think there is a bug, and you can test it by checking out my branch and commenting out this line, which will raise the linearization problem.

If the answer is "no", any help regarding how should we remove specific (non-smart?) factors from the "new_graph" container would be appreciated!

PS: I loved you ICRA paper on smart factors!

[Frank Dellaert]

Thanks for the kind words :-)

You definitely need to clear. In fact, I'm not super-happy about this imperative style example, anyway: you should ideally just create a new graph every time. but, I remember another issue about smart factors, so maybe there is a big and this is another surfacing of this issue. I'd recommend two things:

1) search for the other issue 2) Check what happens if you add exactly the same factors to a full factor graph?

Frank

[José Luis Blanco-Claraco]

Thanks for the feedback!

1) I'll search for the other issue ticket...

2) If the same factors are added to a single graph and it's optimized in a batch mode, it seems to work, at least for this toy example. In my real application it eventually also degenerates to an ill-pose problem, but that might be a problem on my side, that's the reason of starting debugging it from the toy problem first.

[Frank Dellaert]

In principle, iSAM, iSAM2, and a batch algorithm should yield exactly the same solution and covariances. iSAM variants have built-in support to query covariances, you need to use Marginals for batch. If I was to debug this, I'd run them side by side and figure out when they diverge, in response to which update step, then dig deeper as to why. If there is a bug, we want to find and fix it ! :-)

[Frank Dellaert]

Note: the issue could be with the iSAM-smart-factor combination. Since you recently re-read the paper, you're probably in a better position to speculate than me at this point...

[José Luis Blanco-Claraco]

Cool! Sure, I want to get this fixed too... :-) Will start debugging next week, probably.

By the way: the Doxygen docs mention that for ill-pose problems, it may be useful to dump the Jacobians for inspection in MATLAB, but I couldn't figure out how to do it with iSAM2... any help would be really appreciated! So far, I guessed that a NonLinearFactorGraph should be linearized to get a GaussianFactorGraph, then probably one should call its methods to get the sparse Jacobian representation? Is that correct? Is there any helper function to write Jacobians to a format that MATLAB can easily read?

PS: Thanks again!

[José Luis Blanco-Claraco]

Hi Frank,

In principle, iSAM, iSAM2, and a batch algorithm should yield exactly the same solution and covariances.

Just a quick update on this issue: a ground-truth path file was added and the test was updated to run either iSAM or a batch optimizer.

Batch optimizer recovers the cameras poses perfectly, apparently, up to machine precision. Yay! So stereo smart factors work perfectly at least in that case.

So it seems the issue is in combination with iSAM2, as you suggested. Will continue debugging.

[José Luis Blanco-Claraco]

Hi all,

I found what I think is the/a source of problems of smart factors + iSAM2. iSAM2 has a member VariableIndex variableIndex_; which is .augment()ed at each update() with the incoming newFactors... I'm sure you already see the problem :-) => a smart factor may be created involving only one key (e.g. x0), then added to iSAM2, and in a subsequent time step, extended to also include another key (e.g. x1). But the latter is done via a method of the smart factor itself, iSAM2 knows nothing about the new variable, so when the time comes for:

  KeySet candidates = getAffectedFactors(affectedKeys);

with affectedKeys being the new keys (e.g. x1), none of the factors gets selected.

I can think of some solutions, but wanted to ask here first what's the preferred solution:

• Add a new optional parameter to isam2::update() to explicitly enumerate the smart factors that have changed (and possibly define a "set of changes" struct inside smart factors for iSAM2 to check it)
• Create some sort of "callback" in iSAM2 which can be called from the smart factor whenever its keyset is extended... perhaps this one is dangerous for potential dangling references, multi-threading, etc... I'm actually stepping back from this one as I'm writing it down :-)
• Any other idea?

Cheers

[Frank Dellaert]

Aha ! Yep, that makes a lot of sense :-) I like your first solution, because it also allows us to document this case in the update method for future uses. However, I think it is more appropriate to pass in a KeySet of keys, to be merged with the affectedKeys set, because otherwise we have to think how to talk about a particular factor. And in any case, it's the affected keys we are interested in anyway. Will you propose a PR? Would be amazing if there was a tiny test that fails before and succeeds after :-)

[José Luis Blanco-Claraco]

Sure! Thanks for the quick feedback.

[José Luis Blanco-Claraco]

Hi, Update on this bug:

Here is a branch with a new small unit test that reveals this problem, plus an initial attempt to fix it. Can be tested with make testISAM2SmartFactor && tests/testISAM2SmartFactor.

It also runs a batch solver to double-check the smart-factors Jacobians themselves (that part passes ok).

On this:

However, I think it is more appropriate to pass in a KeySet of keys, to be merged with the affectedKeys set, because otherwise we have to think how to talk about a particular factor.

After some digging, it seems that we would need to talk about factors (which is a bit cumbersome from the user's side, but doable), since we need to update the VariableIndex which maps factor indices to Keys... At least, I think it's necessary.

Another restriction is that we cannot enable linearized-factor caching when using Factors with a dynamic number of Keys (e.g. Smart Factors, but possibly others), hence the new check and throw to detect that situation. An alternative would be to remove that strong restriction, and parse the new list of "changed factors" and automatically add them to the list of "re-linearize"... what do you think? (iSAM2 is so large, it's easy to miss important points!)

At present, iSAM2 now runs without the indeterminate linear problem... but the Jacobians must be incorrect, since the overall error does not decrease. Will continue investigating, but please, if time permit, take a look at each of these commits, since any feedback will be greatly appreciated! :-)

[Frank Dellaert]

Sorry, very busy. I started looking at your commits but could not finish. One things is that we definitely want to be backwards compatible on update, including in wrappers. In theory, even smart-factors know which keys they are connected to, so I am not sure why simply giving the keys is not an option: given the keys of poses that have new measurements associated with them (even if point is implicit), can't we find those factors that should be re-linearized?

[Frank Dellaert]

@jlblancoc did we ever reach agreement on this? Esp. backwards compatible and “In theory, even smart-factors know which keys they are connected to, so I am not sure why simply giving the keys is not an option.”

[Frank Dellaert]

@jlblancoc we are moving to Github this weekend. I created a new issue on github linking back here: https://github.com/borglab/gtsam/issues/10

[Frank Dellaert]

Re-opened on github: https://github.com/borglab/gtsam/issues/10