ISAM2 marginalizeLeaves

[jczarnowski]

Hi,

I am working on a dense, keyframe based system using ISAM2 for optimizing the graph. Currently I am trying to implement marginalization of certain frames in the graph, but I've run into some trouble.

My graph consists of keyframe poses (p0...pn), keyframe codes (c0...cn) that represent a dense depth map and auxiliary poses (a0...am) of throw-away frames that are used to optimise keyframe depth (via photometric error) and that are to be marginalised. Here's an example of what my graph looks like:

The current strategy is using the constrainKeys parameter of ISAM2::update to get an ordering that will push the auxiliary pose cliques to be children of the root. For that, variables (p_i, c_i) for i = 1:N-1 are in constraint group 0, auxiliary poses are in constraint group 1 and the last key frame (p_N, c_N) is in group 2.

This indeed causes all auxiliary poses to become children of the root in the Bayes tree, which will be the last key frame variables:

After that I'm using marginalizeLeaves to marginalise out variables a0 ... am, which works fine most of the time but happens to fail in certain situations. I've debugged the function and it seems the failure occurs when the marginalisation does not start from the root clique, i.e. starting with "a44" in the tree shown above. The algorithm then proceeds up the tree to find the root of the marginalised sub-tree and decides to split the clique, but then forgets to remove their information from the variableIndex and possibly other things. The rest of cliques (other leaves) are being marginalised properly.

There are few workarounds that can be applied here: I can always marginalise a single frame (works) or enforce ordering within the frames so that the first to be marginalised will always reside in the root (with this I encounter another bug within the ordering code).

I wanted to ask you whether what I'm trying to do makes sense and is within normal use of marginalizeLeaves function and what I experience are just bugs or am I trying to do something completely silly here?

I will be grateful for any information!

Jan

[Frank Dellaert]

Hmmm. It is very possible that there is a bug in marginalizeLeaves. You are in a unique position now in which to isolate a (as simply as possible) case in a unit test that fails :-) then if you find a fix, submit a PR? Really, you are the expert now!!!!

[Frank Dellaert]

@jczarnowski I will close this now as "wontfix" but if you have an update or a PR we'd be grateful!

[jczarnowski]

@dellaert Ah, sorry for the delay, I've been pushing this for ICRA and worked around the problem temporarily by always having only one throw-away frame and marginalizing it out ASAP. This approach does not trigger the bug as there is never more than 1 frame in the system.

I suspected it was a bug but wanted to quickly pop in and check if anyone had any issues with this part of the code and maybe I would get lucky and get a comment.

Unfortunately I will be gone now for an internship for a couple of months and the NDA will not let me pursue this, but I will be continuing the project afterwards as part of my PhD and will find a proper fix as the temporary one isn't up to standard.

Thanks! Jan