Gtsam学习笔记

Gtsam学习笔记

文章目录

  • Gtsam学习笔记
      • cmake引入
      • 因子factor
        • 预定义的factor
        • 生成factor
      • 初值定义
      • 噪声定义
      • 优化方法
        • GaussNewton法
        • LevenbergMarquardt法
      • 边缘化 marginal
      • 读写 g2o文件
      • iSAM 更新过程
      • LevenbergMarquardtOptimizer

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

优化方法

GaussNewton法

引入 #include

GaussNewtonParams parameters;//参数对象
parameters.relativeErrorTol = 1e-5; // 迭代变化量小于这个值就退出
parameters.maxIterations = 100; //最大迭代次数
//创建一个高斯牛顿优化器 arg:		因子图 初值				参数对象
GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);
//求出优化解 得到变量的最优值
Values result = optimizer.optimize();

LevenbergMarquardt法

引入 #include

LevenbergMarquardtOptimizer optimizer(graph,initialEstimate);
Values result=optimizer.optimize(); 

边缘化 marginal

    Marginals marginals(graph,result);
    for (int j = 1; j < 4; ++j) {
        boost::format fmt("\n x%1% covariance: \n %2% \n");
        fmt%j%marginals.marginalCovariance(j); //得到每个变量的协方差
        cout<

读写 g2o文件

#include  //引入头文件
//读
NonlinearFactorGraph::shared_ptr graph1;
Values::shared_ptr initialEstamate1;
string g2ofile=findExampleDataFile("noisyToyGraph.txt");
bool is3D =false;
boost::tie(graph1,initialEstamate1)=readG2o(g2ofile,is3D);
//写 g2o文件
writeG2o(graph,result,"graph2g2o.g2o"); //args: 因子图 ,变量结果,目标文件名

iSAM 更新过程

在文件ISAM2.cpp的update函数中

  1. 添加新的因子 Impl::AddFactorsStep1
    F a c t o r s : = F a c t o r F a c t o r ′ Factors:=Factor Factor' Factors:=FactorFactor
   //关键变量
   ISAM2Result result;
   isam.update(graph, initialEstimate);
   ISAM2Params params_; //在定义ISAM2实例的时候存储参数的。
   	//  parameters.relinearizeThreshold = 0.01;
     	// parameters.relinearizeSkip = 1;
   Impl //Internal implementation functions 内部实施函数
   useUnusedSlots //slot 插槽
  1. 初始化新的变量 Impl::AddVariables
    Θ : = Θ ∪ Θ n e w . \Theta:=\Theta\cup\Theta_{new}. Θ:=ΘΘnew.

  2. 标记线性化更新

  3. 标记 变化量大于阈值的key
    β : J = { Δ j ∈ Δ ∣ Δ j ≥ β } . \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. β:J={ΔjΔΔjβ}.

  4. 标记所有与变量相关小团体clique 以及他们的所有父节点

  5. 更新线性化点对于标记的变量
    Θ J : = Θ J + Δ J . \Theta_{J}:=\Theta_{J}+\Delta_{J}. ΘJ:=ΘJ+ΔJ.

  6. 线性化 新的因子

  7. 重新整理贝叶斯数的树顶

LevenbergMarquardtOptimizer

LevenbergMarquardtOptimizer继承自 NonlinearOptimizeroptimize->defaultOptimize iterate();(这个函数是在子类中实现的,不是在父类中实现的)

Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
virtual const Values& optimize() { defaultOptimize(); return values(); }

在iterate中:
KaTeX parse error: Undefined control sequence: \f at position 2: \̲f̲ ̲Ax-b \approx h(…

1、
GaussianFactorGraph::shared_ptr linear = linearize();
就是对每一个因子进行线性化:NonlinearFactor::linearize()
2、
whitenedError 白化后的噪声

你可能感兴趣的:(SLAM)