因子图原理推导及Gtsam简要使用说明

因子图推导及Gtsam简要使用说明

参考链接：
Theory, Programming, and Applications of Factor Graph, Jing Dong；
Gtsam学习笔记——三川小哥；

因子图原理推导

因子图建模

下图为一个简单SLAM文件的抽象。蓝色三角形表示世界中被观测的路标点，黄色五边形为各个时刻的机器人姿态。红色箭头与绿色箭头分别为路标点观测与相邻位置姿态变换观测。

将上述简单SLAM问题表述为贝叶斯单向图有以下形式。将位姿与观测均表示为变量形式。箭头表明了由姿态生成观测的因果关系。

用$X$表示位姿，$Z$表示观测，可以得出以下关系
$$P(X,Z)=P(Z|X)P(X)$$
位姿与观测的联合概率将由位姿的概率与已知位姿下的观测条件概率乘积获得。其中$P(Z|X)$可展开为：
$$P(Z|X)=\prod _i{P}_i(Z_i|X_i)=P(z_{11}|l_1,x_1)P(z_{12}|l_1,x_2)P(z_{13}|l_2,x_3)P(z_{21}|x_1,x_2)P(z_{22}|x_2,x_3)$$
然而，我们的问题是求取已知观测量的情况下各点位姿的概率，即要求$P(X|Z)$
$$P(X|Z)=\frac{P(Z|X)P(X)}{P(Z)}$$
对于目标为求解X的该问题，$P(Z)$与问题无关，故可省略。
$$\frac{P(Z|X)P(X)}{P(Z)}\propto P(Z|X)P(X)$$
其中$P(X)$表示的先验在实际问题中并不存在，故可暂时舍去。
由此，SLAM问题简化为了求取$X^{\ast }$的极大似然概率问题，其中：
$$X^{\ast }=\underset{x}{\mathrm{argmax}}P(Z|X)=\underset{x}{\mathrm{argmax}}\prod _i{P}_i(Z_i|X_i)$$
将上式中每一个$P_i(Z_i|X_i)$抽象为一个因子，将各个因子记为
$${\varphi }_i\dot{=}\psi (Z_i,X_i)\propto P_i(Z_i|X_i)$$
因此，问题可以进一步转化用因子表示的以下形式：
$$X^{\ast }=\underset{x}{\mathrm{argmax}}\prod _i{P}_i(Z_i|X_i)=\underset{x}{\mathrm{argmax}}\prod _i{\varphi }_i$$
因子图抽象为下图形式：

根据因子图进一步展开${\prod }_i{\varphi }_i$可以得到以下形式：
$$\prod _i{\varphi }_i\dot{=}\varphi (x_1,l_1)\varphi (x_2,l_1)\varphi (x_3,l_2)\varphi (x_1,x_2)\varphi (x_2,x_3)\varphi (x_1)$$
其中：
$\varphi (x_1,l_1)\varphi (x_2,l_1)\varphi (x_3,l_2)$为观测因子；
$\varphi (x_1,x_2)\varphi (x_2,x_3)$为里程计因子；
$\varphi (x_1)$为先验因子。

因子图问题转化

对于每一个因子，都为其应用适当误差模型对齐进行建模。
使用最常见的高斯分布对其进行建模,则有：
$${\varphi }_i=exp(-\frac{1}{2}||f_i|{|}_{\Sigma }^2)$$
对于不同因子类型，有不同的误差函数模型。
先验因子：$f_{prior}(x_i)=x_i-z$
里程计因子：$f_{odometry}(x_i,x_{i+1})=(x_{i+1}-x_i)-z$
观测因子：$f_{landmark}(x_i,l_j)=projection(x_i,l_j)-z$
对于因子图问题进行对数化，可化简问题为：
$$X^{\ast }=\underset{x}{\mathrm{argmax}}\prod _i{\varphi }_i=\underset{x}{\mathrm{argmax}}\log (\prod _i{\varphi }_i)=\underset{x}{\mathrm{argmin}}\sum _i||f_i|{|}_{\Sigma }^2$$
至此，因子图问题可以转化为马氏距离(马氏范数)非线性最小二乘的形式。

接下来对上式进行线性化。
记所有因子的误差函数为统一格式：
$$f_i=h_i(X_i)-z_i$$
式中：
$h_i(X_i)$表示第i个因子的预测，$z_i$表示第i个因子的观测。

对各$h_i(X_i)$在各点附近进行一阶泰勒展开：
$$f_i=h_i(X_i)-z_i=h_i(X_i^0)+J_i{\Delta }_i-z_i$$
其中：
雅可比矩阵$J_i$是在给定线性化点处$X_i^0$对预测函数$h_i(\cdot )$的多变量偏微分：
$$J_i=\frac{\partial h_i(X_i)}{\partial X_i}{|}_{X_i^0}$$
${\Delta }_i$表示为状态更新量：
$${\Delta }_i=X_i-X_i^0$$

