std::out_of_range error when using GTSAM/Incremental=true and landmarks are detected

[matlabbe]

To reproduce:

• Enable GTSAM/Incremental=true
• Start robot in front of a tag that can be detected, then it will crash some iterations afterwards

gdb:

[ INFO] (2024-10-18 13:17:26.117) Rtabmap.cpp:2699::process() timeProximityBySpaceSearch=0.000132s
[ INFO] (2024-10-18 13:17:26.117) Rtabmap.cpp:2815::process() timeProximityBySpaceVisualDetection=0.000010s
[DEBUG] (2024-10-18 13:17:26.117) Rtabmap.cpp:2820::process() Proximity detection (local loop closure in SPACE with scan matching)
[DEBUG] (2024-10-18 13:17:26.117) Rtabmap.cpp:2823::process() Proximity by scan matching is disabled (RGBD/ProximityPathMaxNeighbors=0).
[ INFO] (2024-10-18 13:17:26.117) Rtabmap.cpp:3005::process() timeProximityBySpaceDetection=0.000030s
[ INFO] (2024-10-18 13:17:26.117) Rtabmap.cpp:3077::process() timeAddLoopClosureLink=0.000010s
[DEBUG] (2024-10-18 13:17:26.117) Rtabmap.cpp:3086::process() hasGlobalLoopClosuresInOdomCache=0
[ INFO] (2024-10-18 13:17:26.117) Rtabmap.cpp:3107::process() Landmark 301 observed again! Seen the first time by node 12.
[DEBUG] (2024-10-18 13:17:26.117) Rtabmap.cpp:3154::process() RGB-D SLAM mode: 1
[DEBUG] (2024-10-18 13:17:26.117) Rtabmap.cpp:3155::process() Incremental: 1
[DEBUG] (2024-10-18 13:17:26.117) Rtabmap.cpp:3156::process() Loop hyp: 0
[DEBUG] (2024-10-18 13:17:26.117) Rtabmap.cpp:3157::process() Last prox: 0
[DEBUG] (2024-10-18 13:17:26.117) Rtabmap.cpp:3158::process() Reduced ids: 0
[DEBUG] (2024-10-18 13:17:26.117) Rtabmap.cpp:3159::process() Has prior: 0 (prior ignored=1)
[DEBUG] (2024-10-18 13:17:26.117) Rtabmap.cpp:3160::process() Has gravity: 0 (sigma=0.300000, odomGravity=0, refined=0)
[DEBUG] (2024-10-18 13:17:26.117) Rtabmap.cpp:3161::process() Has virtual link: 0
[DEBUG] (2024-10-18 13:17:26.117) Rtabmap.cpp:3162::process() Prox Time: 0
[DEBUG] (2024-10-18 13:17:26.117) Rtabmap.cpp:3163::process() Landmarks: 1
[DEBUG] (2024-10-18 13:17:26.117) Rtabmap.cpp:3164::process() Retrieved: 0
[DEBUG] (2024-10-18 13:17:26.117) Rtabmap.cpp:3165::process() Not self ref links: 1
[ INFO] (2024-10-18 13:17:26.117) Rtabmap.cpp:3746::process() Update map correction
[ INFO] (2024-10-18 13:17:26.117) Rtabmap.cpp:5131::optimizeCurrentMap() Optimize map: around location 14 (lookInDatabase=false)
[ INFO] (2024-10-18 13:17:26.117) Rtabmap.cpp:5140::optimizeCurrentMap() get 4 ids time 0.000006 s
[DEBUG] (2024-10-18 13:17:26.117) Rtabmap.cpp:5181::optimizeGraph() ids=4
[DEBUG] (2024-10-18 13:17:26.117) Memory.cpp:6427::getMetricConstraints() 
[ INFO] (2024-10-18 13:17:26.117) Rtabmap.cpp:5183::optimizeGraph() get constraints (ids=4, 5 poses, 6 edges) time 0.000054 s
[DEBUG] (2024-10-18 13:17:26.117) Rtabmap.cpp:5228::optimizeGraph() recompute poses using only links (robust to multi-session)
[DEBUG] (2024-10-18 13:17:26.117) Optimizer.cpp:195::getConnectedGraph() IN: fromId=1 poses=5 links=6 priorsIgnored=1 landmarksIgnored=0
[DEBUG] (2024-10-18 13:17:26.117) Optimizer.cpp:294::getConnectedGraph() OUT: poses=5 links=6
[DEBUG] (2024-10-18 13:17:26.117) OptimizerGTSAM.cpp:160::optimize() Optimizing graph...
[DEBUG] (2024-10-18 13:17:26.117) OptimizerGTSAM.cpp:254::optimize() Add new poses...
[DEBUG] (2024-10-18 13:17:26.117) OptimizerGTSAM.cpp:261::optimize() Adding pose 14 to factor graph
[DEBUG] (2024-10-18 13:17:26.117) OptimizerGTSAM.cpp:264::optimize() Add new links...
[DEBUG] (2024-10-18 13:17:26.117) OptimizerGTSAM.cpp:272::optimize() Adding constraint -301 (-301->14) to factor graph
[DEBUG] (2024-10-18 13:17:26.117) OptimizerGTSAM.cpp:272::optimize() Adding constraint 13 (13->14) to factor graph
[DEBUG] (2024-10-18 13:17:26.117) OptimizerGTSAM.cpp:278::optimize() Remove links...
[DEBUG] (2024-10-18 13:17:26.117) OptimizerGTSAM.cpp:308::optimize() fill poses to gtsam... rootId=1 (priorsIgnored=1 landmarksIgnored=0)
[DEBUG] (2024-10-18 13:17:26.117) OptimizerGTSAM.cpp:372::optimize() fill edges to gtsam...
terminate called after throwing an instance of 'std::out_of_range'
  what():  map::at
