(iSAM2) GTSAM exception caught: Requested the BayesTree clique for a key that is not in the BayesTree

[matlabbe]

Issue introduced by #1249, with GTSAM/Incremental=True, we could get:

[DEBUG] (2024-03-28 12:10:09.623) OptimizerGTSAM.cpp:928::optimize() Computing marginals for node 2702...
[ERROR] (2024-03-28 12:10:09.188) OptimizerGTSAM.cpp:975::optimize() GTSAM exception caught: Requested the BayesTree clique for a key that is not in the BayesTree

that would eventually result in:

[DEBUG] (2024-03-28 12:10:10.048) OptimizerGTSAM.cpp:770::optimize() Update iSAM with the new factors
terminate called after throwing an instance of 'gtsam::ValuesKeyDoesNotExist'
  what():  Attempting to at the key "2702", which does not exist in the Values.

Signal 6 caught...
Aborted (core dumped)

From the log, what happens is after triggering a new map, there was a loop closure detected from the new map to old one (e..g, 2659->2702), which eventually get rejected. The robot was not moving, after 2705 got merged (rehearsal) to 2072, another constraint has been added 2659->2702 to input, but not added to factor graph because the nodes already exist. The missing link then caused the first error. For the second error, it happened when a new node not merged to 2072 has been added. I think the errors are related but 2072 should have been already in the values.

full log: log.txt

[matlabbe]

Workaround is to set Mem/RehearsalSimilarity=1. That makes memory management not compatible with GTSAM/Incremental. The cleanest way to solve this issue would be to be able to remove links from external call instead of trying to detect which links/poses have been changed internally.