ISAM2 marginalization fixes

[gradyrw]

Fixes #1101

This PR addresses 3 separate problems with the ISAM2 marginalization.

1. The first issue is that a missing null pointer check would cause a segfault if a root was attempted to be marginalized. A test was added in Adding failing tests for ISAM2 marginalization #1105 ( TEST(ISAM2, MarginalizeRoot) ) which triggers this behavior. The fix added here is to add a null pointer check before the addition of the marginal factor. This works since, if we are marginalizing a root, we don't need to keep track of the resulting marginal factor as the entire tree is discarded. The new test passes with the fix included here.
2. The second issue is that the clique conditional's block matrix was being incorrectly updated. When we remove variables from a clique, the column start of the vertical block matrix needs to be incremented by the number of variables removed (it was being set equal to the number of variables instead). Triggering this behavior can be difficult: you need to repeatedly marginalize variables from the same clique, but not remove the entire clique. A test was added in Adding failing tests for ISAM2 marginalization #1105 (TEST(ISAM2, marginalizeLeaves6) which manages to trigger this behavior. That test now passes with these fixes.
3. The last issue is that marginalizeLeaves ignores the findUnusedFactorSlots parameter, and always pushes new marginal factors onto the back of the graph instead of inserting them into unused slots. This was fixed by replacing the call to push_back with a call to add_factors which takes into account the findUnusedFactorSlots param, and then using the newFactorIndices returned by that call to insert new factors into the graph. Another related change is removing factors before inserting the new marginal factors into the graph. Together, these changes prevent the graph from growing when variables are marginalized. A new test which failed previously (and now passes) had been added in this PR (TEST(ISAM2, marginalizationSize)

[dellaert]

Amazingly detailed issue, and great PR. It’s been a while since we wrote this code, so I am not super confident just from reading the code. Is there a way to make the test cases you highlighted in the issue into additional unit tests, or you can you provide context/evidence in the PR comments that those 3 issues indeed have been fixed?

[gradyrw]

Amazingly detailed issue, and great PR. It’s been a while since we wrote this code, so I am not super confident just from reading the code. Is there a way to make the test cases you highlighted in the issue into additional unit tests, or you can you provide context/evidence in the PR comments that those 3 issues indeed have been fixed?

The two failing tests that were added previously now pass, as does the third test added in this PR. If we're still unsure then it might be a good idea to do a more extensive refactor, breaking down this function into smaller components and adding unit tests for each of them.

[mhkabir]

@gradyrw I believe that the markAffectedKeys function in IncrementalFixedLagSmoother also suffers from the same issue as your implementation initially did - should that also be fixed in this PR?

[dellaert]

@gradyrw I believe that the markAffectedKeys function in IncrementalFixedLagSmoother also suffers from the same issue as your implementation initially did - should that also be fixed in this PR?

I think it should be a separate PR. I'm sorry I lost track of this PR, I will merge it now.

[dellaert]

Thanks @gradyrw !!!!!