Marginalization fail IncrementalFixedLagSmoother #1638
Description
Description
I have an odometry pipeline that iteratively adds preintegrated IMU factors and unary sensor measurements to an IncrementalFixedLagSmoother. Most of the time, the Bayes tree has a very simple form and marginalization works well:
Usual Bayes tree structure
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( b102 x102 v102 | b101 v101 x101 )
However, very rarely the Bayes tree ends up in a situation, where the key to marginalize is somewhere in the middle with additional frontal variables.
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 . In this case, IncrementalFixedLagSmoother::update does not reorder correctly and marginalization fails for the three keys. The affected row appears again in the Final Bayes Tree. IncrementalFixedLagSmoother::calculateEstimate() will now throw an exception that it cannot access any of the keys b94 v94 x94, because they have been removed from the ValueVector but are still variables of the tree.
This problem seems to be very closely related to #1101, where @gradyrw suggests in a side note that the algorithm may fail to mark x95 v95 b95 affected. I have a fix that additionally marks all frontal variables, e.g., x95 v95 b95 x94 v94 b94, affected. The marginalization seems to work then correctly. However, I am not sure if the problem is not in the underlying ISAM2. To judge this I'm missing expertise.
Steps to reproduce
Unfortunately, I was only able to reproduce this on real data. There it occurs every now and then. I felt like it occurs if a state is only partially observable, but that's only a wild guess. As I don't know how to control the Bayes tree ordering, I cannot create a unit test that ends up with this variable ordering. On a fixed actual dataset, the problem usually occurs at the same key. However, it also seems to be depending on the state of the information matrix and other influences.
If you can give me a link on how I can control the Bayes tree order in a unit test, I can give it a shot. Simply incrementally adding BetweenFactors did not end up in this situation.
Environment
Ubuntu 20.04 5.15.0-83-generic
gtsam develop branch
GIT_REPOSITORY https://github.com/borglab/gtsam.git
GIT_TAG fc5cb3e
C++