因子图优化

最大后验概率估计问题

我们常将状态估计问题建模为最大后验估计（MAP）。之所以命名为最大后验估计，是因为它在给定了观测$\mathbf{Z}$的情况下，最大化状态$\mathbf{X}$的后验概率密度$p(\mathbf{X}|\mathbf{Z})$
$$\begin{array}{rl}{\mathbf{X}}^{\mathrm{MAP}}& =\arg \underset{\mathbf{X}}{\max }p(\mathbf{X}|\mathbf{Z})\\ & =\arg \underset{\mathbf{X}}{\max }\frac{p(\mathbf{Z}|\mathbf{X})p(\mathbf{X})}{p(\mathbf{Z})}\end{array}$$
上式利用了贝叶斯法则，将后验概率密度表示为观测概率密度$p(\mathbf{Z}|\mathbf{X})$与状态的先验概率密度$p(\mathbf{X})$的乘积，再利用因子$p(\mathbf{Z})$进行归一化。当给定观测$\mathbf{Z}$时，归一化项就与最大后验概率无关，可以被忽略。将最大后验估计表述为另一种形式：
$${\mathbf{X}}^{\mathrm{MAP}}=\arg \underset{\mathbf{X}}{\max }l(\mathbf{X};\mathbf{Z})p(\mathbf{X})$$
这里，是给定观测$\mathbf{Z}$的情况下状态量$\mathbf{X}$的似然估计，被定义为任意一个与$p(\mathbf{Z}|\mathbf{X})$成正比的函数：
$$l(\mathbf{X};\mathbf{Z})\propto p(\mathbf{Z}|\mathbf{X})$$
符号$l(\mathbf{X};\mathbf{Z})$也强调了似然估计是关于$\mathbf{X}$的函数，而不是关于$\mathbf{Z}$的函数。并非状态$X$中的每一部分都和所有观测相关，因此根据变量块的独立性可以将$p(\mathbf{X}|\mathbf{Z})$分解为$p(\mathbf{X}|\mathbf{Z})={\prod }_{i}p({\mathbf{x}}_i|{\mathbf{z}}_i)$，等同于$l(\mathbf{X};\mathbf{Z})={\prod }_{i}l({\mathbf{x}}_i;{\mathbf{z}}_i)$

设观测模型为
$${\mathbf{z}}_i=h_i({\mathbf{x}}_i)+{\mathbf{\eta }}_i$$

在多数情况下，认为观测值受到均值为零的高斯噪声干扰。其中$h_i(\cdot )$是观测函数，噪声${\mathbf{\eta }}_i$由协方差矩阵为${\mathbf{R}}_i$的零均值高斯分布描述。对于观测量${\mathbf{z}}_i$条件概率$p({\mathbf{z}}_i|{\mathbf{x}}_i)$ 如下：
$$p({\mathbf{z}}_i|{\mathbf{x}}_i)=\mathcal{N}({\mathbf{z}}_i;h({\mathbf{x}}_i),{\mathbf{\Sigma }}_i)=\frac{1}{\sqrt{|2\pi {\mathbf{\Sigma }}_i|}}\exp \left\{-\frac{1}{2}{\Vert h_i({\mathbf{x}}_i)-{\mathbf{z}}_i\Vert }_{{\mathbf{\Sigma }}_i}^2\right\}$$
其中$|\cdot |$表示求行列式，$\Vert \mathbf{x}-\mathbf{\mu }{\Vert }_{\mathbf{\Sigma }}^2\stackrel{\mathrm{def}}{=}(\mathbf{x}-\mathbf{\mu }{)}^T{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })$表示马氏距离的平方。

概率问题的图表示

贝叶斯网络

贝叶斯网络是一个有向图模型，图中的节点代表变量${\mathbf{\theta }}_j$。我们将全部感兴趣的随机变量集合表示为 Θ = { θ 1 , ⋯   , θ n } \varTheta=\{\bm \theta_1,\cdots,\bm\theta_n\} Θ={θ1​,⋯,θn​}。一个贝叶斯网络在所有的变量$\Theta$上的联合概率密度函数 $p(\Theta )$被定义为与每个节点相关联的条件概率密度的乘积：

