Open rikba opened 11 months ago
I am also having this problem. Could you share your fix?
You can find my fix here @sanderscience : https://github.com/borglab/gtsam/pull/1639
It's one line of code but rather a hotfix.
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
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:
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.
Note the row
P( x95 v95 b95 x94 v94 b94 | b97 v97 x97 )
which contains the marginalizable keysb94 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 keysb94 v94 x94
, because they have been removed from theValueVector
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++