--Type <RET> for more, q to quit, c to continue without paging--

Thread 7 "nodelet" received signal SIGABRT, Aborted.
[Switching to Thread 0x7ffff19fd700 (LWP 1166291)]
__GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
50      ../sysdeps/unix/sysv/linux/raise.c: No such file or directory.
(gdb) bt
#0  __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
#1  0x00007ffff7887859 in __GI_abort () at abort.c:79
#2  0x00007ffff7b108d1 in  () at /lib/x86_64-linux-gnu/libstdc++.so.6
#3  0x00007ffff7b1c37c in  () at /lib/x86_64-linux-gnu/libstdc++.so.6
#4  0x00007ffff7b1c3e7 in  () at /lib/x86_64-linux-gnu/libstdc++.so.6
#5  0x00007ffff7b1c699 in  () at /lib/x86_64-linux-gnu/libstdc++.so.6
#6  0x00007ffff7b1333c in std::__throw_out_of_range(char const*) ()
    at /lib/x86_64-linux-gnu/libstdc++.so.6
#7  0x00007fffd20f8446 in  ()
    at /home/mathieu/workspace/rtabmap/build/bin/librtabmap_core.so.0.21
#8  0x00007fffd2127cc5 in rtabmap::OptimizerGTSAM::optimize(int, std::map<int, rtabmap::Transform, std::less<int>, std::allocator<std::pair<int const, rtabmap::Transform> > > const&, std::multimap<int, rtabmap::Link, std::less<int>, std::allocator<std::pair<int const, rtabmap::Link> > > const&, cv::Mat&, std::__cxx11::list<std::map<int, rtabmap::Transform, std::less<int>, std::allocator<std::pair<int const, rtabmap::Transform> > >, std::allocator<std::map<int, rtabmap::Transform, std::less<int>, std::allocator<std::pair<int const, rtabmap::Transform> > > > >*, double*, int*) ()
    at /home/mathieu/workspace/rtabmap/build/bin/librtabmap_core.so.0.21
#9  0x00007fffd1d919ec in rtabmap::Rtabmap::optimizeGraph(int, std::set<int, std::less<int>, std::allocator<int> > const&, std::map<int, rtabmap::Transform, std::less<int>, std::allocator<std::pair<int const, rtabmap::Transform> > > const&, bool, cv::Mat&, std::multimap<int, rtabmap::Link, std::less<int>, std::allocato