GPGN-高斯过程与SLAM后端优化求解

高斯过程-高斯牛顿优化求解SLAM后端优化问题

GPGN与SLAM

高斯过程(GP)此处就不再赘述啦，是一个相对基础的概念且目前许多领域均在使用。本文提到的GPGN指的是利用高斯过程(GP)能够处理连续时间的特性，来对SLAM中后端轨迹优化问题进行建模，并且使用高斯牛顿(GN)方法进行求解，从而到达非参数、连续时间、非线性、批量估计这样子一个目的。

GPGN的好处是在估计完轨迹后，能够给出两个确定轨迹之间的任意时刻的轨迹以及其不确定度(协方差)；与之相比的B-Spline就比较难以给出不确定度。另外GPGN因为引入GP prior先验因子，相当于引入了一个平滑因子，一般来说因为机器人不会有较大的加速度，所以这种优化也许会更加贴合物理规则。

本文会从几篇GPGN的文章出发，先推导GPGN一般性公式，然后介绍在线性时变随机微分方程(LTV-SDE)的先验下的可以快速计算逆矩阵的原理，并引入恒速先验然后推导出SE3空间上的更新公式，最后来看看名为STEAM的一个库。

另外，本文涉及了多篇文章，这些文章里符号定义是不同的，但是为了上下文推导统一，所以本文内符号是一致的，因此可能会和原文不尽相同。

（另外因为公式有点太多了，有些矩阵没有加粗，就稍微牺牲下严谨性吧，懒得搞了欸）

另外，再叠一个甲，因为这篇文章因为公式太多，再加上有点拖延，陆陆续续写了挺长时间，中间思路可能会有所跳脱，也说不定会出现一些错误，一切仅供参考。

基于Basis Function的GPGN推导

本部分推导主要来自于Gaussian Process Gauss–Newton for non-parametric simultaneous localization and mapping这篇文章，由于公式较多，略去一部分推导，直接给出结果。

高斯过程对运动过程建模

我们不妨将机器人的运动使用高斯过程来建模，记机器人的离散时间概率分布为高斯随机变量，而对应的地标则记为，这样做的目的是把时变量和时不变量分开。

与此同时，我们记对应的高斯过程为

以及对应的观测方程：

上述将机器人状态变量与地标变量分开，是为了时变与时不变分离，但是在优化时两个都需要同时更新，所以我们可以定义一个新的变量：

其中

这样，我们就可以将上面的观测方程简写成如下形式：

其中，也可以写成高斯过程的形式：

参数化表示

接下来，可以定义一系列基函数和对应系数，并用来表示，类似傅里叶级数用一系列三角函数逼近任意函数一样，这里用很多基函数乘以对应系数来逼近，这里的维度会非常大，甚至可能是无穷维，这样做的目的是我们可以直接优化系数，而不用考虑任何原函数本来的性质：

即：

在使用基函数表示后，观测方程又有了新的变化：

按照高斯分布的计算公式，此时的对应的均值函数和协方差函数分别为：

这里和分别为：

到这里，我们其实已经完成了对高斯过程的基函数参数化，因为我们引入了一个高斯先验，所以我们最终的优化目标一共两项，第一项是观测方程的似然函数，第二项是高斯过程的先验分布：

因为我们的变量是，所以理论上只需要更新即可，但是我们已经用参数化来表示了，所以先推导出与相关的更新：

其中是对当前系数的估计，而则是更新扰动，即梯度下降过程中的更新量，此时我们对观测方程进行易解泰勒展开：

并将其回代回优化目标中，就能得到优化目标包含更新扰动的形式：

其中：

然后我们对扰动求导：

我们要求出最优的，所以一定是导数为0的极值点，所以解得：

通过上式就可以完成对参数的更新了，但是前文我们也说过，基函数是无穷个的，对应的维度也非常大，我们没法直接去更新，还需要去参数化，从而能够直接更新。

核函数去参数化

因为，而我们已经有了的更新，所以我们也能直接写出：

然后利用下面的公式回代：

可以解出的具体更新公式：

其中：

这里和是将所有时刻拼接成一个列向量后再乘积，表示全局之间的关系；而这里和则是使用此时刻与列向量相乘，表示当前时刻与其他所有时刻的关系。注意，这里只有是无穷维度的，似乎，我们还是要计算无穷维的矩阵乘法，但，因此，我们可以使用核函数来假装我们计算过了，也就是直接给出一个的矩阵，并认为这就是我们计算的结果就可以了。这种技巧在很多地方都很有用，比如SVM的推导中就使用了核函数。

