[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... rootId=1 (priorsIgnored=1 landmarksIgnored=0)
[DEBUG] (2022-02-03 16:13:45.949) OptimizerGTSAM.cpp:223::optimize() fill edges to gtsam...
[DEBUG] (2022-02-03 16:13:45.953) OptimizerGTSAM.cpp:479::optimize() create optimizer
[DEBUG] (2022-02-03 16:13:45.955) OptimizerGTSAM.cpp:504::optimize() GTSAM optimizing begin (max iterations=20, robust=0)
[DEBUG] (2022-02-03 16:13:45.965) OptimizerGTSAM.cpp:584::optimize() iteration 1 error =2850609649897726489254101293091235944356044801931291559347145600874420166397741503058701851780329932591817167121596483299665242156038849992125986899196008202587784806682300342868079360100003408894353952842967492154479751175590697972670210540378883501996026547213724436053723262923791400960.000000
[DEBUG] (2022-02-03 16:13:45.976) OptimizerGTSAM.cpp:584::optimize() iteration 2 error =5016100078967479096064413010768809019148602854242979119938905283661092705639338472477011700353588127562436548176456729989570331508193090845168052534407557742985955853652363314364564935033288790078552428268421593627697933614414369113679293174995444909290919633032349218707196942858868340438922690560.000000
[DEBUG] (2022-02-03 16:13:45.976) OptimizerGTSAM.cpp:590::optimize() Negative improvement?! Ignore and continue optimizing... (-5016100076116869425110442501786949926674733499886072217178394925254059757313327868568474126116375716917423374279035923904149843802861944430925552915508911836392252585395110302645738006052198719011840191255833450124487785933472970703237412918514014549742862808981949781291961016050608007111191298048.000000 < 0.000010)
[DEBUG] (2022-02-03 16:13:45.986) OptimizerGTSAM.cpp:584::optimize() iteration 3 error =4762437314404316677678978362215431360901508662362565105929317273411226380242999817166248601054457756356314417395982789444608445999853387397856784778582078705214311653018871259333382528033412543819451588388981733574931330715497184027876694693487667040715851249105737640310706930901554561024.000000
[DEBUG] (2022-02-03 16:13:45.996) OptimizerGTSAM.cpp:584::optimize() iteration 4 error =10375071687008403818694769841431843491355929407087161691691665919401692325307968802150380172489645670836245273758214944374368613158103528730104116996642719655751021310965017658033602275517442857849603218646021211197352441906817076859211969627919396267107659523192607703809860802465813757952000.000000
[DEBUG] (2022-02-03 16:13:45.996) OptimizerGTSAM.cpp:590::optimize() Negative improvement?! Ignore and continue optimizing... (-10370309249693999211514267725940937073799608250251315401149818182777489539027667437073554994776271686411532953788783224558210290659927700369449925412051392869421429210283080719775332002144501721930346975002849411698388125145667610495033111088655127153698549802735347982290158922839559774404608.000000 < 0.000010)
terminate called after throwing an instance of 'std::out_of_range'
  what():  map::at

Thread 1 "rtabmap-reproce" received signal SIGABRT, Aborted.
__GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:51
51	../sysdeps/unix/sysv/linux/raise.c: No such file or directory.
(gdb) bt
#0  0x00007fff273fefb7 in __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:51
#1  0x00007fff27400921 in __GI_abort () at abort.c:79
#2  0x00007fff27df3957 in  () at /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#3  0x00007fff27df9ae6 in  () at /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#4  0x00007fff27df9b21 in  () at /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#5  0x00007fff27df9da9 in __cxa_rethrow () at /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#6  0x00007fff1ecaac08 in gtsam::VectorValues gtsam::internal::linearAlgorithms::optimizeBayesTree<gtsam::GaussianBayesTree>(gtsam::GaussianBayesTree const&) ()
    at /home/mathieu/workspace/latest/lib/libgtsam.so.4
#7  0x00007fff1ec99d7d in gtsam::GaussianBayesTree::optimize() const ()
    at /home/mathieu/workspace/latest/lib/libgtsam.so.4
#8  0x00007fff1ecc1ddd in gtsam::GaussianFactorGraph::optimize(gtsam::Ordering const&, std::function<std::pair<boost::shared_ptr<gtsam::GaussianConditional>, boost::shared_ptr<gtsam::GaussianFactor> > (gtsam::GaussianFactorGraph const&, gtsam::Ordering const&)> const&) const ()
    at /home/mathieu/workspace/latest/lib/libgtsam.so.4
#9  0x00007fff1ed8be04 in gtsam::NonlinearOptimizer::solve(gtsam::GaussianFactorGraph const&, gtsam::NonlinearOptimizerParams const&) const () at /home/mathieu/workspace/latest/lib/libgtsam.so.4
#10 0x00007fff1ed48c27 in gtsam::GaussNewtonOptimizer::iterate() ()
    at /home/mathieu/workspace/latest/lib/libgtsam.so.4
#11 0x00007ffff74d593a 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/latest/lib/librtabmap_core.so.0.20
#12 0x00007ffff71fb74d 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::allocator<std::pair<int const, rtabmap::Link> > >*, double*, int*) const ()
    at /home/mathieu/workspace/latest/lib/librtabmap_core.so.0.20
#13 0x00007ffff71fbbed in rtabmap::Rtabmap::optimizeCurrentMap(int, bool, std::map<int, rtabmap::Transform, std::less<int>, std::allocator<std::pair<int const, rtabmap::Transform> > >&, cv::Mat&, std::multimap<int, rtabmap::Link, std::less<int>, std::allocator<std::pair<int const, rtabmap::Link> > >*, double*, int*) const () at /home/mathieu/workspace/latest/lib/librtabmap_core.so.0.20
#14 0x00007ffff720f2aa in rtabmap::Rtabmap::process(rtabmap::SensorData const&, rtabmap::Transform, cv::Mat const&, std::vector<float, std::allocator<float> > const&, std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, float, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, float> > > const&) () at /home/mathieu/workspace/latest/lib/librtabmap_core.so.0.20
#15 0x000055555555e3ee in main ()