std::out_of_range of FixedLagSmoother methods

[HeXu1]

Hi,
I have test BatchFixedLagSmoother and IncrementalFixedLagSmoother method to fuse imu and lidar data with gtsam::CombinedImuFactor and addExpressionFactor factors.

I have built the latest gtsam from source they all have bug when they start fixedlagsmooth,
the BatchFixedLagSmoother method output is:
terminate called after throwing an instance of 'std::out_of_range'
what(): map::at

and the IncrementalFixedLagSmoother method output is:
terminate called after throwing an instance of 'std::out_of_range'
what(): invalid key

hope for your reply, thanks.

[HeXu1]

and I don't know how to output debug information like #1092
[DEBUG] (2022-02-03 16:13:45.949) OptimizerGTSAM.cpp:101::optimize() Optimizing graph...
[DEBUG] (2022-02-03 16:13:45.949) OptimizerGTSAM.cpp:146::optimize() hasGPSPrior=false
[DEBUG] (2022-02-03 16:13:45.949) OptimizerGTSAM.cpp:164::optimize() fill poses to gtsam...

[RomanStadlhuber]

I just want to chip in and say that I am experiencing the same issue.

Here is a minimal example to reprocude this problem.
While there are existing issues like #1092 where this problem seemingly occurs at random, I have consitently observed this problem to occur whenever I am adding timestamps that exceed the smoother lag.

While my example above was created by piecing together parts of the preintegration and FixedLagSmoother examples respectively, it might be worth mentioning that the FixedLagSmoother example constructs a minimal problem where factors are only added up to the lag time, leaving open the question if there is any further logic that needs to be called after the smoothers window is "full"?

Maybe the author of that example, @svwilliams knows more about that.

Kind regards.

[keevindoherty]

I'm not sure about the original poster, but in the snippet shared by @RomanStadlhuber I think this may be simply a usage error. On line 52 and again on line 70, you have:

    nodeTimestamps[nodeCount] = tCurr;

However, nodeTimestamps which is the KeyTimestampMap passed to the smoother, is a map from keys to timestamps. nodeCount in that gist is being used as the index of several symbols (X, V, and B), but it is not the key representation of those symbols. To get the key, you want something like:

gtsam::Symbol x_n = X(nodeCount);
nodeTimestamps[x_n] = tCurr;
/// ...and so on for V and B

I think the implicit cast from Symbol to Key should "just work." But if it doesn't, you can always do something like x_n.key() or gtsam::Key(x_n).

I believe what was happening in the gist was the following: since nodeCount is an int, C++ was happy to implicitly convert it to a gtsam::Key, which is itself really just a typedef for a uint64_t. Therefore, when trying to look up the keys for the actual variables in your factor graph in the nodeTimestamps map, those keys would not exist (and so the out_of_range error was exactly what one would expect).

[HeXu1]

@keevindoherty thanks, I have tried as your recommend, but it does't work.