第257篇 全局一致性优化——位姿图优化和后端优化策略

前三篇聊了回环检测、ICP和NDT这些都是SLAM前端的活儿。前端给出帧间的相对位姿估计偶尔检测到回环。但这些估计都有噪声累积起来轨迹就会漂移。后端优化的任务就是把所有位姿估计和回环约束放在一起全局优化一遍让整条轨迹和地图尽可能一致。全局一致性优化是SLAM系统的总指挥。前端不管多精确误差总是有的。回环检测给了一个纠错的机会但纠错的效果取决于后端优化做得好不好。一个好的后端可以把带噪声的轨迹优化成平滑一致的全局地图一个差的后端会让错误约束把地图搞得一团糟。位姿图优化的数学模型位姿图优化Pose Graph OptimizationPGO是后端优化最常用的形式。图的节点是机器人在各个时刻的位姿边是位姿之间的约束来自里程计、配准、回环检测等。每条边包含两个信息测量的相对变换均值和测量的不确定度信息矩阵或协方差矩阵。优化的目标是找到一组节点位姿使得所有边的测量误差之和最小。# 位姿图优化的数学模型 # 目标函数: min Σ e_ij^T * Ω_ij * e_ij # e_ij measurement(T_i, T_j) - observed(T_i, T_j) # Ω_ij: 信息矩阵不确定度的逆 def pose_graph_cost(nodes, edges): total_cost 0 for edge in edges: i, j edge.node_i, edge.node_j e edge.measurement_residual(nodes[i], nodes[j]) total_cost e.T edge.information_matrix e return total_cost这个优化问题是非线性的因为旋转部分涉及三角函数。常用的求解方法有高斯-牛顿法、Levenberg-Marquardt法。g2o、GTSAM、Ceres这几个优化库都支持位姿图优化接口大同小异。前端和后端的关系SLAM系统通常分为前端和后端两部分。前端负责实时处理传感器数据给出帧间位姿估计速度要求高通常要和传感器帧率匹配。后端负责全局优化速度要求没那么高可以在独立线程中运行。前端给后端的输入包括相邻帧的相对位姿里程计约束、回环帧的相对位姿回环约束、可能还有IMU预积分约束、GPS约束等。后端把这些约束全部放到一个优化问题中求解最优的全局位姿。# 典型的SLAM前后端交互 class SLAMSystem: def process_frame(self, frame): # 前端实时里程计 T self.frontend.odometry(frame) self.graph.add_node(T) self.graph.add_edge(prev_id, curr_id, T, odom_info) # 回环检测 if self.loop_detector.detect(frame): self.graph.add_edge(loop_id, curr_id, T_loop, loop_info) # 后端异步优化 self.backend.optimize_async()前端和后端解耦的好处是前端可以跑得快保证实时性。后端可以在后台慢慢优化不影响前端的帧率。当后端完成一次优化后前端的位姿会被更新为优化后的值后续的里程计就从优化后的位姿开始累积。g2o和GTSAM——两个主流优化库g2oGeneral Graph Optimization是Rainer Kuemmerle等人开发的图优化库ORB-SLAM、HDL-Graph-SLAM等系统都用它。g2o的设计比较灵活用户可以自定义节点类型、边类型和优化策略。但g2o的API设计有点老旧用起来不够直观文档也不太完善。GTSAMGeorgia Tech Smoothing and Mapping是Georgia Tech开发的因子图优化库iSAM2算法就包含在里面。GTSAM的优势是数学上更严谨支持增量优化iSAM2API设计也更现代。很多新的SLAM系统倾向用GTSAM替代g2o。两个库的核心区别在优化策略上。g2o用的是批量优化每次把所有变量重新优化一遍。GTSAM的iSAM2是增量优化只对受影响的变量做更新计算效率更高。对于实时SLAM来说增量优化的优势很明显——不是每次都从头优化而是在上次结果的基础上做增量更新。# GTSAM的增量优化示例 import gtsam isam2 gtsam.ISAM2() for new_factors in incoming_constraints: isam2.update(new_factors) # 增量更新 result isam2.calculateEstimate()鲁棒优化——处理错误约束实际系统中回环检测不可能百分百准确。偶尔会有错误的回环约束被加入位姿图。如果后端优化对这些错误约束不做任何处理一条错误的边就可能把整张地图拉变形。鲁棒优化的核心思想是给每条约束分配一个权重可靠的约束权重大不可靠的约束权重小甚至为零。怎么判断一条约束是否可靠有几种常用方法。Switchable Constraints在每条边上加一个可学习的开关变量。优化过程中如果某条边的残差持续很大开关变量会被优化到接近零相当于关掉这条边。这种方法不需要显式地判断哪条边是错的优化器自己会学。Max-Mixture Model把每条边的误差模型从单一高斯改成多个高斯的混合。一个正常的高斯窄分布和一个异常的高斯宽分布。正常约束走窄分布异常约束走宽分布残差大但代价不高。# Switchable Constraints的思路 # 原来: e^T * Ω * e # 改后: s^2 * e^T * Ω * e (1-s)^2 * λ # s是开关变量优化过程中自动调整 def switchable_cost(e, omega, s, lam1.0): return s**2 * (e.T omega e) (1 - s)**2 * lam面试追问环节面试官位姿图优化的信息矩阵怎么设信息矩阵是协方差矩阵的逆表示约束的可信度。里程计约束的信息矩阵通常根据传感器的噪声模型来设。比如激光里程计平移部分的噪声假设是0.01米旋转部分的噪声假设是0.001弧度信息矩阵就是diag(1/0.01^2, 1/0.01^2, 1/0.01^2, 1/0.001^2, 1/0.001^2, 1/0.001^2)。回环约束的信息矩阵通常设得比里程计小一些因为回环检测的不确定度更大。面试官g2o和GTSAM选哪个好如果是新项目建议用GTSAM。API更现代文档更完善增量优化iSAM2对实时系统更友好。如果是维护老项目或者需要和现有系统集成比如ORB-SLAM那就继续用g2o。两者的优化结果差别不大主要是工程体验的差异。GTSAM的学习曲线稍陡一些但用起来更舒服。面试官增量优化和批量优化有什么区别批量优化每次把所有变量都重新求解计算量是O(N^3)N是变量数量。增量优化只更新受新约束影响的变量计算量远小于批量优化。在SLAM系统中每来一帧数据只有最近的几个位姿会受影响大部分历史位姿不需要重新优化。增量优化的优势就体现在这里。但增量优化在检测到回环时也需要做较大范围的更新这时候和批量优化的差别就小了。面试官怎么检测和剔除错误的回环几种方法。一是在优化前做几何一致性检查比较回环约束和当前估计的相对位姿差异太大的直接丢弃。二是在优化后检查每条边的残差残差特别大的边可能是错误的。三是用鲁棒优化Switchable Constraints或Max-Mixture让优化器自动降权。四是做多次优化第一次用所有边剔除残差大的边第二次用过滤后的边重新优化。面试官位姿图优化能处理大规模场景吗直接优化一个有几十万个节点的位姿图是不现实的计算量太大。处理大规模场景的常用策略是用iSAM2做增量优化避免每次全量优化用边缘化marginalization去掉旧的变量控制图的大小用子地图分层优化局部子地图内部优化子地图之间用少量约束连接。Cartographer和HDL-Graph-SLAM都用了这些策略来应对大规模场景。全局一致性优化是SLAM系统的最后一道关卡。前端的误差在这里被修正回环的约束在这里发挥作用。位姿图优化用一个统一的数学框架把所有传感器信息融合在一起输出全局一致的轨迹和地图。后端优化的核心是把所有约束放在一起求解最优解。这个思路简单但强大是现代SLAM系统的基石。上一篇第256篇 NDT正态分布变换——大规模点云配准的高效方案下一篇我们聊动态环境SLAM看看当场景中有运动物体时SLAM系统要怎么应对。如果这篇文章对你有帮助欢迎点赞支持一下你的鼓励是我持续更新的动力