$$p\left(\Theta \right)\triangleq \prod _{j}p\left({\mathbf{\theta }}_j|{\mathbf{\pi }}_j\right)$$
式中，$p\left({\mathbf{\theta }}_j|{\mathbf{\pi }}_j\right)$是节点的条件概率密度函数，${\mathbf{\pi }}_j$是${\mathbf{\theta }}_j$的父节点。在一个贝叶斯网络中，联合概率密度的分解由其图结构，特别是它与父节点之间的关系所决定。

以一个小型SLAM问题为例，一个机器人在3个连续位姿${\mathbf{x}}_1,{\mathbf{x}}_2,{\mathbf{x}}_3$对2个路标点${\mathbf{l}}_1,{\mathbf{l}}_2$进行观测，假设在第一个位姿${\mathbf{x}}_1$处有队绝对位姿的观测量${\mathbf{z}}_1$，其余观测量为对路标的相对观测${\mathbf{z}}_2,{\mathbf{z}}_3,{\mathbf{z}}_4$。按照通常的贝叶斯网络定义，联合概率密度$p(\mathbf{X},\mathbf{Z})=p({\mathbf{x}}_1,{\mathbf{x}}_2,{\mathbf{x}}_3,{\mathbf{l}}_1,{\mathbf{l}}_2,{\mathbf{z}}_1,{\mathbf{z}}_2,{\mathbf{z}}_3,{\mathbf{z}}_4)$可由一系列概率密度函数的乘积得到：

$$\begin{array}{rl}p\left(\mathbf{X},\mathbf{Z}\right)& =p\left({\mathbf{x}}_1\right)p\left({\mathbf{x}}_2|{\mathbf{x}}_1\right)p\left({\mathbf{x}}_3|{\mathbf{x}}_2\right)p\left({\mathbf{l}}_1\right)p\left({\mathbf{l}}_2\right)\\ & \times p\left({\mathbf{z}}_1|{\mathbf{x}}_1\right)p\left({\mathbf{z}}_2|x_1,{\mathbf{l}}_1\right)p\left({\mathbf{z}}_3|{\mathbf{x}}_2,{\mathbf{l}}_1\right)p\left({\mathbf{z}}_4|{\mathbf{x}}_3,{\mathbf{l}}_2\right)\end{array}$$

因子图

因子图也是一种刻画概率问题的方式。贝叶斯网络是一种非常适合用于建模的语言，相比而言因子图则更适合用于推断。像贝叶斯网络一样，因子图允许我们将一个联合概率密度表示为一系列因子的乘积。然而因子图比贝叶斯网络更加通用，因为它不仅可以指定概率密度，还可以指定在变量$\mathbf{X}$的集合上的任意一个因子函数$\varphi (\mathbf{X})$。

我们考虑在小型 SLAM 例子上进行最大后验概率推断。考虑到未知量依赖于观测量 $\mathbf{Z}$ ，可以利用贝叶斯法则将后验概率$p(\mathbf{X}|\mathbf{Z})$重写为如下形式：
$$\begin{array}{rl}p\left(\mathbf{X}|\mathbf{Z}\right)& \propto p\left({\mathbf{x}}_1\right)p\left({\mathbf{x}}_2|{\mathbf{x}}_1\right)p\left({\mathbf{x}}_3|{\mathbf{x}}_2\right)p\left({\mathbf{l}}_1\right)p\left({\mathbf{l}}_2\right)\\ & \times l\left({\mathbf{x}}_1;{\mathbf{z}}_1\right)l\left({\mathbf{x}}_1,{\mathbf{l}}_1;{\mathbf{z}}_2\right)l\left({\mathbf{x}}_2,{\mathbf{l}}_1;{\mathbf{z}}_3\right)l\left({\mathbf{x}}_3,{\mathbf{l}}_2;{\mathbf{z}}_4\right)\end{array}$$
显然，上式表示在未知变量上的未归一化的因子的概率密度。为了使这个因式分解更加明确，我们使用了因子图（factor graph）。与贝叶斯网络一样，所有的未知状态量$\mathbf{X}$，包括机器人位姿和路标点，都用一个节点表示。然而，与贝叶斯网络不同的是，观测量不会被显式地表示出来。

