GTSAM学习笔记（因子图优化/非线性优化）

文档总结：

因子图是图模型，非常适合用于 解决 复杂的估计问题，如SLAM或者SFM.

因子图是由变量和因子构成的双边图。变量代表未知的需要优化的量，因子代表这些量之间的约束，因子通过测量或者先验的知识获得。

GTSAM也提供了MATLAB版本的接口，可以快速开发、可视化、用户交互。

1 Factor Graphs

讲了贝叶斯网络。

2 Modeling Robot Motion

2.1 Modeling with Factor Graphs

x1 x2 x3：机器人位姿

o1 o2：里程计

f0是一元因子，f1 f2是二元因子。

2.3 Factor Graphs versus Values

两个重要的设计理念：

1、因子图及其在代码中的实施指定了机器人整个轨迹的联合概率分布, 而不仅仅是最后的姿势（和8.4中叙述的对应起来了，如果只是最后的姿势，那就是滤波）。

2、因子图只是概率密度的规范，相应的FactorGraph类和派生类并不包含解决方案。

请记住，在设计GTSAM时，我们采用了与数学对象相对应的类的函数方法，这些对象通常是不可变的。应该把因子图视为要用值的函数，而不是要更改的对象。

2.4 Non-linear Optimization in GTSAM

GTSAM需要执行非线性优化的原因是里程计因子f1 f2 是非线性的，因为他们包含了机器人的朝向。

优化类对此图进行线性化, 可能是多次线性化, 以最大限度地减少由因子指定的非线性平方误差。（？？？没看懂这句话）

2.5 Full Posterior Inference

GTSAM也可以用于计算在整合了所有的测量值之后的每个位姿的协方差 矩阵  。

3 Robot Localization

3.1 Unary Measurement Factors

一元测量因子。为什么叫一元的呢，因为它只跟一个变量节点有关。因此也可以有二元的，多元的因子。

这部分往因子图中添加测量，这些测量会帮助我们随着时间的推移定位机器人。该示例还可作为创建新因子类型的教程

。

3.2 定义自定义的因子

这部分主要讲的就是自定义因子。

m是测量值，q是未知变量（优化变量），h(q)是测量函数。注意m是已知的，L(q;m)只是q的函数，这也解释了为什么它是一个一元因子f(q)

具体代码见 examples/LocalizationExample.cpp

一定要重写两个函数：

Vector evaluateError( const Pose2& q, boost::optional<Matrix&> H = boost::none)  const 。

传入需要优化的变量和雅可比矩阵

4 PoseSLAM

4.1 Loop Closure Constraints

具体代码在examples/Pose2SLAMExample.cpp

4.2 Using the MATLAB Interface

MATLAB接口和C++接口的一些对比。

4.3 读取和优化位姿图（这里的例子可以跑跑熟悉一下）

这表明了位姿图是可以存储的。因为它就是变量、因子和因子之间的关系。

MATLAB极大地加快了开发速度，并且提供了更 有效的  图形输出。

4.4 PoseSLAM in 3D

PoseSLAM可以很容易地扩展到3D位姿，不过需要注意的是要更新3D旋转。

GTSAM 支持四元数和3x3 旋转矩阵来表示3D 旋转。通过编译标志 GTSAM _ USE _ QU来选择。

5、Landmark-based SLAM

在以地标为基础的SLAM中，我们显式地构建了一个带有观测到地标(landmark)的 地图  , 它在因子图中引入了除机器人姿势之外的第二种变量。注意这里与Robot Localization 中自定义factor的不同。

添加到图里的类型也不同了

5.2 Of Keys and Symbols

主要讲解了MATLAB中的symbol和C++中的Symbol。(the symbol function in MATLAB, and the Symbol type in C++)

MATLAB例子 gtsam_examples/PlanarSLAMExample_graph.m

6 Structure from Motion

没有细看，暂时还没有用到

7 iSAM: Incremental Smoothing and Mapping

GTSAM 提供了一种基于更高级的图模型贝叶斯树的增量推理算法, 该算法由 iSAM 算法保持更新。(incremental Smoothing and Mapping, see Kaess et al. (2008, 2012) )

对于实时运行的移动机器人来说, 一旦新的传感器测量结果出来, 就可以访问更新后的地图是很重要的。iSAM 以高效的方式使地图保持最新状态。

例12展示了怎样在一个简单的视觉SLAM中使用iSAM。

examples/VisualISAMExample.cpp

主要是定义了一个NonlinearISAM 对象

此参数的相关值取决于问题的非线性程度以及您希望在每个步骤中与黄金标准解决方案的距离。在 iSAM 2.0 中, 不需要此参数, 因为 iSAM2 会自动确定何时需要线性化以及针对哪些变量。

8 More Applications

8.1 Conjugate Gradient Optimization

GTSAM 还包括用于解决大规模 SLAM 问题的高效预置共轭梯度 (PCG) 方法。虽然直接方法在文献中很流行, 表现出二次收敛性并且对于稀疏问题来说是相当有效的, 但它们通常需要大量的存储和被发现的有效的消除序列。相反, 迭代优化方法只需要访问梯度, 内存占用小, 但可能会导致收敛性较差。我们的方法, 子图预处理, 详细解释在Dellaert,Jian 等人结合了直接和迭代方法的优点, 通过识别一个可以使用直接方法轻松解决的子问题, 并使用 PCG 求解剩余部分。简单的子问题对应于生成树、平面子图或任何其他可以有效解决的子结构。

8.2 Visual Odometry

有c++和MATLAB例子代码

8.3 Visual SLAM

注意视觉里程计和视觉slam不一样。里程计只是前端，SLAM是前端加后端。

8.4 Fixed-lag Smoothing and Filtering

GTSAM 可以很容易地执行递归估计, 在因子图中只保留一个姿势子集, 而剩余的姿势则被边缘化。在上面的所有示例中，我们使用所有可用的测量显式地优化所有变量，这被称为平滑，因为轨迹被“平滑”了，这就是GTSAM的名称(GT Smoothing and Mapping)的由来。相反, 当只有最后几个姿势被保留在图中时, 这被称为固定滞后平滑(

Fixed-lag Smoothing )。最后, 当只保留了最近的一个姿势时, 这被称为滤波，事实上, 最开始的SLAM就是基于滤波的。

免责声明：本文系网络转载或改编，未找到原创作者，版权归原作者所有。如涉及版权，请联系删