RTAB-Map在Jetson平台上的GTSAM优化器异常问题分析

问题背景

在机器人SLAM领域，RTAB-Map是一个广泛使用的实时外观基于的映射和定位系统。近期在使用Jetson Orin（aarch64架构）平台运行RTAB-Map时，发现了一个与GTSAM优化器相关的异常现象：在进行图优化时，优化器会计算出极其巨大的误差值（如4.57×10^308），导致原本正确的闭环检测被错误地拒绝。

问题现象

在Jetson Orin平台上运行RTAB-Map时，当启用iSAM2增量优化（GTSAM/Incremental=true）时，GTSAM优化器会报告异常巨大的误差值。具体表现为：

1. 优化器初始误差和最终误差都显示为4.57×10^308这样的天文数字
2. 由于误差值过大，系统误判为错误的闭环检测，导致正确的闭环被拒绝
3. 在x86_64架构上运行相同的数据库时，优化器表现正常，闭环被正确接受

根本原因分析

经过深入排查，发现问题源于RTAB-Map中对第一个节点设置的先验约束。在代码中，为了固定初始节点的方向，使用了极小的数值（std::numeric_limits::min()）作为先验噪声模型的方差。

在aarch64架构上，这种极小值的设置导致了数值计算的不稳定性，产生了巨大的误差估计。而在x86_64架构上，同样的设置却能得到预期的零误差结果。这揭示了不同硬件架构在浮点运算实现上的细微差异。

解决方案

修复方案相对简单：将先验噪声模型中的极小值替换为一个合理的小数值（如1e-9）。具体修改如下：

// 修改前
priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, std::numeric_limits<double>::min()));

// 修改后
priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, 1e-9));

临时解决方案

在正式修复前，用户可以采用以下临时解决方案：

1. 设置Optimizer/Epsilon=0，这会影响GTSAM中的relativeErrorTol参数
2. 减少优化迭代次数，如设置Optimizer/Iterations=5
3. 禁用增量优化，使用批处理模式（GTSAM/Incremental=false）

技术启示

这个问题给我们几个重要的技术启示：

1. 跨平台开发时，数值计算的稳定性需要特别关注，不同硬件架构可能有不同的浮点运算实现
2. 在设置约束条件时，应避免使用极端值（如最小/最大浮点数）
3. 对于SLAM系统中的优化问题，合理的噪声模型参数设置至关重要
4. 错误处理机制应考虑数值溢出的可能性

总结

RTAB-Map在Jetson平台上的这个优化器异常问题，展示了SLAM系统中数值稳定性问题的重要性。通过合理设置先验噪声模型的参数，我们能够确保系统在不同硬件平台上都能稳定运行。这也提醒开发者在跨平台开发时，需要对数值计算保持高度警惕，特别是在涉及优化和矩阵运算的场景中。