尧图网站建设 
g2o讲完了这篇聊另一个重要的优化框架——GTSAM。GTSAM的全称是Georgia Tech Smoothing and Mapping是Georgia Tech的Frank Dellaert教授团队开发的用因子图Factor Graph来建模优化问题。和g2o的图优化相比GTSAM的建模方式更灵活API设计也更现代。越来越多的SLAM和机器人项目在用GTSAM包括很多学术界的顶级系统。VINS-Fusion的后端就用了GTSAM。因子图是什么因子图是一种二部图有两类节点变量节点和因子节点。变量节点代表优化变量位姿、路标点因子节点代表约束观测、运动模型。每条边只连接一个因子和一个变量表示这个因子涉及这个变量。和g2o的图相比因子图的区别在于g2o的边直接连接两个变量因子图通过因子节点间接连接。这个区别表面上看起来很小但带来了建模上的灵活性——一个因子可以涉及任意数量的变量。比如IMU的预积分约束涉及两个位姿和IMU零偏用因子图表达就很自然。# GTSAM因子图的直觉 # 变量节点x0, x1, x2位姿, l0, l1路标点 # 因子节点 # between(x0, x1) → 相邻帧的相对运动 # between(x1, x2) → 相邻帧的相对运动 # project(x0, l0) → 视觉观测约束 # project(x1, l0) → 视觉观测约束 # loop(x0, x2) → 回环约束GTSAM的基本使用import gtsam # 创建因子图 graph gtsam.NonlinearFactorGraph() # 添加里程计因子between factor odometry gtsam.Pose2(1.0, 0.0, 0.0) noise gtsam.noiseModel_Diagonal.Sigmas(...) graph.add(gtsam.BetweenFactorPose2(0, 1, odometry, noise)) # 添加先验因子 graph.add(gtsam.PriorFactorPose2(0, initial_pose, prior_noise)) # 创建初始估计 initial gtsam.Values() initial.insert(0, gtsam.Pose2(0, 0, 0)) # 优化 params gtsam.LevenbergMarquardtParams() result gtsam.LevenbergMarquardtOptimizer(graph, initial, params).optimize()GTSAM的因子类型很丰富。BetweenFactor表示两个变量之间的相对约束PriorFactor表示先验约束ProjectionFactor表示视觉投影约束。你还可以自定义因子只要实现误差函数和雅可比矩阵就行。GTSAM的噪声模型也很灵活。Diagonal表示对角噪声各维度独立Isotropic表示各向同性噪声各维度相同Full表示完整协方差噪声。选择合适的噪声模型对最终的优化结果影响很大。如果你不确定传感器的噪声特性先用Isotropic只设一个sigma值后面再根据实验结果调整。# 不同的噪声模型 diag_noise gtsam.noiseModel_Diagonal.Sigmas(sigma_vector) iso_noise gtsam.noiseModel_Isotropic.Sigma(0.1) full_noise gtsam.noiseModel_Gaussian.Information(info_matrix)GTSAM vs g2o两者都能做SLAM后端优化但设计哲学不同。g2o是面向SLAM的内置了SLAM常用的顶点和边类型用起来比较直接。但扩展性有限想加新的变量类型或约束类型需要修改底层代码。对于标准的视觉SLAM后端g2o已经足够用了。GTSAM是面向通用机器人的因子图的建模方式可以表达各种复杂的优化问题。多传感器融合、协同定位这些复杂场景GTSAM处理起来更加自然。GTSAM的C代码质量很高文档和测试覆盖也很完善适合学习参考。GTSAM的iSAM2算法支持增量优化。每来一个新观测iSAM2只更新受影响的变量不需要重新优化整个图。这在实时SLAM中特别有用。g2o目前还没有内置增量优化的支持。# iSAM2增量优化 isam2 gtsam.ISAM2(params) for new_factors, new_values in data_stream: isam2.update(new_factors, new_values) result isam2.calculateEstimate() # 每次只更新受影响的变量非常快面试中的高频追问因子图和g2o的图有什么区别 因子图是二部图变量节点因子节点g2o的图是普通图节点是变量边是约束。因子图的表达力更强——一个因子可以涉及多个变量而g2o的边通常只连接两个变量。GTSAM的iSAM2是什么 iSAM2是增量平滑和建图算法。它利用Bayes Tree数据结构来维护优化问题的因子化结果。每次新来一个观测只需要更新Bayes Tree中受影响的分支不需要重新计算整个树。复杂度接近O(1)每次更新只涉及少数变量。Bayes Tree的直觉是这样的把优化问题的变量按消去顺序排列成一棵树。树的根节点是最后被消去的变量叶子节点是最先被消去的变量。每次新来一个因子只需要更新从叶子到根的路径上的节点。路径之外的节点不受影响不需要重新计算。这就是iSAM2能高效增量更新的原因。GTSAM的VariableKey是什么 GTSAM用整数key来标识变量。key的高位表示变量的类型位姿、路标点等低位表示变量的序号。这种设计让变量的管理很方便但也要求你自己维护key的分配。GTSAM提供了一些辅助函数如Symbol类来简化key的管理。GTSAM的marginalize是什么意思 边缘化就是把一个变量从优化问题中消去同时保留它对其他变量的约束信息。在iSAM2中当滑动窗口向前移动时最老的变量会被边缘化。边缘化之后剩下的变量之间会产生新的约束fill-inBayes Tree会自动处理这些fill-in。实际项目中的选择如果你的SLAM系统只涉及视觉BA通常g2o和GTSAM都可以胜任。g2o的教程更多社区更大。GTSAM的API更优雅增量优化也是加分项。如果你的系统涉及多传感器融合视觉IMU激光GPSGTSAM更合适。因子图的建模方式天然适合把不同类型的约束统一到一个优化问题里每种传感器对应一类因子代码结构清晰。如果你需要增量优化每来一帧就更新结果不用等所有数据到齐GTSAM的iSAM2是唯一的选择。而g2o只能做批处理优化每次都要从头算。GTSAM和g2o是SLAM后端的两大工具。理解了图优化和因子图的思想不管用哪个框架都能快速上手。两者的核心都是把SLAM建模为优化问题区别主要在于建模方式和求解策略。实际项目中很多团队会同时用到两个框架。比如用g2o做视觉BA因为它的BA实现很成熟用GTSAM做位姿图优化和多传感器融合因为因子图更灵活。你不需要只选一个了解两者的优劣根据场景选择最合适的工具。学习GTSAM的建议先读Frank Dellaert的《Factor Graphs and GTSAM》教程讲得很清楚。然后看GTSAM的examples目录从简单的位姿图优化开始逐步过渡到视觉BA和多传感器融合。GTSAM的C接口比Python接口更完整建议使用C来开发。上一篇第245篇 视觉SLAM后端之g2o框架——图优化的工程实现下一篇我们进入经典系统解析聊ORB-SLAM2——视觉SLAM的里程碑式工作。这是面试中被问得最多的SLAM系统你必须深入了解它的架构和每个模块的细节包括三个线程的协作、关键帧的选择策略、共视图的结构等。如果这篇文章对你有帮助欢迎点赞支持一下你的鼓励是我持续更新的最大动力 第246篇 视觉SLAM后端之GTSAM——因子图优化的另一种方案
g2o讲完了这篇聊另一个重要的优化框架——GTSAM。GTSAM的全称是Georgia Tech Smoothing and Mapping是Georgia Tech的Frank Dellaert教授团队开发的用因子图Factor Graph来建模优化问题。和g2o的图优化相比GTSAM的建模方式更灵活API设计也更现代。越来越多的SLAM和机器人项目在用GTSAM包括很多学术界的顶级系统。VINS-Fusion的后端就用了GTSAM。因子图是什么因子图是一种二部图有两类节点变量节点和因子节点。变量节点代表优化变量位姿、路标点因子节点代表约束观测、运动模型。每条边只连接一个因子和一个变量表示这个因子涉及这个变量。和g2o的图相比因子图的区别在于g2o的边直接连接两个变量因子图通过因子节点间接连接。这个区别表面上看起来很小但带来了建模上的灵活性——一个因子可以涉及任意数量的变量。比如IMU的预积分约束涉及两个位姿和IMU零偏用因子图表达就很自然。# GTSAM因子图的直觉 # 变量节点x0, x1, x2位姿, l0, l1路标点 # 因子节点 # between(x0, x1) → 相邻帧的相对运动 # between(x1, x2) → 相邻帧的相对运动 # project(x0, l0) → 视觉观测约束 # project(x1, l0) → 视觉观测约束 # loop(x0, x2) → 回环约束GTSAM的基本使用import gtsam # 创建因子图 graph gtsam.NonlinearFactorGraph() # 添加里程计因子between factor odometry gtsam.Pose2(1.0, 0.0, 0.0) noise gtsam.noiseModel_Diagonal.Sigmas(...) graph.add(gtsam.BetweenFactorPose2(0, 1, odometry, noise)) # 添加先验因子 graph.add(gtsam.PriorFactorPose2(0, initial_pose, prior_noise)) # 创建初始估计 initial gtsam.Values() initial.insert(0, gtsam.Pose2(0, 0, 0)) # 优化 params gtsam.LevenbergMarquardtParams() result gtsam.LevenbergMarquardtOptimizer(graph, initial, params).optimize()GTSAM的因子类型很丰富。BetweenFactor表示两个变量之间的相对约束PriorFactor表示先验约束ProjectionFactor表示视觉投影约束。你还可以自定义因子只要实现误差函数和雅可比矩阵就行。GTSAM的噪声模型也很灵活。Diagonal表示对角噪声各维度独立Isotropic表示各向同性噪声各维度相同Full表示完整协方差噪声。选择合适的噪声模型对最终的优化结果影响很大。如果你不确定传感器的噪声特性先用Isotropic只设一个sigma值后面再根据实验结果调整。# 不同的噪声模型 diag_noise gtsam.noiseModel_Diagonal.Sigmas(sigma_vector) iso_noise gtsam.noiseModel_Isotropic.Sigma(0.1) full_noise gtsam.noiseModel_Gaussian.Information(info_matrix)GTSAM vs g2o两者都能做SLAM后端优化但设计哲学不同。g2o是面向SLAM的内置了SLAM常用的顶点和边类型用起来比较直接。但扩展性有限想加新的变量类型或约束类型需要修改底层代码。对于标准的视觉SLAM后端g2o已经足够用了。GTSAM是面向通用机器人的因子图的建模方式可以表达各种复杂的优化问题。多传感器融合、协同定位这些复杂场景GTSAM处理起来更加自然。GTSAM的C代码质量很高文档和测试覆盖也很完善适合学习参考。GTSAM的iSAM2算法支持增量优化。每来一个新观测iSAM2只更新受影响的变量不需要重新优化整个图。这在实时SLAM中特别有用。g2o目前还没有内置增量优化的支持。# iSAM2增量优化 isam2 gtsam.ISAM2(params) for new_factors, new_values in data_stream: isam2.update(new_factors, new_values) result isam2.calculateEstimate() # 每次只更新受影响的变量非常快面试中的高频追问因子图和g2o的图有什么区别 因子图是二部图变量节点因子节点g2o的图是普通图节点是变量边是约束。因子图的表达力更强——一个因子可以涉及多个变量而g2o的边通常只连接两个变量。GTSAM的iSAM2是什么 iSAM2是增量平滑和建图算法。它利用Bayes Tree数据结构来维护优化问题的因子化结果。每次新来一个观测只需要更新Bayes Tree中受影响的分支不需要重新计算整个树。复杂度接近O(1)每次更新只涉及少数变量。Bayes Tree的直觉是这样的把优化问题的变量按消去顺序排列成一棵树。树的根节点是最后被消去的变量叶子节点是最先被消去的变量。每次新来一个因子只需要更新从叶子到根的路径上的节点。路径之外的节点不受影响不需要重新计算。这就是iSAM2能高效增量更新的原因。GTSAM的VariableKey是什么 GTSAM用整数key来标识变量。key的高位表示变量的类型位姿、路标点等低位表示变量的序号。这种设计让变量的管理很方便但也要求你自己维护key的分配。GTSAM提供了一些辅助函数如Symbol类来简化key的管理。GTSAM的marginalize是什么意思 边缘化就是把一个变量从优化问题中消去同时保留它对其他变量的约束信息。在iSAM2中当滑动窗口向前移动时最老的变量会被边缘化。边缘化之后剩下的变量之间会产生新的约束fill-inBayes Tree会自动处理这些fill-in。实际项目中的选择如果你的SLAM系统只涉及视觉BA通常g2o和GTSAM都可以胜任。g2o的教程更多社区更大。GTSAM的API更优雅增量优化也是加分项。如果你的系统涉及多传感器融合视觉IMU激光GPSGTSAM更合适。因子图的建模方式天然适合把不同类型的约束统一到一个优化问题里每种传感器对应一类因子代码结构清晰。如果你需要增量优化每来一帧就更新结果不用等所有数据到齐GTSAM的iSAM2是唯一的选择。而g2o只能做批处理优化每次都要从头算。GTSAM和g2o是SLAM后端的两大工具。理解了图优化和因子图的思想不管用哪个框架都能快速上手。两者的核心都是把SLAM建模为优化问题区别主要在于建模方式和求解策略。实际项目中很多团队会同时用到两个框架。比如用g2o做视觉BA因为它的BA实现很成熟用GTSAM做位姿图优化和多传感器融合因为因子图更灵活。你不需要只选一个了解两者的优劣根据场景选择最合适的工具。学习GTSAM的建议先读Frank Dellaert的《Factor Graphs and GTSAM》教程讲得很清楚。然后看GTSAM的examples目录从简单的位姿图优化开始逐步过渡到视觉BA和多传感器融合。GTSAM的C接口比Python接口更完整建议使用C来开发。上一篇第245篇 视觉SLAM后端之g2o框架——图优化的工程实现下一篇我们进入经典系统解析聊ORB-SLAM2——视觉SLAM的里程碑式工作。这是面试中被问得最多的SLAM系统你必须深入了解它的架构和每个模块的细节包括三个线程的协作、关键帧的选择策略、共视图的结构等。如果这篇文章对你有帮助欢迎点赞支持一下你的鼓励是我持续更新的最大动力