Open matlabbe opened 1 month ago
To reproduce:
GTSAM/Incremental=true
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
To reproduce:
GTSAM/Incremental=truegdb: