Marginalization fail IncrementalFixedLagSmoother

rikba commented 11 months ago

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 https://github.com/borglab/gtsam/issues/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++

sanderscience commented 6 months ago

I am also having this problem. Could you share your fix?

rikba commented 5 months ago

You can find my fix here @sanderscience : https://github.com/borglab/gtsam/pull/1639

It's one line of code but rather a hotfix.

JACKLiuDay commented 4 months ago

Hi, thank you for sharing your work. I am trying to use IncrementalFixedLagSmoother in my project. But I met some error when I run my test rosbag. I set the lag time to be 3.0. And I built gtsam 4.2.0 with code additionalKeys.insert(clique->conditional()->frontals().begin(), clique->conditional()->frontals().end()); in IncrementalFixedLagSmoother.cpp. But my code still have errors. https://github.com/borglab/gtsam/issues/1746

borglab / gtsam