在形式上，因子图是有两种类型节点的二分图$\mathbf{F}=(\mathcal{U},\mathcal{V},\mathcal{E})$：因子节点（factor）${\mathbf{\varphi }}_i$和变量节点（variable）${\mathbf{x}}_j\in \mathcal{V}$，边总是存在于因子节点和变量节点之间。与因子${\mathbf{\varphi }}_i$相邻的变量节点几何被写作$\mathcal{N}({\mathbf{\varphi }}_i)$，并且我们将对这个变量集合的赋值写作${\mathbf{X}}_i$。然后就可以将因子图$\mathbf{F}$定义为对作用于全体变量的函数$\mathbf{\varphi }(\mathbf{X})$：
$$\varphi (\mathbf{X})=\prod _i{\varphi }_i({\mathbf{X}}_i)$$

每一个贝叶斯网络都可以很容易的转换为因子图。因为贝叶斯网络中的每一个节点都指示了相应的变量及其父节点的一个条件概率密度，所以转换过程非常简单：每一个贝叶斯网络节点都分解为相应因子图中的一个变量节点和一个因子节点。因子节点与变量节点相连接，和贝叶斯网络中的父节点与对应的变量节点相连接的含义是一样的。如果贝叶斯网络中的一些节点是证据节点（evidence node），即它们所包含的变量是已知的。我们就可以忽略相应的变量节点：已知变量变成了响应因子中的固定参数。

将一个贝叶斯网络$p(\mathbf{X}|\mathbf{Z})$转换为一个因子图，通过在证据观测$\mathbf{Z}$上进行条件化会生成一个后验概率的表示$\varphi (\mathbf{X})\propto p(\mathbf{X}|\mathbf{Z})$。

下面我们将说明对一个符合高斯噪声模型的SLAM问题进行最大后验概率估计，等价于求解一个非线性最小二乘问题。实际上对于任何一个因子图，最大后验概率估计等价于最大化所有因子的乘积：
$$\begin{array}{rl}{\mathbf{X}}^{\mathrm{MAP}}& =\arg \underset{\mathbf{X}}{\max }\varphi (\mathbf{X})\\ & =\arg \underset{\mathbf{X}}{\max }\prod _i{\varphi }_i({\mathbf{X}}_i)\end{array}\text{\hspace{1em}(1)}$$
让我们假设所有因子都具有如下形式：
$${\varphi }_i({\mathbf{X}}_i)\propto \exp \left\{-\frac{1}{2}{\Vert h_i({\mathbf{X}}_i)-{\mathbf{z}}_i\Vert }_{{\mathbf{\Sigma }}_i}^2\right\}$$
其中既包括简单的高斯先验，也包括由受到零均值高斯噪声干扰的观测量推导出来的似然因子。
对(1)式取负对数并丢掉$\frac{1}{2}$，问题就转化为非线性最小二乘的和，每一个最小二乘项对应着一个概率：

$${\mathbf{X}}^{\mathrm{MAP}}=\arg \underset{\mathbf{X}}{\max }\sum _i\Vert h_i({\mathbf{X}}_i)-{\mathbf{z}}_i{\Vert }_{{\mathbf{\Sigma }}_i}^2$$

线性化

使用泰勒展开公式将非线性最小二乘目标函数中的所有观测函数$h_i(\cdot )$线性化

$$h_j({\mathbf{X}}_i)=h_i({\mathbf{X}}_i^0+{\mathbf{\Delta }}_i)\approx h_i({\mathbf{X}}_i^0)+{\mathbf{H}}_i{\mathbf{\Delta }}_i$$

其中${\mathbf{\Delta }}_i\triangleq {\mathbf{X}}_i-{\mathbf{X}}_i^0$是状态更新向量，${\mathbf{H}}_i$是偏微分矩阵
$${\mathbf{H}}_i\triangleq {\frac{\partial h_i({\mathbf{X}}_i)}{\partial {\mathbf{X}}_i}|}_{{\mathbf{X}}_i^0}$$

将泰勒展开式带入非线性最小二乘问题，可以得到线型最小二乘问题，其状态更新向量如下：

