Mark current clique affected.

[rikba]

This PR fixes the issue where the IncrementalFixedLagSmoother fails to marginalize a key that is located in the middle of the tree as described here: #1638

I'm open for suggestions on how to write a unit test for this problem.

Please also let me know, if

• marking the marginalized variable itself affected is potentially dangerous or
• this PR is just covering up a problem in ISAM2.

[dellaert]

Just coming up for water :-)
As for a unit test: make a drawing of the simplest situation where you think this will happen, and make iSAM2 fail? The drawing will help us understand and then we can also better asses whether iSAM2 should handle it...

[rikba]

Hello! As described in #1638 the issue seems to occur when the states to marginalize occur not at the front of the frontal variables. For example:

Current Timestamp: 1695052916.172259092
Marginalizable Keys: b94 v94 x94 
Constrained Keys: b94(0)  b95(1)  b96(1)  b97(1)  b98(1)  b99(1)  b100(1)  b101(1)  b102(1)  b103(1)  v94(0)  v95(1)  v96(1)  v97(1)  v98(1)  v99(1)  v100(1)  v101(1)  v102(1)  v103(1)  x94(0)  x95(1)  x96(1)  x97(1)  x98(1)  x99(1)  x100(1)  x101(1)  x102(1)  x103(1)  
Bayes Tree After Update, Before Marginalization:
P( x98 v98 b98 x97 v97 b97 )
 P( x99 v99 b99 | b98 v98 x98 )
  P( x100 v100 b100 | b99 v99 x99 )
   P( x101 v101 b101 | b100 v100 x100 )
    P( x102 v102 b102 | b101 v101 x101 )
     P( b103 x103 v103 | b102 v102 x102 )
 P( x95 v95 b95 x94 v94 b94 | b97 v97 x97 )
  P( b96 x96 v96 | b95 b97 v95 v97 x95 x97 )
END
Final Bayes Tree:
P( x98 v98 b98 x97 v97 b97 )
 P( x99 v99 b99 | b98 v98 x98 )
  P( x100 v100 b100 | b99 v99 x99 )
   P( x101 v101 b101 | b100 v100 x100 )
    P( x102 v102 b102 | b101 v101 x101 )
     P( b103 x103 v103 | b102 v102 x102 )
 P( x95 v95 b95 x94 v94 b94 | b97 v97 x97 )
  P( b96 x96 v96 | b95 b97 v95 v97 x95 x97 )
END

Note the row P( x95 v95 b95 x94 v94 b94 | b97 v97 x97 ) which contains the marginalizable keys b94 v94 x94 and does not get marginalized in the final bayes tree.

It would be great if you can give me a hint/example how I can reproduce this tree for a unit test.

Since I used this fix I never had marginalization key issues anymore on my odometry setup.