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.

[RomanStadlhuber]

@keevindoherty yes, many thanks!! I can confirm that was in fact it,
I have also had a look at the source code for Kimera-VIO and how they insert keys into the timestamp map, and it is done once for each value.

So instead what I had done previously, and which was incorrect

nodeTimestamps[nodeCount] = tCurr;

I did it like in Kimeras VioBackend.cpp, Line 1163

// insert predicted state into graph nodes
nodes.insert(X(nodeCount), predState.pose());
nodes.insert(V(nodeCount), predState.v());
nodes.insert(B(nodeCount), currBias);
// need one key for each value (that is pose, velocity and bias separately)
for (auto const &val : nodes)
{
    nodeTimestamps[val.key] = tCurr;
}

And now my minimal preintegration example runs through to the end.
@HeXu1 I have updated the code in my gist. Can you confirm it works for you?

Thank you and kind regards.

[HeXu1]

@RomanStadlhuber Yes,it works for your example,thanks a lot.

[HeXu1]

The optimization time is significantly increased: I am fusing lidar and imu, and the optimized fixed lag is set to 0.5 seconds. With the iteration of optimization, the time consumption of state estimation is gradually increasing.
Have you encountered similar problems? Thanks.
@keevindoherty @RomanStadlhuber

[HeXu1]

The IncrementalFixedLagSmoother mode has the situation where the time consumption increases, while the BatchFixedLagSmoother does not.

[RomanStadlhuber]

@HeXu1 hard to tell where this might be coming from, but the IncrementalFixedLagSmoother has a "debug logging" option which you can activate using SETDEBUG("IncrementalFixedLagSmoother update", true) as in this test.

Altenatively, you could play with the relinearization thresholds of iSAM2.

But I suggest building a release build with debug info and attaching a profiler to it to see where most of the computational load is coming from.

[HeXu1]

@RomanStadlhuber ok,thanks. I'm sure the problem comes from the function of “update”.