$$\begin{array}{rl}{\mathbf{\Delta }}^{\ast }& =\arg \underset{\mathbf{\Delta }}{\min }\sum _i{\Vert h_i({\mathbf{X}}_i^0)+{\mathbf{H}}_i{\mathbf{\Delta }}_i-{\mathbf{z}}_i\Vert }_{{\mathbf{\Sigma }}_i}^2\end{array}$$
通过简单的变量变换，可以将马氏范数转换为2-范数：
$${\Vert \mathbf{e}\Vert }_{\mathbf{\Sigma }}^2={\mathbf{e}}^T{\mathbf{\Sigma }}^{-1}\mathbf{e}=({\mathbf{\Sigma }}^{-\frac{1}{2}}\mathbf{e}{)}^T({\mathbf{\Sigma }}^{-\frac{1}{2}}\mathbf{e})={\Vert {\mathbf{\Sigma }}^{-\frac{1}{2}}\mathbf{e}\Vert }_2^2$$
令
$$\begin{array}{rl}{\mathbf{A}}_i& ={\mathbf{\Sigma }}_i^{\frac{1}{2}}{\mathbf{H}}_i\\ b_i& ={\mathbf{\Sigma }}_i^{\frac{1}{2}}\left({\mathbf{z}}_i-h_i({\mathbf{X}}_i^0)\right)\end{array}$$
最终得到如下标准最小二乘问题：
$$\begin{array}{rl}{\mathbf{\Delta }}^{\ast }& =\arg \underset{\mathbf{\Delta }}{\min }\sum _i{\Vert {\mathbf{A}}_i{\mathbf{\Delta }}_i-b_i\Vert }_2^2\\ & =\arg \underset{\mathbf{\Delta }}{\min }{\Vert \mathbf{A}\mathbf{\Delta }-\mathbf{b}\Vert }_2^2\end{array}$$
因此这样就将求解最大后验概率问题转化为了矩阵求解问题，概率的条件化操作对应着矩阵的QR分解。

变量消元

变量消元算法是一种将因子图转换回贝叶斯网络的方法， 将未归一化的后验概率$\varphi (\mathbf{X})\propto p(\mathbf{X}|\mathbf{Z})$表示为一系列因子的乘积，等价于概率的条件分解。变量消元算法将因子图$\varphi (\mathbf{X})=\varphi ({\mathbf{x}}_1,\cdots ,{\mathbf{x}}_n)$分解为如下形式的因子化贝叶斯网络概率密度。

$$p(\mathbf{X})=p({\mathbf{x}}_1|{\mathbf{S}}_1)p({\mathbf{x}}_2|{\mathbf{S}}_2)\cdots p({\mathbf{x}}_n)=\prod _{j}p({\mathbf{x}}_j|{\mathbf{S}}_j)$$
选定的变量消元顺序为${\mathbf{x}}_1,\cdots ,{\mathbf{x}}_n$，与变量${\mathbf{x}}_j$相关的分离器$\mathcal{S}({\mathbf{x}}_j)$记为${\mathbf{S}}_j$。分离器是这样一组变量的集合：在${\mathbf{x}}_j$被消去后，${\mathbf{x}}_j$ 在这组变量上会被条件化。

线性最小二乘项的和可以表示为
$$\mathbf{A}\left[\begin{array}{c}\mathbf{x};\mathbf{S}\end{array}\right]-\mathbf{b}$$
通过对增广矩阵$\left[\begin{array}{c}\mathbf{A}|\mathbf{b}\end{array}\right]$进行部分QR分解可以转化为
$$\left[\begin{array}{c}\mathbf{A}|\mathbf{b}\end{array}\right]=\mathbf{Q}\left[\begin{array}{ccc}\mathbf{R}& \mathbf{T}& \mathbf{d}\\ & \bar{\mathbf{A}}& \bar{\mathbf{b}}\end{array}\right]$$
因此
$${\Vert \mathbf{A}\left[\begin{array}{c}\mathbf{x};\mathbf{S}\end{array}\right]-\mathbf{b}\Vert }_2^2={\Vert \mathbf{R}\mathbf{x}+\mathbf{T}\mathbf{S}-\mathbf{d}\Vert }_2^2+{\Vert \bar{\mathbf{A}}\mathbf{S}-\bar{\mathbf{b}}\Vert }_2^2$$

