点击下方卡片，关注“自动驾驶之心”公众号
戳我-> 领取自动驾驶近15个方向学习路线
今天自动驾驶之心为大家分享上交最新的泊车工作GCSLAM！如果您有相关工作需要分享，请在文末联系我们！
自动驾驶课程学习与技术交流群事宜，也欢迎添加小助理微信AIDriver004做进一步咨询
论文作者 | 3D视觉之心
编辑 | 自动驾驶之心
写在前面 & 笔者的个人理解
上交的工作：本文提出了一种全局一致的语义SLAM系统（GCSLAM）和语义融合定位子系统（SF-Loc），在复杂的停车场中实现了精确的语义建图和鲁棒的定位。视觉相机（前视和环视）、IMU和车轮编码器构成了我们系统的输入传感器配置。我们工作的第一部分是GCSLAM。GCSLAM引入了一种新的因子图来优化位姿和语义图，该图结合了基于多传感器数据和BEV（鸟瞰图）语义信息的创新误差项。此外，GCSLAM还集成了一个全局停车位管理模块，用于存储和管理停车位观测值。SF-Loc是我们工作的第二部分，它利用GCSLAM构建的语义图进行基于地图的定位。SF-Loc将配准结果和里程计位姿与一个新的因子图相结合。我们的系统在两个真实世界的数据集上表现出了优于现有SLAM的性能，在鲁棒的全局定位和精确的语义建图方面表现出了出色的能力。
总结来说，本文的主要贡献如下：
提出了一种全局一致的语义SLAM系统GCSLAM，该系统基于因子图优化，具有创新的车位表示和新的几何语义组合误差项约束。
引入了一个停车位管理模块，该模块存储停车位观测值并更新全局停车位，同时有效地处理噪声和错误检测。
提出了一种基于地图的定位子系统SF-Loc，该子系统使用因子图优化将语义ICP结果和里程计约束融合在一起。
在复杂的现实世界室内停车场验证了我们的系统，表明我们的系统实现了实时、高精度的定位和语义建图性能。
相关工作回顾
早期的视觉SLAM方法是基于滤波方法实现的。随后，利用BA优化的SLAM系统出现了。DSO在估计稠密或半稠密几何体之前引入了光度误差和几何误差。ORB-SLAM采用ORB特征和滑动窗口来实现精确的位姿估计。与滤波方法相比，基于优化的方法提供了更高的精度和更好的全局一致性。
尽管如此，具有单个摄像头的SLAM无法恢复规模，并且容易受到视觉模糊的影响。为了提高系统的鲁棒性和准确性，开发了将视觉数据与其他传感器相结合的多传感器融合方法。MSCKF使用视觉信息构建观测模型，并使用惯性测量单元（IMU）数据更新状态。VINS Mono提出了一种紧密耦合、基于优化的视觉惯性系统。VIWO开发了一种基于MSCKF的位姿估计器，该估计器集成了IMU、相机和车轮测量。DM-VIO通过延迟边缘化和位姿图束调整来增强IMU初始化。Ground-Fusion引入了一种自适应初始化策略来解决多个角点情况。
然而，由于室内环境的复杂条件，如有限的独特特征和复杂的照明条件，这些方法无法在室内停车位内执行AVP任务的SLAM。为了解决这些问题，一些工作都利用鸟瞰（BEV）图像作为输入，可以提供丰富的地面特征，以解决停车场独特特征有限的问题。AVP-SLAM使用语义分割来注释图像中的停车位、地面标记、减速带和其他信息，因为分割方法可以有效地适应复杂的照明条件。该语义信息被添加到全局图中，然后用于注册辅助定位。然而，他们的地图是用于注册的纯点云图，没有独立记录每个停车场，也缺乏每个停车位的位置和角度等重要属性信息。赵等人利用停车位检测器检测停车位的入口点，并将停车位的观测与里程计相结合，构建新的定位因子。然而，这种方法并不能维护一个完整的停车位地图。相反，它主要使用地图作为定位的辅助工具。VISSLAM在停车位之间添加了约束，结合里程计信息提出了一种改进建图结果的停车位管理算法。后续工作MOFISSLAM结合了滑动窗口优化，实现了更高的定位精度和改进的建图结果。
然而，现有的方法对噪声很敏感，在复杂的停车场表现不佳。为了解决这个问题，我们提出了一种新的室内停车SLAM因子图，提高了鲁棒性和准确性。
方法详解
我们的系统采用多个传感器作为输入，包括一个前视摄像头、IMU、车轮编码器和四个全景摄像头。我们工作的总体框架如图2所示。我们工作的第一部分是SLAM系统GCSLAM。GCSLAM集成了三个模块：全局时隙管理模块、里程计和因子图优化。里程计模块与其他模块松散耦合，使其可替换，增强了系统的灵活性和可用性。本文采用VIW作为里程计模块。全局时隙管理模块包括BEV感知模块和时隙关联。我们的BEV感知模块是一个基于多任务框架。它以BEV图像为输入，实时生成语义分割结果（地面标记）和时隙检测结果（停车边界端点），使用统一的骨干网络，为每个特定任务提供不同的输出头。此外，该全局时隙管理模块将检测结果注册到全局时隙并执行时隙关联。基于里程计位姿、语义信息和时隙关联结果，因子图优化可以实现精确的位姿估计和全局语义图构建。在建立全局语义图后，我们工作的第二部分，定位子系统SF-Loc，将里程计位姿与语义配准结果融合在一起，用于基于地图的定位。
带语义车位节点的因子图
我们将SLAM任务视为一个因子图优化问题，旨在估计关键帧的精确位姿。基于里程计提供的帧间距离来选择关键帧。因子图由节点和边组成，其中节点表示要优化的变量，边是约束节点的误差项。如图3所示，GCSLAM使用两种类型的节点和四种类型的边构建因子图
节点和误差术语的定义将具体介绍如下。
1）位姿节点：由于我们的SLAM系统假设一个平面停车场，姿势节点在世界坐标系中存储了第i帧的3自由度（DoF）车辆位姿（x，y，θ）。我们使用里程计模块提供的估计位姿位姿来初始化位姿节点。此模块作为单独的线程运行。
2）车位节点：当BEV感知模块检测到停车位时，它会在像素坐标中输出其入口边缘的端点坐标和方向。我们首先使用BEV图像的虚拟固有K和当前帧位姿Ti将入口边缘的中点注册到世界坐标。Ti的方程为：
3）里程表误差项（OET）：我们基于里程表模块在之间构建OET。OET的具体形式是：
4）配准误差项（RET）：RET限制了和之间的关系。
5）Adjacent Error Term (AET):
6）Global Vertical Error Term (GVET)：
全局车位管理
在因子图优化过程中，自车位姿不断优化。在自车位姿优化后，从自车位姿和车位观察配准的全局车位位姿也需要更新。因此，我们使用全局车位管理模块来存储和管理多个车位观测帧。当新的观测帧到达时，管理系统将当前观测与现有的全局时隙相关联。否则，它将创建一个新的全局车位。当自车的位姿优化或新的观察帧到达时，管理系统会更新全局停车位。
1）车位关联：为了确定车位观测值是否与之前观测到的现有全局车位相关联，我们首先将当前观测值配准到世界坐标，表示为Sobs。然后，我们使用kd树来找到最近的全局车位。根据它们中点之间的距离d，我们确定它们是否相关。如图4所示，如果S不与任何现有的obs全局车位相关联，则它将被创建为新的停车位或作为错误检测被丢弃。具体参数如图4所示。
一旦全局车位Sk与Sobs相关联，我们就增加该时隙的观测帧计数。通过记录每个全局时隙的观测频率，我们可以排除低频时隙作为错误检测。这种滤波策略可以有效地减轻BEV感知模块的噪声。具体的过滤逻辑如Alg 1所示。
2）车位更新：由于因子图是实时优化的，因此每个位姿节点对应的汽车位姿都在不断变化。由于全局车位是根据汽车位姿Ti和车位观测配准的，因此当因子图中每一帧的位姿发生变化时，它应该相应地更新。
基于地图的定位子系统
GCSLAM将车位和其他语义信息转换为点云，并获得全局图。基于该全局图，我们提出了一个融合里程计位姿和配准结果的定位子系统SF-Loc。SF-Loc和GCSLAM不会同时激活。GCSLAM仅在首次进入未知停车场时执行，而SF-Loc仅在使用已建立的全局地图重新访问停车场时激活。如图5所示，SF-Loc由因子图GSF-Loc构建：
我们的语义ICP配准算法在局部地图和当前点云之间进行配准。局部地图是基于之前的姿势从全局地图中提取的30m×30m的地图。当前点云是从BEV语义转换而来的。在语义ICP过程中，使用kd树识别每个点具有相同语义的最近邻。基于语义点对的匹配关系，计算当前点云与局部地图之间的转换。此过程迭代执行，直到收敛，提供精确的姿势
语义ICP误差项是一元边，提供配准的绝对位姿结果：
由于一元边施加的强约束和语义分割的高噪声，我们降低了添加ICP一元边的频率。我们每10帧添加一次ICP错误项，并在添加之前进行跳跃检测。我们计算当前帧的ICP配准结果与前一帧的ICP注册结果之间的距离。如果距离超过2米的阈值，则认为当前帧的配准结果不准确。在这种情况下，我们不会为当前帧添加ICP错误项。
语义ICP误差项有效地纠正了里程计的累积漂移，而OET减轻了ICP中的不稳定跳跃。因此，SF-Loc提高了定位的精度和鲁棒性。
实验结果
结论
本文介绍了一种新型的室内停车跟踪和建图系统GCSLAM。GCSLAM结合了创新的因子图和新颖的误差项，在复杂的停车环境中实现了稳健和高精度的建图。此外，我们还开发了一个基于地图的定位子系统SF-Loc。SF-Loc基于一种新的因子图将配准结果和里程计位姿融合在一起，有效地提高了定位精度。我们通过真实世界的数据集验证了我们的算法，证明了我们系统的有效性和鲁棒性。
参考
[1] Towards Autonomous Indoor Parking: A Globally Consistent Semantic SLAM System and A Semantic Localization Subsystem
『自动驾驶之心知识星球』欢迎加入交流！重磅，自动驾驶之心科研论文辅导来啦，申博、CCF系列、SCI、EI、毕业论文、比赛辅导等多个方向，欢迎联系我们！
① 全网独家视频课程
端到端自动驾驶、仿真测试、自动驾驶C++、BEV感知、BEV模型部署、BEV目标跟踪、毫米波雷达视觉融合、多传感器标定、多传感器融合、多模态3D目标检测、车道线检测、轨迹预测、在线高精地图、世界模型、点云3D目标检测、目标跟踪、Occupancy、CUDA与TensorRT模型部署、大模型与自动驾驶、NeRF、语义分割、自动驾驶仿真、传感器部署、决策规划、轨迹预测等多个方向学习视频（扫码即可学习）
② 国内首个自动驾驶学习社区
国内外最大最专业，近4000人的交流社区，已得到大多数自动驾驶公司的认可！涉及30+自动驾驶技术栈学习路线，从0到一带你入门自动驾驶感知（端到端自动驾驶、世界模型、仿真闭环、2D/3D检测、语义分割、车道线、BEV感知、Occupancy、多传感器融合、多传感器标定、目标跟踪）、自动驾驶定位建图（SLAM、高精地图、局部在线地图）、自动驾驶规划控制/轨迹预测等领域技术方案、大模型，更有行业动态和岗位发布！欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频
③【自动驾驶之心】技术交流群
自动驾驶之心是首个自动驾驶开发者社区，聚焦感知、定位、融合、规控、标定、端到端、仿真、产品经理、自动驾驶开发、自动标注与数据闭环多个方向，目前近60+技术交流群，欢迎加入！扫码添加汽车人助理微信邀请入群，备注：学校/公司+方向+昵称（快速入群方式）
④【自动驾驶之心】全平台矩阵
