优化库——GTSAM（一）

前言

本文主要从代码架构方面，简要了解一下框架，使得使用的时候“踏实一点”，想要细致了解以下的，我在后面参考链接里面放置一些推荐链接，可以作为了解

以example_odomtry为例讲解一下主要流程

• 0 创建图对象
• 1 添加先验一元边
• 2 添加约束因子
• 3 设定初值（观测值）
• 4 进行优化
  

1-1 FactorGraph & Factor

图对象的顶层基类为FactorGraph，常用的非线性图属于派生类NonlinearFactorGraph-->FactorGraph

所有的因子都继承一个顶层因子基类Factor，

在图对象中有一个变量FastVector factors_会存储所有加入的约束因子，对于sharedFactor这里被定义为智能指针引用typedef boost::shared_ptr sharedFactor。

需要了解一下因子的继承关系，有助于理解如何写自定义的因子

继承关系如下（最后一个字母表示变量个数）：
NoiseModelFactor1/2/3/4/5/6（在这里开始出现纯虚函数evaluateError函数）
	-->NoiseModelFactor
	-->NonlinearFactor
	-->Factor
//==============================================================================
举例（IMU因子）：
ImuFactor: public NoiseModelFactor5
  /**
   * Constructor
   * @param pose_i Previous pose key
   * @param vel_i  Previous velocity key
   * @param pose_j Current pose key
   * @param vel_j  Current velocity key
   * @param bias   Previous bias key
   */
  ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
      const PreintegratedImuMeasurements& preintegratedMeasurements);

1-2 evaluateError

evaluateError 函数需要传递几个参数，这个是由继承的基类决定的（参考上一小节），继承的基类里面已经定义好了相应的纯虚函数接口
=================
Factor的继承关系如下：

继承关系如下（最后一个字母表示变量个数）：
NoiseModelFactor1/2/3/4/5/6（在这里开始出现纯虚函数evaluateError函数）
	-->NoiseModelFactor
	-->NonlinearFactor
	-->Factor

形成了一一对应关系，需要几个变量，后缀就为几，对应的函数evaluateError就有一个雅克比传参

一些推荐

GTSAM 库的学习建议

GTSAM/iSAM 原理学习

Gtsam学习笔记
Github:
https://github.com/borglab/gtsam
Paopao:
https://www.sohu.com/a/133646392_715754