引言
关于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完整优化流程
-
添加Node和Factor
slam.add_node();
slam.add_factor();
-
更新因子图, 具体更新因子图时分为两种情况,一种是批量更新,另一种是增量更新
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();
-
重复步骤1和步骤2,直至所有节点都添加完成。
参考资料
- 论文: Michael Kaess. iSAM: Incremental Smoothing and Mapping
- 代码: https://github.com/ori-drs/isam