当然，如果我们只考虑测量时刻的状态扰动，即，那么更新公式还会更简单：

这两步我们使用了和这两个SMW等式，读者不妨将其作为习题自行尝试（：。

优化流程

初始化
计算必要的系数，包括使用核技巧
更新
重复上述步骤直到收敛

计算插值

在上式完成收敛后，我们已经得到高斯过程的超参数，可以插值出任意一点的位姿了，具体推导可以见原论文，总之最后的插值公式如下：

实际上，GPGN还能同时获取到协方差，也就是对应位置的不确定度，这部分可以参见原文，但是因为公式比较长，这里就先省略了。

LTV-SDE与恒速度先验

前文给出的一般性GPGN在推导上没有问题，但是它却并不好用，因为它太一般化了，既没有给出详尽的李群上运动学的更新公式，也没有给出具体的先验假设，这导致它的在更新时的系数矩阵的左上角是稠密的，在求逆矩阵的时候计算量较大。原文中给出了一个白噪声加速度过程的先验假设的例子，也就是，这个假设太强了，一般不具备实用性。

而基于线性时变随机微分方程(LTV-SDE)的恒速度先验假设则是一个比较好的假设，具体可以参见这篇文章：Batch Continuous-Time Trajectory Estimation as Exactly Sparse Gaussian Process Regression，LTV-SDE假设可以最终在计算逆矩阵时有一个稀疏的块-对角矩阵，能够大大降低计算逆矩阵的复杂度。

另外，注意前文推到中同时包含时变的和时不变的两部分，而这里我们只会考虑时变量，因此不要和上文的搞混了。

逆核矩阵稀疏性

我们先不考虑恒速度先验，先来看看LTV-SDE，其定义为：

这里的和是时变的系统系数矩阵，是一个控制输入，是一个线性时变噪声矩阵，是一个高斯白噪声过程，即，这里的是一个功率谱密度矩阵也就是一个超参数，是一个狄拉克函数，即只有在的时刻才会有输出。

可以看到，我们假设系统状态变量的导数由系统状态变量、控制量以及噪声项构成，通过对此进行积分，那么我们就能状态变量本身：

这里的是状态转移矩阵（前文我们虽然已经把当作基函数这样子用过了，但是基实际最终的结果跟基函数无关，所以这里再赋予他一个新的定义，注意别搞混了），一般来说可以定义为:

但是别忘了我们的目的，我们希望获取到一个先验，也就是，而基于LTV-SDE假设推导出来的系统状态变量是一个带有噪声项的积分式，不过这个噪声项的均值是0，所以通过计算期望，就能得到：

因为系统的控制量是已知的，系统初始状态也是已知的，如果我们认为，那么整个上式都可以利用已知数据来完成计算，也就在实际上得到了高斯过程的先验。这是一个递推式，如果写成矩阵的形式就有：

其中：

还记得我们在基函数参数化推导出来的的更新公式嘛，不过同时包含时变的位姿和时不变路标，我们这里把时变的部分拿出来（其实拿不拿出来形式都是一样的，时不变的部分更新公式也是相同的，因为下面这个公式的推导跟时变时不变都没有关系）：

这里的、、和分别是观测、通过状态计算出来的观测、观测函数的一阶雅可比矩阵、系统噪声协方差矩阵，其中和都是块对角化的，所以计算逆矩阵没有太多开销，问题的关键在于，其中是系统状态变量的协方差矩阵。

而通过的积分式，我们直接计算出来的协方差为：

第二个等号成立是因为我们可以认为初始状态下的协方差为0。此时，如果我们定义，那么就有：

这里如果我们定义，，那么同样也可将上式写成矩阵形式：

所以其逆矩阵为：

这里的是一个块-三角矩阵，所以逆矩阵非常好求，而是一个单位下三角矩阵，它的逆矩阵也是非常好求的：

所以，经过推导发现，、和我们都能很方便的求出来，所以在更新状态变量时计算逆矩阵不存在非常大的开销，并且此推导对于LTV-SDE形式的假设都是有效的。

引入LTV-SDE假设并没有改变的更新公式，只不过通过确定了的形式来让更新公式中需要计算逆矩阵的部分变为稀疏的块-三角矩阵从而降低了逆矩阵计算时的开销。

但是我们只将进行了假设，而时不变的路标是没有做任何假设的，因此路标部分不能用这部分积分式的结果进行更新，换言之，对于更新式：