将泰勒展开后的因子误差函数带入因子图问题中：
$$X^{\ast }=\underset{\Delta }{\mathrm{argmin}}\sum _i||h_i(X_i^0)+J_i{\Delta }_i-z_i|{|}_{\Sigma }^2=\underset{\Delta }{\mathrm{argmin}}\sum _i||J_i{\Delta }_i-(z_i-h_i(X_i^0))|{|}_{\Sigma }^2$$
对于马氏范数到2范数有以下转化关系：
$$||e|{|}_{\Sigma }^2=e^T{\Sigma }^{-1}e=({\Sigma }^{-0.5}e{)}^T({\Sigma }^{-0.5}e)=||{\Sigma }^{-0.5}e|{|}_2^2$$
所以：
$$X^{\ast }=\underset{\Delta }{\mathrm{argmin}}\sum _i||{\Sigma }^{-0.5}{J}_i{\Delta }_i-{\Sigma }^{-0.5}(z_i-h_i(X_i^0))|{|}_2^2$$

记：
$$\{\begin{array}{l}{A}_i={\Sigma }^{-0.5}{J}_i\\ b_i={\Sigma }^{-0.5}(z_i-h_i(X_i^0))\end{array}$$
以上过程称为白化，通过该过程消去协方差矩阵。

则因子图问题转化为标准线性最小二乘问题：
$$X^{\ast }=\underset{\Delta }{\mathrm{argmin}}\sum _i||A_i{\Delta }_i-b_i|{|}_2^2$$

GtSAM使用简析

GtSAM 是目前因子图增量平滑方面成果的集成，其中囊括了从isam1到isam2、贝叶斯树的优化以及之后出现的流行优化等优化方法。无论是研究还是应用因子图优化，Gtsam都是不二之选。

以下示例以LOAM中的用法为例。

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)

因子图构建

创建因子图问题

//头文件
#include <gtsam/geometry/Rot3.h>  // 三维旋转量
#include <gtsam/geometry/Pose3.h>  // 三维平移量
#include <gtsam/slam/PriorFactor.h>  // 一元因子，系统先验
#include <gtsam/slam/BetweenFactor.h>  // 二元因子，位姿之间，回环之间
#include <gtsam/nonlinear/NonlinearFactorGraph.h>  // 因子图
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>  // LM求解器
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>

#include <gtsam/nonlinear/ISAM2.h>  // 优化问题主体

// 此处案例为LegoLOAM及lio-SAM采用的增量更新因子图方案

//创建变量
NonlinearFactorGraph gtSAMgraph;  // 因子图 增量更新下用于存放每次更新的增量
Values initialEstimate;  // 各因子优化初值
Values optimizedEstimate;
ISAM2 *isam;  // 优化问题主体
Values isamCurrentEstimate;  // 存放各因子优化结果
//创建因子噪声
noiseModel::Diagonal::shared_ptr priorNoise;
noiseModel::Diagonal::shared_ptr odometryNoise;
noiseModel::Diagonal::shared_ptr constraintNoise;

// 定义噪声
gtsam::Vector Vector6(6);
Vector6 << 1e-6, 1e-6, 1e-6, 1e-8, 1e-8, 1e-6;
priorNoise = noiseModel::Diagonal::Variances(Vector6);
odometryNoise = noiseModel::Diagonal::Variances(Vector6);

// 初始化优化问题
ISAM2Params parameters;
parameters.relinearizeThreshold = 0.01;
parameters.relinearizeSkip = 1;
isam = new ISAM2(parameters);

因子图新增一元边

// 新增系统先验(首点因子，即一元边)
// PriorFactor<因子类型>(key因子编号，约束测量值，噪声)
gtSAMgraph.add(PriorFactor<Pose3>(0, Pose3(Rot3::RzRyRx(dRx, dRy, dRz),
                                Point3(dTransx, dTransy, dTransz)), priorNoise));
// 新增该因子优化初值
// initialEstimate.insert(key因子编号，位姿优化初值）
initialEstimate.insert(0, Pose3(Rot3::RzRyRx(dSumRx, dSumRy, dSumRz),
                                Point3(dSumTransx, dSumTransy, dSumTransz)));

因子图新增二元边

// 新增位姿约束(位姿之间，回环之间，即二元边)
// BetweenFactor<因子类型>(key因子编号1，key因子编号1，1-2的约束测量值，噪声)
gtSAMgraph.add(BetweenFactor<Pose3>(iKey1, iKey2, 
                                    poseFrom.between(poseTo), odometryNoise));
// 新增新出现因子的优化初值(绝对位姿)
// initialEstimate.insert(key因子编号，位姿优化初值）
initialEstimate.insert(iKey2,Pose3(Rot3::RzRyRx(dRx, dRy, dRz),
                            Point3(dTransx, dTransy, dTransz)));

因子图求解

// 每次新增因子后，进行优化问题更新
isam->update(gtSAMgraph, initialEstimate);
isam->update();
// 清空增量容器
gtSAMgraph.resize(0);
initialEstimate.clear();
// 优化问题求解
isamCurrentEstimate = isam->calculateEstimate();
// 获取优化结果
int numPoses = isamCurrentEstimate.size();
for (int i = 0; i < numPoses; ++i)
{
    points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().x();
    points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().y();
    points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().z();

    points[i].roll = isamCurrentEstimate.at<Pose3>(i).rotation().roll();
    points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().pitch();
    points[i].yaw = isamCurrentEstimate.at<Pose3>(i).rotation().yaw();
}