ISAM1代码解析

引言

关于ISAM1的理论可参考我的上一篇博客或者ISAM1论文，本篇博客主要梳理一下ISAM1代码中的各种类和增量更新的代码运行流程。

基类：Element

这个类是ISAM1中最基础的一个类，Factor和Node都是继承自它。其成员变量有四个：_name, _start, _id和 _dim

class Element
{
    const char* _name;  // 用于描述一个factor或者Node的名字
    int _start;         // 在求雅可比矩阵时会用到，用来表示Node对应雅可比矩阵的那一列
    int _id;            // 表示Node的id号或者Factor的id号
    int _dim;           // 表示Node的维度或者Factor的残差维度
};

派生类1：Node

继承自Element，表示SLAM问题中的状态量，其中有三个成员： _next_id, _deleted, _factors

class Node : public Element
{
    static int _next_id;            // 每次构造新的Node时，都会自增，并用这个变量值作为node id
    bool _deleted;                  // 表示这个Node是否被删除了
    std::list<Factor*> _factors;    // 表示这个Node所连接的所有factor

};

派生的派生： NodeT

继承自Node，这是一个模版类，用于表示每个节点的具体类型，其中包含两个成员变量： _value, _value0
可以通过模版参数配置Pose3d_Node, Point3d_Node等。

template <class T>
class Node : public Node
{
    T* _value;          // 表示节点的当前估计值
    T* _value0;         // 表示节点的线性化点
};

基类： Function

这个类是一个纯虚函数，主要负责提供各种纯虚函数。

派生类2： Factor

继承自Element和Factor，用于表示因子图中节点间的测量信息，成员变量有：ptr_cost_func, _next_id, _deleted, _noise, _nodes。

class Factor : public Element, Function
{
    cost_func_t* ptr_cost_func;     // 表示鲁棒核函数
    static int _next_id;            // 每次构造一个新的Factor时，都会自增，并用这个变量作为新的factor的id号
    bool _deleted;                  // 表示这个factor是否被删除了
    const Noise _noise;             // 表示协方差矩阵
    std::vector<Node*> _nodes;      // 表示这个factor所连接的所有节点
};

派生的派生： FactorT

继承自Factor，是一个模版类，可表示特定的测量值类型，成员变量有：_measure;
可以通过配置模版参数，再派生出特定的Factor，比如Pose3d_Pose3d_factor等。

template <class T>
class FactorT : public Factor
{
    const T _measure;       // 表示测量值
};

基类： Jacobian

这个类就是我们常用的雅可比矩阵，但这个是小雅可比矩阵，一个Jacobian与一个Factor相对应，其中的成员变量为：_dimtotal, _terms, _residual;

class Jacobian
{
    int _dimtotal;      // 表示该factor所连接的所有变量的维度总和
    Terms _terms;       // 表示残差关于每个Node的小雅可比矩阵块
    Eigen::VectorXd;    // 表示残差
};

基类： Cholesky

这个类是专门用于Cholesky分解的，有两种模式，一种是基于CSparse的，一种是基于CHOLMOD的

派生类1： CholeskyImpl

这个就是基于CHOLMOD的派生类，用于Cholesky分解，这个是默认的模式

class CholeskyImpl : public Cholesky
{
    cholmod_sparse* _L;
    cholmod_dense* _rhs;
    cholmod_common Common;  // 这是三个成员都是用于cholmod分解的
    int* _order;            // 为了防止fill-in，在做Cholesky分解时用做重排序，这个变量表示变量重排序后的顺序，后续需要将它与原有的顺序对应起来。
};

派生类2： CholeskyImplCSparse

这个就是基于CSparse的派生类，也用于Cholesky分解

class CholeskyImplCSparse : public Cholesky
{
    cs*     _L;
    double* _rhs;
    int*    _order;     // 同样也是用于表示变量重排序后的变量顺序
};

基类： SparseVector

这个类用于存储稀疏R矩阵中的每一行，并且只保存非零项，成员变量有：_nnz, _nnz_max, _indices, _values