此处的可以由LTV-SDE保证是块-对角的，因为是路标本身，所示也是块-对角的。至于，它取决于对于路标的观测，可能稠密也可能稀疏，但一般来说也是相对稀疏的。即LTV-SDE只解决了左上角的稀疏性，也就是通过LTV-SDE把GPGN的优化问题求逆的复杂度降低到和常规BA大致相同的地步。

恒速度先验

机器人在运行过程中，一般不会有较大的加速度，而如果我们的观测频率如果相对较高，那么自然可以假设在相邻每个时刻内的速度的为恒定值，而速度是加速度的积分，所以这等价于认为加速度是白噪声。

那如何在我们的状态变量中体现这一点呢？注意，这显然要求我们的时变状态变量满足马尔可夫假设，我们可以直接令嘛，这里的表示位姿，即：

这是最容易想到的，但是这样没有体现出"恒速"假设，所以实际上我们得让，这样子最终得到的LTV-SDE就有如下形式:

因此，带有恒速先验的LTV-SDE假设要求我们同时让位置和速度为状态变量，并同时对其进行优化和更新。

注意，上述两篇文章里都出现了恒速度/白噪声加速度假设（第一篇文章在实验部分给了一个恒速的experiment），但是我个人认为两者是不一样。

首先，第一篇文章明确写了，这说明全局的先验就是初始状态。而第二篇文章则引入了马尔可夫假设，且，因此实际应该是描述了在每个马尔可夫时刻的状态变量的均值。

恒速度先验在SE3上的更新

回顾一下，我们已经推导了如何利用基函数来得到GPGN的更新公式和插值公式；并通过引入LTV-SDE假设来让更新公式中时变部分的求逆复杂度降低到和常规BA一样；接下来还使用恒速先验来进一步约束机器人的轨迹。但机器人的位姿一般用SE3表示，而我们上述所有的推导都没有涉及流型空间，这其实很难直接使用。

所以下面我们就来跟随Sparse Gaussian Processes on Matrix Lie Groups: A Unified Framework for Optimizing Continuous-Time Trajectories和Full STEAM Ahead: Exactly Sparse Gaussian Process Regression for Batch Continuous-Time Trajectory Estimation on SE(3)这两篇文章来将GPGN的更新公式推导到SE3空间上。

对于一个SE3上的位姿，他的运动学公式是非常显然的：

其中，我们假设加速度是白噪声的，因此我们直接将其加注到机器人的速度上：

如果实在是看不出来SE3上的运动学公式，这里就再稍微证明一下，如果读者熟悉SO3上的运动学的话，会发现两者形式其实是一致的()，不过SO3上的推导简单的多。

对于一个SE3位姿，我们可以将表示为，这里是左乘模型，右乘也是类似的：

现在我们对求时间导数：

其中最后进行了一阶泰勒展开。

因此，我们定义全局时刻下的状态变量为，这跟上文的推导是一致的：

另外，还记得我们之前已经假定的马尔可夫过程吗，我们认为在每个马尔可夫过程内，对应时刻下的速度是恒定的，因此我们可以为这个极短的区间定义局部SE3位姿(这里的含义其实是希望用局部的线性时不变来等效替代了全局的线性时变)：

这里的描述了定义在第个时刻下的局部相对位姿，而前文的是定义在世界坐标系下的SE3位姿。而因为局部坐标系下的相对位姿仍然遵循恒速假设，也即满足：

对于局部坐标系下的SE3位姿，我们同样可以写出其李代数se3的运动学公式，这跟SO3与so3之间的运动学公式也是非常相似的：

那我们为什么要定义这样一个局部坐标系下的时不变状态变量呢，本质是因为前文推导的GPGN是基于线性空间的，而SE3是流型空间，我们自然会想要找一个能够用线性空间代替流型空间进行优化的方法，而恰好流型空间中对于某一点的正切空间就是一个线性空间，而这个正切空间又恰好对应了李代数。而且作者经过仿真实验发现，通过这种局部时不变状态变量优化后的结果，与全局时变状态变量推算出来的协方差与均值是高度符合的，所以为了方便起见，就使用局部时不变来代替全局时变来优化了。

所以，我们其实要优化的状态变量是：

而对应的导数为

这里来证明一下为什么局部时不变状态变量能等价于全局时变状态变量。

首先，考虑下式(前文推导过的SE3的右乘的时间导数，只不过这里是左乘并写成了李代数形式)：

而又可以定义为全局坐标系SE3的导数：

