[论文解读] TagSLAM: Robust SLAM with Fiducial Markers

Bernd Pfrommer, Kostas Daniilidis|arXiv (Cornell University)|Oct 1, 2019

Robotics and Sensor-Based Localization参考文献 11被引用 13

一句话总结

TagSLAM 是一个开源的 ROS 框架，通过利用 GTSAM 的因子图优化，实现了基于 AprilTag 标识标记的鲁棒同步定位与地图构建（SLAM）。它通过基于子图的、误差验证的优化流程，实现可靠的初始化和回环检测，显著减少了视觉-惯性里程计系统中的轨迹漂移，通过将位姿锚定到已知标记位置来实现。

ABSTRACT

TagSLAM provides a convenient, flexible, and robust way of performing Simultaneous Localization and Mapping (SLAM) with AprilTag fiducial markers. By leveraging a few simple abstractions (bodies, tags, cameras), TagSLAM provides a front end to the GTSAM factor graph optimizer that makes it possible to rapidly design a range of experiments that are based on tags: full SLAM, extrinsic camera calibration with non-overlapping views, visual localization for ground truth, loop closure for odometry, pose estimation etc. We discuss in detail how TagSLAM initializes the factor graph in a robust way, and present loop closure as an application example. TagSLAM is a ROS based open source package and can be found at https://berndpfrommer.github.io/tagslam_web.

研究动机与目标

• 为机器人研究提供一种灵活、鲁棒且易于部署的 SLAM 解决方案，使用标识标记。
• 解决基于图的 SLAM 中初始化不良的问题，该问题可能导致收敛到局部极小值或失败。
• 通过利用已知的 AprilTag 位姿，在视觉-惯性里程计（VIO）中实现回环检测与轨迹校正。
• 支持多样化的机器人任务，如相机外参标定、物体位姿估计以及使用标识标记的真值定位。
• 通过一个文档齐全的开源 ROS 包，使先进 SLAM 技术对研究人员更加易用。

提出的方法

• 使用模块化的前端抽象，通过物体（如实验台、支架、积木）、标记（AprilTags）和相机来建模动态与静态场景元素。
• 采用基于子图的初始化策略：从检测到的标记生成候选位姿解，并通过误差阈值进行验证。
• 应用多阶排序初始化启发式方法：在子图中旋转因子排序，优先处理较大的标记，然后逐步处理较小的标记，从而提高鲁棒性。
• 对每个子图执行非增量式优化，以在合并到主图之前验证初始估计，模拟 RANSAC 模型选择过程。
• 在子图验证成功后，使用 iSAM2 对完整图进行增量式优化，实现高效的在线更新。
• 在因子图中整合投影因子（相机到标记）和相对位姿因子（来自里程计），以联合优化相机和标记位姿。

实验结果

研究问题

• RQ1 如何为基于因子图的 SLAM 设计一种鲁棒的初始化策略，使用标识标记以避免陷入局部极小值？
• RQ2 基于 AprilTag 的 SLAM 是否能可靠地在视觉-惯性里程计系统中实现回环检测，且漂移极小？
• RQ3 在动态或复杂环境中，使用已知标记位姿在多大程度上能提高轨迹精度？
• RQ4 TagSLAM 在标准 SLAM 之外，能在多大程度上支持多样化的机器人任务，如外参标定和物体位姿估计？
• RQ5 当轨迹长度增加、帧数增多时，TagSLAM 的计算性能和实时可行性如何？

主要发现

• 在废弃实验室中，TagSLAM 成功完成了一段 13 分钟、630 米的轨迹回环检测，修正了 VIO 中显著的轨迹漂移，否则该轨迹将无法返回起点。
• TagSLAM 的轨迹精确返回起点，而 VIO 和 ORB-SLAM2 均表现出显著漂移，证明了基于标记的回环检测的有效性。
• 在 Intel i9-9900K CPU 上，每帧平均处理时间为 12ms，完整非增量式优化耗时 4.3 秒，表明其在离线处理中具有强大的实时可行性。
• 单帧处理时间最高可达 260ms，源于 iSAM2 的重线性化，但当仅保留最后两个时间步时，系统仍具备实时使用能力。
• 基于子图的初始化策略结合因子排序的迭代重排，成功拒绝了异常测量值，防止了优化图被污染。
• TagSLAM 可在静态相机与移动标记的场景中部署，如 Smart Aviary 项目，而传统 SLAM 软件因缺乏显式物体跟踪而失效。