对应高斯概率的条件化：
$$\begin{array}{rl}& p(\mathbf{x};\mathbf{S})\propto \exp \left\{-\frac{1}{2}{\Vert \mathbf{A}\left[\begin{array}{c}\mathbf{x};\mathbf{S}\end{array}\right]-\mathbf{b}\Vert }_2^2\right\}\\ =& p(\mathbf{x}|\mathbf{S})p(\mathbf{S})\propto \exp \left\{-\frac{1}{2}{\Vert \mathbf{R}\mathbf{x}+\mathbf{T}\mathbf{S}-\mathbf{d}\Vert }_2^2\right\}\exp \left\{-\frac{1}{2}{\Vert \bar{\mathbf{A}}\mathbf{S}-\bar{\mathbf{b}}\Vert }_2^2\right\}\end{array}$$
通过上述的变量消元法，按照特定的变量消元顺序，可以将一个因子图转化为一个贝叶斯网络，并且消元得到的贝叶斯网络有一个特殊的性质：他是一个弦图（chordal graph），即任何一个长度大于3的无向环都有一个弦。弦定义为链接环中两个非连续节点的边。通过识别这个弦图中的团（一组全连接的变量），贝叶斯网络可以被重写为贝叶斯树（Bayes tree）。

利用贝叶斯树存储和计算信息矩阵（即协方差矩阵），的的树结构，在进行增量推断时，每增加一个新的观测时。对应增加一个新的因子，这种情况下，贝叶斯树中只有包含因子的变量节点的团到根团之间的路径会受影响。包含因子的变量节点的团以下的子树，以及任何其他不包含 因子的变量节点的子树则并未受影响。因此，为了更新贝叶斯树，树中受影响的部分被转换回一个因子图，将与新的测量相关联的新的因子添加进去。通过对这个临时因子图使用任意一种方便的消元顺序进行重新消元，可以生成一棵新的贝叶斯子树，并且之前没有受影响的子树也可以重新添加回去。

之所以能够实现这样的效果是由于贝叶斯树的两个重要的性质。两个性质都来自贝叶斯树包含了消元过程的信息流：第一，贝叶斯树是由反向消元顺序的弦贝叶斯网络生成的。在这种方式下，每个团中的变量通过对它们的子团进行消元来收集信息。因此，在任何团中的信息只会向上传播直到根节点上。第二，只有与某个因子连接的第一个变量被消元，这个因子的信息才进入消元过程。结合这两个性质，我们看到，一个新的因子并不能影响任何没有与该因子相连的变量的后继变量。然而，变量上的因子到根节点有不同的（即独立的）路径，这意味着必须重新对这些路径进行消元来表达它们之间新的依赖。

因子图优化 iSAM2 利用上述方法实现了增量平滑建图，可以高效的增量求解非线性优化问题。选择性地对那些距离线性化点的偏差大于阈值的变量因子进行重新线性化。相比于由增加新的因子引起的对树的修改，我们现在对包含受影响变量（不仅仅是前端变量，还有分离器变量）的所有团进行重新处理。

这会影响到树的大部分，但是在绝大多数情况下，这也比重新计算整棵树的代价要小得多。我们也需要回到原始的完整因子图上计算，而不是将团转换成局部因子图。在消元的过程中同时需要进行一定量的缓存。在文献中有对整个增量非线性算法（iSAM2）详细的描述。

GTSAM

流型运算

$a,b$是李群上的元素，$\delta$是李代数空间的向量
$$\begin{array}{rl}a.\text{compose}(b)& =a\ast b\\ a.\text{between}(b)& =a.\text{inverse}()\ast b\\ a.\text{localaordinates}\left(b\right)& =b\boxminus a=\log \left(a^{-1}b\right)\\ a.\text{retract}\left(\delta \right)& =a⊞\delta =a\exp \left(\delta \right)\end{array}$$
对于gtsam::Pose2的构造函数是平移在前，旋转在后gtsam::Pose2(x, y, yaw)，其噪声协方差模型也是平移在前(x,y)，旋转在后(yaw)。而这与gtsam::Pose3的定义恰恰相反：
对于gtsam::Pose3的构造函数是旋转在前，平移在后gtsam::Pose3(gtsam::Rot3, gtsam::Point3)，其噪声协方差模型也是旋转在前(roll,pitch,yaw)，平移在后(x,y,z)

 // Add a prior on pose x0. This indirectly specifies where the origin is.
  // 0.1 rad std on roll, pitch, yaw, 30cm std on x,y,z.
  auto noise = noiseModel::Diagonal::Sigmas(
      (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
  newgraph.push_back(PriorFactor<Pose3>(X(0), pose_0, noise));

Jacobian矩阵

GTSAM中对Jacobian矩阵$H$的定义如下：
$$h(q\exp \hat{\xi })\approx h(q)+H\xi$$
其中$\exp \hat{\xi }$是变量的指数映射