将其回代得到：

而右雅可比矩阵在局部坐标系下是非常接近单位矩阵(因为扰动很小)，所以我们就可以近似认为：

因此局部坐标系下位置的SE3的导数非常接近全局坐标系下的SE3的导数，这就是能用局部时不变来代替全局时变的理论基础。

然后我们来看看如何更新SE3状态变量，因为我们的局部坐标系是时不变的，所以我们在构建先验误差项的时候可以考虑把两端都包含进来(局部坐标系原点，即)：

这里的是先验位姿，可以用前文描述的假设以及相应公式计算出来。这部分是局部坐标系原点的误差项；而这部分是局部坐标系下的相对位姿的误差项；如果局部坐标系原点与先验是有误差的，那么这个误差就会通过转移矩阵累进到对应时刻下的相对位姿上。

当然，上式是完全由局部坐标系下的状态变量来描述的，我们可以直接将全局坐标系下的SE3代入，这样就省的后续再重新算一遍了(但是误差项的构建还是基于局部坐标系的，只是我们用全局坐标系等价代入了)：

当我们通过计算出来时，它会包含三个部分，一部分是李代数的更新，另一部分是加速度的更新，最后一部分是路标的更新，而这些变量对于状态变量的更新分别为：

也就是除了SE3的部分使用指数映射进行更新外，其他的部分仍然是线性更新。至此，我们就完成了GPGN在SE3上的更新公式的推导。当然同样是处于篇幅考虑(懒)，没把协方差和插值公式列出来，这部分就留给读者去看原文好啦。

GPGN与B-Spline

看到轨迹插值，有人可能会立马想到B-Spline，确实B-Spline也是很常用的用来拟合轨迹的方法，在拟合轨迹后同样可以插值出位姿以及求出速度和加速度，也是平滑轨迹的方法之一。但是B-Spline没法给出插值后的协方差矩阵，而且需要预设拟合的阶数，理论上B-Spline应该对曲线复杂的部分有更多控制点，平坦的部分有更少的控制点，但目前针对SE3的B-Spline基本上都是均匀控制点。而GPGN则是基于高斯过程的，每个观测到的时刻都相当于一个约束，不存在预设阶数或者均不均匀的问题，这一点上看起来还是GPGN更灵活一点。

STEAM库

STEAM库是一个给出了GPGN实现的库，可以参见https://github.com/utiasASRL/steam，也可以参考这篇文章Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems。这个库基于lmath这个作者自行开发的李代数运算库。在STEAM库里，作者提供了恒定速度、恒定加速度的先验假设，以及包含地标、SE3状态变量的实现，并为双目、IMU、GPS等传感器提供了观测模型，不过并没有为单目提供观测模型，如果需要使用单目进行优化的话，可能需要自己进行修改。

我们来看这里面最具有典型性的一个例子，SimpleBAandConstVelTrajPrior.cpp，从名字也能看出来，这基于恒速度先验假设，来进行BA轨迹优化。

代码整体使用起来也是非常简单，只要给定先验，并创建好对应状态变量并构建误差项就可以了。

#include <iostream>
#include "lgmath.hpp"
#include "steam.hpp"
#include "steam/data/ParseBA.hpp"

using namespace steam;

// 状态变量，即x(t)，包含SE3位姿和6维速度(加速度与角加速度)
struct TrajStateVar {
  traj::Time time;
  se3::SE3StateVar::Ptr pose;
  vspace::VSpaceStateVar<6>::Ptr velocity;
};

