Landmarks ignores will cause crashing in optimizer about rtabmap HOT 3 CLOSED

Humm, I may know what is going on. The out of bound error may happen here:

Hypotheses:

• The map has been created with landmarks and Optimizer/LandmarksIgnored=false, so the optimized graph will contain landmark poses.
• In localization mode, after setting Optimizer/LandmarksIgnored=true, the landmark links are somewhat still used but internal graph optimization is ignoring them, so after local graph optimization the landmarks are ignored and optPoses won't contain landmark ids anymore.

Possible solution could be to add && !_graphOptimizer->landmarksIgnored() to this if:

I'll try to reproduce in simulation and test if the above could fix it.

from rtabmap.

If I use GTSAM then this is the error
[ INFO] (2024-02-29 18:09:59.549) Rtabmap.cpp:2992::process() Landmark 400 observed again! Seen the first time by node 112. [FATAL] (2024-02-29 18:09:59.549) OptimizerGTSAM.cpp:144::optimize() Condition (uContains(poses, rootId)) not met! terminate called after throwing an instance of 'UException' what(): [FATAL] (2024-02-29 18:09:59.549) OptimizerGTSAM.cpp:144::optimize() Condition (uContains(poses, rootId)) not met!

from rtabmap.

I found out that the std map out of range happened in this for loop

					Transform newOptPoseInv = optPoses.at(signature->id()).inverse();
					for(std::multimap<int, Link>::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter)
					{
						if(!_localizationSmoothing)
						{
							// Add original link without optimization
							UDEBUG("Adding new odom cache constraint %d->%d (%s)", 
								iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str());
						}
						else
						{
							// Adjust with optimized poses, this will smooth the localization
							Transform newT = newOptPoseInv * optPoses.at(iter->first);
							UDEBUG("Adjusted localization link %d->%d after optimization", iter->second.from(), iter->second.to());
							UDEBUG("from %s", iter->second.transform().prettyPrint().c_str());
							UDEBUG("  to %s", newT.prettyPrint().c_str());
							iter->second.setTransform(newT);

							// Update link in the referred signatures
							if(iter->first > 0)
								_memory->updateLink(iter->second, false);
						}
						
						_odomCacheConstraints.insert(std::make_pair(signature->id(), iter->second));
					}

from rtabmap.