class SparseVector
{
    int _nnz;           // 表示非零项的个数
    int _nnz_max;       // 表示最大的非零项的个数
    int* _indices;      // 由于保存非零项，所以需要和原来的索引号对应起来
    double* _values;    // 所有非零项的值
};

基类： SparseMatrix

用于表示稀疏矩阵R

class SparseMatrix
{
    int _num_rows;          // 表示矩阵行个数
    int _nnz_cols;          // 表示矩阵列个数
    int _max_num_rows;      // 已分配的行索引数
    int _max_num_cols;      // 已分配的列索引数
    SparseVector_p* _rows;  // 行矩阵的指针
};

派生类： OrderedSparseMatrix

有序的稀疏矩阵

class OrderedSparaseMatrix : public SparseMatrix
{
    int* _r_to_a;       // R矩阵的列索引号到A矩阵的列索引号的对应关系
    int* _a_to_r;       // A矩阵的列索引号到R矩阵的列索引号的对应关系

};

派生的派生： SparseSystem

通过Givens旋转用来更新R矩阵的一个类，以及求解Rx=b的类

class SparseSystem : public OrderedSparseMatrix 
{
    Eigen::Vectorxd _rhs;   // Rx = b中的b
};

基类： OptimizationInterface

一个抽象接口类，提供了各种非线性优化接口，成员变量只有一个：_R

class OptimizationInterface
{
    SparseSystem _R;    // 基于当前线性化点因式分解后的雅可比矩阵
};

基类： Graph

一个抽象接口类，提供了各种非线性优化接口，成员变量只有一个：_R

class OptimizationInterface
{
    SparseSystem _R;    // 基于当前线性化点因式分解后的雅可比矩阵
};

基类： Optimizer

用于执行各种优化操作的类，比如执行高斯牛顿的增量、增量更新R矩阵、重新做线性化等操作

class Optimizer
{
    OptimizationInterface& function_system;     // 提供用于操作SLAM中线性化系统的各种接口
    Cholesky* _cholesky;                        // 用于完成Cholesky分解
};

派生类： Slam

整个优化系统的入口类

class Slam : public Graph, OptimizationInterface
{ 
    int _step;                      // 用于统计更新次数，通过这个标志位周期性执行重新线性化
    Properties _prop;               // 各种优化器相关的参数
    Covariances _covariances;       // 协方差类
    int _dim_nodes;                 // 所有优化节点的总维度
    int _dim_measure;               // 所有残差的总维度
    int _num_new_measurements;      // 距离上次更新后新添加的残差个数
    int _num_new_rows;              // 距离上次更新后新添加的节点个数
    Optimizer _opt;                 // 执行各种优化操作的类
};

ISAM完整优化流程

1. 添加Node和Factor

   slam.add_node();

   slam.add_factor();

2. 更新因子图, 具体更新因子图时分为两种情况，一种是批量更新，另一种是增量更新

   slam.update();

   2.1 批量更新，每更新一定次数就会运行批量更新，批量更新会重新重新做线性化，完整计算雅可比矩阵和R矩阵

    batch_optimization_step();
   
    2.1.1 重新做线性化
   
        _opt.relinearize();
   
    2.1.2 重新计算$J^TJ \delta x = b$，用$\delta x$更新节点
   
        function_system.apply_exmap(h_gn);
   

   2.2 增量式更新，会通过Givens旋转将新来的测量值直接更新到R矩阵中

    2.2.1 计算新的factor所对应的雅可比矩阵
   
        SparseSystem jac_new = jacobian_partial();
   
    2.2.2 增量式更新优化器，将新计算的雅可比通过Givens旋转添加到R矩阵中
   
        _opt.augment_sparse_linear_system();
   
    2.2.3 计算$R \delta x = d$，用$\delta x$更新节点
   
        _opt.update_estimate();
   

3. 重复步骤1和步骤2，直至所有节点都添加完成。

参考资料

1. 论文： Michael Kaess. iSAM: Incremental Smoothing and Mapping
2. 代码： https://github.com/ori-drs/isam