int main(int argc, char** argv) {

  // 先验平滑假设，允许在前进方向和偏航方向上加速度较大
  double lin_acc_stddev_x = 1.00;  // body-centric (e.g. x is usually forward)
  double lin_acc_stddev_y = 0.01;  // body-centric (e.g. y is usually side-slip)
  double lin_acc_stddev_z = 0.01;  // body-centric (e.g. z is usually 'jump')
  double ang_acc_stddev_x = 0.01;  // ~roll
  double ang_acc_stddev_y = 0.01;  // ~pitch
  double ang_acc_stddev_z = 1.00;  // ~yaw
  Eigen::Matrix<double, 6, 1> Qc_diag;
  Qc_diag << lin_acc_stddev_x, lin_acc_stddev_y, lin_acc_stddev_z,
      ang_acc_stddev_x, ang_acc_stddev_y, ang_acc_stddev_z;


  /*
  略掉读取数据部分代码
  */

  OptimizationProblem problem;

  std::vector<TrajStateVar> traj_state_vars;
  std::vector<traj::const_vel::Variable::Ptr> traj_evals_ic;
  std::vector<stereo::HomoPointStateVar::Ptr> landmarks_ic;

  // 初始化状态变量
  for (unsigned int i = 0; i < dataset.frames_ic.size(); i++) {
    traj::Time time(dataset.frames_ic[i].time);
    const auto pose = se3::SE3StateVar::MakeShared(dataset.frames_ic[i].T_k0);
    const auto vel = vspace::VSpaceStateVar<6>::MakeShared(initVelocity);
    traj_state_vars.emplace_back(TrajStateVar{time, pose, vel});
  }

  // 固定第一帧的位姿
  traj_state_vars.at(0).pose->locked() = true;

  // 创建一个恒速度先验的轨迹，并为轨迹中添加状态变量
  traj::const_vel::Interface traj(Qc_diag);
  for (const auto& state : traj_state_vars)
    traj.add(state.time, state.pose, state.velocity);

  // 计算路标的相对位姿
  landmarks_ic.resize(dataset.land_ic.size(), nullptr);
  for (unsigned int i = 0; i < dataset.meas.size(); i++) {
    unsigned int frameIdx = dataset.meas[i].frameID;
    unsigned int landmarkIdx = dataset.meas[i].landID;
    if (!landmarks_ic[landmarkIdx]) {
      Eigen::Vector4d p_0;
      p_0.head<3>() = dataset.land_ic[landmarkIdx].point;
      p_0[3] = 1.0;

      lgmath::se3::Transformation pose_vk_0 =
          dataset.frames_ic[frameIdx].T_k0 / dataset.frames_ic[0].T_k0;
      Eigen::Vector4d p_vehicle = pose_vk_0 * p_0;

      landmarks_ic[landmarkIdx] =
          stereo::HomoPointStateVar::MakeShared(p_vehicle.head<3>());
      landmark_map[landmarkIdx] = frameIdx;
    }
  }

  // 添加时变系统状态变量
  for (const auto& state : traj_state_vars) {
    problem.addStateVariable(state.pose);
    problem.addStateVariable(state.velocity);
  }

  // 添加时不变的路标状态变量
  for (unsigned int i = 0; i < landmarks_ic.size(); i++)
    problem.addStateVariable(landmarks_ic[i]);

  // 设置噪声先验与损失函数
  const auto sharedCameraNoiseModel =
      StaticNoiseModel<4>::MakeShared(dataset.noise);
  const auto sharedLossFunc = L2LossFunc::MakeShared();

  // 相机内参，会在重投影误差中使用
  const auto sharedIntrinsics = std::make_shared<stereo::CameraIntrinsics>();
  sharedIntrinsics->b = dataset.camParams.b;
  sharedIntrinsics->fu = dataset.camParams.fu;
  sharedIntrinsics->fv = dataset.camParams.fv;
  sharedIntrinsics->cu = dataset.camParams.cu;
  sharedIntrinsics->cv = dataset.camParams.cv;

  for (unsigned int i = 0; i < dataset.meas.size(); i++) {
    unsigned int frameIdx = dataset.meas[i].frameID;
    unsigned int landmarkIdx = dataset.meas[i].landID;
    auto& landmark = landmarks_ic[landmarkIdx];

    Evaluable<lgmath::se3::Transformation>::Ptr tf_vb_va;
    if (landmark_map[landmarkIdx] == frameIdx) {
      tf_vb_va = tf_identity;
    } else {
      unsigned int firstObsIndex = landmark_map[landmarkIdx];
      tf_vb_va =
          se3::compose(traj_state_vars[frameIdx].pose,
                       se3::inverse(traj_state_vars[firstObsIndex].pose));
    }

    const auto tf_cb_va = se3::compose(tf_c_v, tf_vb_va);

    // 双目相机重投影误差
    const auto errorfunc = stereo::StereoErrorEvaluator::MakeShared(
        dataset.meas[i].data, sharedIntrinsics, tf_cb_va, landmark);

    const auto cost = WeightedLeastSqCostTerm<4>::MakeShared(
        errorfunc, sharedCameraNoiseModel, sharedLossFunc);

    problem.addCostTerm(cost);
  }

  traj.addPriorCostTerms(problem);

  DoglegGaussNewtonSolver::Params params;
  params.verbose = true;
  DoglegGaussNewtonSolver solver(problem, params);

  solver.optimize();

  return 0;
}