RTAB-Map中地标忽略导致的优化器崩溃问题分析

问题背景

在使用RTAB-Map进行SLAM建图与定位时，开发者发现当在建图阶段启用地标(Landmarks)功能后，若在定位阶段设置Optimizer/LandmarksIgnored为true，会导致优化器崩溃。这一问题在使用g2o和GTSAM两种后端优化器时都会出现，但表现略有不同。

问题表现

g2o后端表现

当使用g2o作为后端优化器时，系统会抛出std::map越界访问错误。具体表现为在优化过程中尝试访问不存在的节点ID，导致程序崩溃。

GTSAM后端表现

当使用GTSAM作为后端优化器时，系统会抛出UException异常，提示条件uContains(poses, rootId)未满足，同样导致程序终止。

问题根源分析

经过深入代码分析，发现问题主要出现在以下场景：

1. 建图阶段：使用Optimizer/LandmarksIgnored=false参数建图，此时地标节点会被包含在优化图中。
2. 定位阶段：切换到Optimizer/LandmarksIgnored=true模式，理论上应该忽略地标节点。

问题发生的核心原因在于：

• 虽然优化器被设置为忽略地标，但系统仍尝试处理与地标相关的链接
• 在局部图优化后，优化后的位姿集合optPoses中不再包含地标ID
• 当代码尝试访问这些被忽略的地标位姿时，就会导致越界访问

技术细节

问题具体出现在RTAB-Map核心处理循环中，当系统尝试调整定位链接时：

1. 系统获取优化后的位姿optPoses
2. 遍历所有定位链接localizationLinks
3. 对于每个链接，尝试从optPoses中获取对应节点的位姿
4. 由于地标已被忽略，optPoses中不存在地标ID，导致std::map访问越界

解决方案

针对这一问题，正确的修复方式是在处理定位链接时，增加对优化器地标忽略状态的检查。具体来说：

1. 在创建链接时，如果优化器设置为忽略地标，则不应创建与地标的链接
2. 或者在处理链接时，跳过那些指向地标的链接

代码层面的修改建议是在创建链接的条件判断中加入对_graphOptimizer->landmarksIgnored()的检查，确保在忽略地标模式下不会创建与地标相关的链接。

对系统功能的影响

设置Optimizer/LandmarksIgnored=true确实意味着在定位过程中完全不会使用地标信息。这种设置适用于以下场景：

• 当环境中的地标不够稳定或可靠时
• 当计算资源有限，需要减少优化问题规模时
• 当定位主要依赖其他传感器（如激光雷达）时

然而，开发者需要注意，一旦启用此选项，所有地标信息将不会参与位姿优化计算，可能会影响在某些环境下的定位精度。

总结

这一问题揭示了RTAB-Map在处理不同建图与定位配置切换时的边界条件问题。通过深入分析，我们理解了当地标功能在不同阶段有不同配置时可能导致的优化器崩溃问题。正确的解决方案应该是在链接创建阶段就考虑优化器的地标忽略状态，确保系统在不同配置下都能保持一致性。