Gtsam学习笔记

最新推荐文章于 2025-06-19 09:17:23 发布

三川小哥知乎

最新推荐文章于 2025-06-19 09:17:23 发布

阅读量1.5w

点赞数 11

CC 4.0 BY-SA版权

分类专栏： SLAM 文章标签： SLAM 后端

本文链接：https://blog.youkuaiyun.com/qq_27262241/article/details/84291471

这篇博客详细记录了Gtsam库的学习过程，包括cmake的引入、因子factor的预定义与生成、初值和噪声的定义、优化方法如GaussNewton法和LevenbergMarquardt法的应用，还探讨了边缘化、iSAM更新过程以及如何使用LevenbergMarquardtOptimizer进行优化。内容深入到Gtsam的内部机制，适合SLAM和后端开发的学习者参考。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

Gtsam学习笔记

文章目录

Gtsam学习笔记

cmake引入

# 寻找第三方库，使用大小写都可以，这里列举了两种方式
find_package(Boost COMPONENTS thread filesystem date_time system REQUIRED)
FIND_PACKAGE(GTSAM REQUIRED)

# 包含第三方库头文件路径，可以使用绝对路径
INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${GTSAM_INCLUDE_DIR})
INCLUDE_DIRECTORIES("/usr/include/eigen3")

add_executable(gtsam_test main.cpp)
# 链接库 
target_link_libraries(gtsam_test ${Boost_LIBRARIES} -lgtsam -ltbb)
install(TARGETS gtsam_test RUNTIME DESTINATION bin)

clion没法识别的话就重启一下，可能是有的玩意没刷新出来。

因子factor

预定义的factor

头文件
#include <gtsam/slam/BetweenFactor.h> //二元因子，位姿之间，回环之间
#include <gtsam/slam/PriorFactor.h> //一元因子，系统先验
//定义因子图
NonlinearFactorGraph graph;
//噪声定义 对角线矩阵：
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
//在因子图中加入一个因子
//   二元因子
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0     ), model);
			//参数解释 ^ ：因子类型  ^边		key 1、2  ^ 边的值			^ 噪声模型
//   一元因子
graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);

生成factor

  Rot2 prior = Rot2::fromAngle(30 * degree);
  prior.print("goal angle"); 
  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree);
  Symbol key('x',1); // 一个key就是一个label
  PriorFactor<Rot2> factor(key, prior, model);

初值定义

一个图中要给每一个变量赋予一个初始值。

  Values initialEstimate; //定义初始值容器
  initialEstimate.insert(1, Pose2(0.5, 0.0,  0.2   )); //加入一个变量 arg1：变量的标签 arg2:这个变量的值

噪声定义

gtsam中的噪声有很多类型

namespace NM = gtsam::noiseModel;

// Set Noise parameters
Vector priorSigmas = Vector3(1,1,M_PI);
Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
const NM::Base::shared_ptr // 基类型
  priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior 对角线噪声
  odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
  gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust 各项同性噪声
  tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust