视觉SLAM总结——后端总结

视觉SLAM总结——后端总结

什么叫温故而知新，最近一直在复习SLAM相关的基础知识，每重新复习到一个点总感觉能有新的收获，今天周末正好没有别的安排，就计划花一天的时间把视觉SLAM的后端再好好总结一下，这篇文章主要是参考《视觉SLAM十四讲》，另外加入了一些我的个人理解

（1）扰动模型

《视觉SLAM十四讲》前六章主要讲了视觉SLAM中所用到一些基础知识，其中第四讲主要介绍的李群李代数基础主要就是为后端的推导服务的，李群和李代数的关系通过书中下面这张图就能看明白了

这里简单解释下，对于三维旋转：
对数映射操作看上去是先对旋转矩阵取对数，然后加以$\vee$符号，实际上$\mathfrak{s}\mathfrak{o}(3)$就是旋转向量$\theta a$所组成的空间，其中$\theta$就是旋转向量的旋转角，求解方法就按照$\theta =\arccos \frac{\mathrm{tr}(R)-1}{2}$获得，$a$其实就是旋转向量的旋转轴（是一个单位向量），旋转轴上的向量不发生改变，因此有$Ra=a$，所以转轴$a$其实就是旋转矩阵$R$特征值1对应的特征向量，再归一化就得到旋转轴
指数映射操作看上去是先对李代数加以$\wedge$符号，然后取指数，实际上指数映射的公式其实就是从旋转向量到旋转矩阵的罗德里格斯公式。

对于三维变换，对数映射中我们求得$\theta$和$a$，然后在计算矩阵$J$，最后从$t=J\rho$解得$\rho$。指数映射直接套公式。

李代数存在的目的是什么?——方便求导进行优化

下面我们就来分析下李代数是如何方便求导的，我们从三维旋转直观来看，李代数求导按照对于空间点$p$定义应该如下进行推导：$$\begin{array}{rl}\frac{\partial \left(\exp \left({\varphi }^{\wedge }\right)p\right)}{\partial \varphi }& =\underset{\delta \varphi \to 0}{\lim }\frac{\exp \left((\varphi +\delta \varphi {)}^{\wedge }\right)p-\exp \left({\varphi }^{\wedge }\right)p}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{\exp \left({\left(J_l\delta \varphi \right)}^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)p-\exp \left({\varphi }^{\wedge }\right)p}{\delta \varphi }\\ & \approx \underset{\delta \varphi \to 0}{\lim }\frac{\left(I+{\left(J_l\delta \varphi \right)}^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)p-\exp \left({\varphi }^{\wedge }\right)p}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{{\left(J_l\delta \varphi \right)}^{\wedge }\exp \left({\varphi }^{\wedge }\right)p}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{-{\left(\exp \left({\varphi }^{\wedge }\right)\mathbf{p}\right)}^{\wedge }{\mathbf{J}}_l\delta \varphi }{\delta \mathbf{\varphi }}=-(\mathbf{R}\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l\end{array}$$上面的推到中，第一行很简单，就是定义而已，第三行很简单，就是指数的泰勒展开，第四行、第五行也很简单，消元化简而已，唯独第二行，怎么来的呢？这里就需要引入一个概念叫Baker-Campbell-Hausdorff 公式（BCH公式），如下：
$$\ln (\exp (\mathbf{A})\exp (\mathbf{B}))=\mathbf{A}+\mathbf{B}+\frac{1}{2}[\mathbf{A},\mathbf{B}]+\frac{1}{12}[\mathbf{A},[\mathbf{A},\mathbf{B}]]-\frac{1}{12}[\mathbf{B},[\mathbf{A},\mathbf{B}]]+...$$其中 [] 为李括号。 BCH 公式告诉我们，当处理两个矩阵指数之积时，它们会产生一些由李括号组成的余项，特别地，考虑 SO(3) 上的李代数$\ln {\left(\exp \left({\varphi }_1^{\wedge }\right)\exp \left({\varphi }_2^{\wedge }\right)\right)}^{\vee }$，当${\varphi }_1$或${\varphi }_2$为小量时，小量二次以上的项都可以被忽略掉。此时， BCH 拥有线性近似表达$$\ln {\left(\exp \left({\varphi }_1^{\wedge }\right)\exp \left({\varphi }_2^{\wedge }\right)\right)}^{\vee }\approx \{\begin{array}{ll}{J}_l{\left({\varphi }_2\right)}^{-1}{\varphi }_1+{\varphi }_2& \text{ if }{\varphi }_1\text{ is small, }\\ J_r{\left({\varphi }_1\right)}^{-1}{\varphi }_2+{\varphi }_1& \text{ if }{\varphi }_2\text{ is small. }\end{array}$$当对一个旋转矩阵$R_2$（李代数为${\varphi }_2$）左乘一个微小旋转矩阵$R_1$（李代数为${\varphi }_1$）时，可以近似地看作，在原有的李代数${\varphi }_2$上，加上了一项$J_l{\left({\varphi }_2\right)}^{-1}{\varphi }_1$（上面给出BCH公式的这一小段是书中的原话），其中左雅克比矩阵$J_l$（注意这里是分左右的）的计算公式为：$$J_l=J=\frac{\sin \theta }{\theta }I+\left(1-\frac{\sin \theta }{\theta }\right)a{a}^T+\frac{1-\cos \theta }{\theta }{a}^{\wedge }$$上面的BCH线性近似表达式我们可以写为$$\exp \left(\Delta {\varphi }^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)=\exp \left({\left(\varphi +J_l^{-1}(\varphi )\Delta \varphi \right)}^{\wedge }\right)$$因此，我们就可以获得上面第二行的推导公式：$$\exp \left((\varphi +\Delta \varphi {)}^{\wedge }\right)=\exp \left({\left(J_l\Delta \varphi \right)}^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)$$好了，这是第一种求导方式，可以看到求导结果中有个左雅克比矩阵$J_l$，是个六维的矩阵，这个是比较烦人的，因此就引入了第二种求导方式，也就是这一节的小标题——扰动模型。
扰动模型和上面的的BCH线性近似表达式一样，也分为左乘和右乘两种情况，以左乘为例，对$R$左乘一个小扰动$\Delta R$(其李代数定义为$\varphi$)：$$\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \phi }=\underset{\phi \to 0}{\lim }\frac{\exp \left({\phi }^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)\mathbf{p}-\exp \left({\varphi }^{\wedge }\right)\mathbf{p}}{\phi }$$这和第一种求导方式的导数定义就不一样了，现在这种定义求导更为简单，如下：$$\begin{array}{rl}\frac{\partial (\mathbf{R}p)}{\partial \phi }& =\underset{\phi \to 0}{\lim }\frac{\exp \left({\phi }^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)\mathbf{p}-\exp \left({\varphi }^{\wedge }\right)\mathbf{p}}{\phi }\\ & \approx \underset{\phi \to 0}{\lim }\frac{\left(1+{\phi }^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)\mathbf{p}-\exp \left({\varphi }^{\wedge }\right)\mathbf{p}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }\mathbf{R}\mathbf{p}}{\phi }=\underset{\phi \to 0}{\lim }\frac{-(\mathbf{R}p{)}^{\wedge }\phi }{\phi }=-(\mathbf{R}\mathbf{p}{)}^{\wedge }\end{array}$$由结论可以看出，第二种求导方式的结果少了那个烦人的左雅克比矩阵$J_l$。

上面的推导和结论都是基于三维旋转的，下面给出三维变换的扰动模型

三维旋转中仅仅是对旋转添加了一个扰动，那么三维变换中需要对旋转和平移都添加一个扰动$\Delta T$才行（其李代数定义为$\delta \mathbf{\xi }=[\delta \mathbf{\rho },\delta \mathbf{\varphi }{]}^T$），那么$$\begin{array}{rl}\frac{\partial (\mathbf{T}\mathbf{p})}{\partial \delta \mathbf{\xi }}& =\underset{\delta \xi \to 0}{\lim }\frac{\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ & \approx \underset{\delta \xi \to 0}{\lim }\frac{\left(\mathbf{I}+\delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \xi \to 0}{\lim }\frac{\delta {\mathbf{\xi }}^{\wedge }\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{cc}\delta {\varphi }^{\wedge }& \delta \rho \\ 0^T& 0\end{array}\right]\left[\begin{array}{c}\mathbf{R}\mathbf{p}+\mathbf{t}\\ 1\end{array}\right]}{\delta \xi }\\ & =\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{c}\delta {\varphi }^{\wedge }(Rp+t)+\delta \rho \\ 0\end{array}\right]}{\delta \xi }=\left[\begin{array}{cc}I& -(Rp+t{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\triangleq (\mathbf{T}\mathbf{p}{)}^{\odot }\end{array}$$$这个(\mathbf{T}\mathbf{p}{)}^{\odot }$是一个运算符，相当于是在一个坐标点对其三维位姿求导的结果，是一个4×6的矩阵。
这里补充一个图加深对扰动模型的理解

对这个图的解释是，首先对于标量，指数空间的加法相当与对数空间的乘法，那么对应到李群李代数空间，我们可以说，李代数空间的加法相当与李群空间的乘法（当然这种说法是错的，上面解释过，因为位姿不是标量，但是我们不严格地可以这么说），那么我们在李代数空间添加扰动的话，公式如下：$$\frac{\partial \left(\exp \left({\varphi }^{\wedge }\right)p\right)}{\partial \varphi }=\underset{\delta \varphi \to 0}{\lim }\frac{\exp \left((\varphi +\delta \varphi {)}^{\wedge }\right)p-\exp \left({\varphi }^{\wedge }\right)p}{\delta \varphi }$$这就是第一种求导模型，那么我们在李群空间添加绕动的话，公式如下：$$\frac{\partial (\mathbf{R}p)}{\partial \phi }=\underset{\phi \to 0}{\lim }\frac{\exp \left({\phi }^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)\mathbf{p}-\exp \left({\varphi }^{\wedge }\right)\mathbf{p}}{\phi }$$这就是我们第二种求导模型，第二种模型就避免了复杂的雅可比矩阵的求解。

（2）Bundle Adjustment推导及稀疏性分析

Step 1：Bundle Adjustment推导

首先摆出SLAM问题的两个描述方程（现在越来越发现这两个方程的重要性了…）：$$\{\begin{array}{l}{\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)+{\mathbf{w}}_k\\ {\mathbf{z}}_{k,j}=h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right)+{\mathbf{v}}_{k,j}\end{array}$$在视觉SLAM的后端问题上，我们没有测量运动的传感器，因此没有$u_k$，或者说我们后端的预测值是又前端提供的，因此我们这里只考虑观测方程，我们假设噪声项${\mathbf{v}}_k\sim N\left(0,{\mathbf{Q}}_{k,j}\right)$，所以观测数据的条件概率表示$$P\left({\mathbf{z}}_{j,i}|{\mathbf{\xi }}_i,{\mathbf{p}}_j\right)=N\left(h\left({\mathbf{p}}_j,{\mathbf{\xi }}_i\right),{\mathbf{Q}}_{k,j}\right)$$其仍然是一个高斯分布，其中${\mathbf{\xi }}_i$指的是相机空间位姿（是一个表示三维变换的矩阵），${\mathbf{p}}_j$指的是路标点的空间坐标（是一个表示平移的向量），而状态变量$\mathbf{x}=\left\{{\mathbf{\xi }}_1,\dots ,{\mathbf{\xi }}_m,{\mathbf{p}}_1,\dots ,{\mathbf{p}}_n\right\}$(状态变量把相机空间位姿放前面和后面的矩阵分析是相关的)，好，回到高斯分布，我们要求解的就是使得高斯分布取得最大值的${\mathbf{\xi }}_i$和${\mathbf{p}}_j$，求解的方法是求最小化负对数来求高斯分布的最大似然（固定方法）：
$$P(\mathbf{x})=\frac{1}{\sqrt{(2\pi {)}^N\det (\mathbf{\Sigma })}}\exp \left(-\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^T{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })\right)$$取负对数得$$-\ln (P(\mathbf{x}))=\frac{1}{2}\ln \left((2\pi {)}^N\det (\mathbf{\Sigma })\right)+\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^T{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })$$这里面第一项和状态$x$无关，因此根据第二项，我们可以获得最终的目标函数：$$\begin{array}{rl}{\mathbf{x}}^{\ast }& =\arg \min \left({\left({\mathbf{z}}_{\mathbf{k},j}-h\left({\mathbf{\xi }}_{\mathbf{i}},{\mathbf{p}}_j\right)\right)}^T{\mathbf{Q}}_{i,j}^{-1}\left({\mathbf{z}}_{\mathbf{k},\mathbf{j}}-h\left({\mathbf{\xi }}_i,{\mathbf{p}}_j\right)\right)\right)\\ & =\arg \min (\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n{\Vert {\mathbf{z}}_{ij}-h\left({\mathbf{\xi }}_i,{\mathbf{p}}_j\right)\Vert }^2)\\ & =\arg \min (\frac{1}{2}\Vert f(\mathbf{x}){\Vert }^2)\end{array}$$可以看出来这其实就是最小化噪声项的平方，当噪声独立是，${\mathbf{Q}}_{k,j}^{-1}$是单位阵，然后就可以推导出整体的代价函数（重投影误差），按照高斯牛顿法的求解方法有$$\frac{1}{2}\Vert f(\mathbf{x}+\Delta \mathbf{x}){\Vert }^2\approx \frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n{\Vert {\mathbf{e}}_{ij}+{\mathbf{F}}_{ij}\Delta {\mathbf{\xi }}_i+{\mathbf{E}}_{ij}\Delta {\mathbf{p}}_j\Vert }^2$$其中${\mathbf{F}}_{ij}$表示整个儿代价函数在当前状态下对相机空间姿态的偏导数，而${\mathbf{E}}_{ij}$表示该函数对路标点的空间坐标的偏导数，令$${\mathbf{x}}_c={\left[{\mathbf{\xi }}_1,{\mathbf{\xi }}_2,\dots ,{\mathbf{\xi }}_m\right]}^T\in {\mathbb{R}}^{6m}$$$${\mathbf{x}}_p={\left[{\mathbf{p}}_1,{\mathbf{p}}_2,\dots ,{\mathbf{p}}_n\right]}^T\in {\mathbb{R}}^{3n}$$则可以进一步简化得$$\frac{1}{2}\Vert f(\mathbf{x}+\Delta \mathbf{x}){\Vert }^2=\frac{1}{2}{\Vert \mathbf{e}+\mathbf{F}\Delta {\mathbf{x}}_c+\mathbf{E}\Delta {\mathbf{x}}_p\Vert }^2$$这里的雅可比矩阵$\mathbf{E}$和$\mathbf{F}$必须是整体目标函数对整体变量的导数，则$\mathbf{J}=[\mathbf{F}\mathbf{E}]$，那么最后得到的增量方程式是$$\mathbf{H}\Delta \mathbf{x}=\mathbf{g}$$其中$$\mathbf{H}={\mathbf{J}}^T\mathbf{J}=\left[\begin{array}{cc}{\mathbf{F}}^T\mathbf{F}& {\mathbf{F}}^T\mathbf{E}\\ {\mathbf{E}}^T\mathbf{F}& {\mathbf{E}}^T\mathbf{E}\end{array}\right]$$

Step 2：稀疏性分析

然后就要开始进行矩阵稀疏性分析了，之前实习面试的时候有个面试官问我$\mathbf{H}$为什么是稀疏的？书上的回答是$\mathbf{H}$矩阵的稀疏性是有雅克比矩阵$\mathbf{J}$引起的，怎么理解呢，这里的这个雅克比矩阵$\mathbf{J}$是我们的代价函数相对于状态变量的雅克比矩阵，通用的雅克比的计算公式如下$$\mathbf{J}=\left[\begin{array}{ccc}\frac{\partial f_1}{\partial x_1}& \cdots & \frac{\partial f_1}{\partial x_n}\\ ⋮& \ddots & ⋮\\ \frac{\partial f_m}{\partial x_1}& \cdots & \frac{\partial f_m}{\partial x_n}\end{array}\right]$$在视觉SLAM的这个雅克比矩阵里面$\frac{\partial f_1}{\partial x_1}$是什么意思呢？假定$x_1$是第一个相机空间位姿 ${\mathbf{\xi }}_1$，$\frac{\partial f_1}{\partial x_1}$其实就是代价函数中的第一项 ${\mathbf{e}}_{11}$（这里是假定${\mathbf{e}}_{11}$存在）对第一个相机空间位姿 ${\mathbf{\xi }}_1$求导，为什么是这样呢？仔细观察推导过程后发现：
$$\frac{1}{2}\Vert f(\mathbf{x}+\Delta \mathbf{x}){\Vert }^2\approx \frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n{\Vert {\mathbf{e}}_{ij}+{\mathbf{F}}_{ij}\Delta {\mathbf{\xi }}_i+{\mathbf{E}}_{ij}\Delta {\mathbf{p}}_j\Vert }^2=\frac{1}{2}{\Vert \mathbf{e}+\mathbf{F}\Delta x_c+\mathbf{E}\Delta x_p\Vert }^2$$这里的$\mathbf{F}$和$\mathbf{E}$是分别由${\mathbf{e}}_{ij}$对${\mathbf{\xi }}_i$、${\mathbf{p}}_j$的雅克比矩阵${\mathbf{F}}_{ij}$、${\mathbf{E}}_{ij}$“拼凑”起来的，换一种方式理解，现在这里的状态向量$\mathbf{x}=[{\mathbf{\xi }}_1,\dots ,{\mathbf{\xi }}_m,{\mathbf{p}}_1,\dots ,{\mathbf{p}}_n{]}^T$，这是一个向量，那么误差$\mathbf{e}$求出来应该也是这样一个向量，例如$[{\mathbf{e}}_{11},{\mathbf{e}}_{12,\dots }]$这样的形式。因此整个雅克比矩阵$\mathbf{J}$可以视为是一个列向量误差向量$\mathbf{e}$对状态向量$\mathbf{x}$的导数（这里指的向量都是广义的向量），因为${\mathbf{e}}_{ij}$的值指和相机空间位姿${\mathbf{\xi }}_i$和路标点空间坐标${\mathbf{p}}_j$有关，因此雅克比矩阵对应的那一行为：
$${\mathbf{J}}_{ij}(\mathbf{x})=\left(0_{2\times 6},\dots 0_{2\times 6},\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{\xi }}_i},0_{2\times 6},\dots 0_{2\times 3},\dots 0_{2\times 3},\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{p}}_j},0_{2\times 3},\dots 0_{2\times 3}\right)$$这里顺便推导下$\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{\xi }}_i}$和$\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{p}}_j}$的具体形式，有图像像素位置和空间点位置关系如下：$$s_i\left[\begin{array}{c}{u}_{}\\ v_{}\\ 1\end{array}\right]=\mathbf{K}\exp \left({\mathbf{\xi }}^{\wedge }\right)\left[\begin{array}{c}{X}_{}\\ Y_{}\\ Z_{}\\ 1\end{array}\right]$$我们把空间点$\mathbf{P}$转相机坐标系下为${\mathbf{P}}^{\prime }$（取前三项）：$${\mathbf{P}}^{\prime }={\left(\exp \left({\xi }^{\wedge }\right)P\right)}_{13}={\left[X^{\prime },Y^{\prime },Z^{\prime }\right]}^T$$又相机坐标系到图像坐标系下有：$$s\mathbf{u}=\mathbf{K}{\mathbf{P}}^{\prime }$$因此有：$$u=f_x\frac{X^{\prime }}{Z^{\prime }}+c_x,v=f_y\frac{Y^{\prime }}{Z^{\prime }}+c_y$$我们按照第一节里面推导的扰动模型的思路，对误差$\mathbf{e}$左乘一个扰动有$$\frac{\partial \mathbf{e}}{\partial \delta \mathbf{\xi }}=\underset{\delta \xi \to 0}{\lim }\frac{e(\delta \mathbf{\xi }\oplus \xi )}{\delta \mathbf{\xi }}=\frac{\partial \mathbf{e}}{\partial {\mathbf{P}}^{\prime }}\frac{\partial {\mathbf{P}}^{\prime }}{\partial \delta \mathbf{\xi }}$$易得第一项$$\frac{\partial \mathbf{e}}{\partial {\mathbf{P}}^{\prime }}=-\left[\begin{array}{ccc}\frac{\partial u}{\partial X^{\prime }}& \frac{\partial u}{\partial Y^{\prime }}& \frac{\partial u}{\partial Z^{\prime }}\\ \frac{\partial v}{\partial X^{\prime }}& \frac{\partial v}{\partial Y^{\prime }}& \frac{\partial v}{\partial Z^{\prime }}\end{array}\right]=-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^2}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]$$那么第二项根据扰动模型的结论有$$\frac{\partial (\mathbf{T}\mathbf{P})}{\partial \delta \mathbf{\xi }}=(\mathbf{T}\mathbf{P}{)}^{\odot }=\left[\begin{array}{cc}\mathbf{I}& -{\mathbf{P}}^{\prime \wedge }\\ 0^T& 0^T\end{array}\right]$$因为${\mathbf{P}}^{\prime }$我们指需要取前三项，因此有$$\frac{\partial {\mathbf{P}}^{\prime }}{\partial \delta \mathbf{\xi }}=\left[\mathbf{I},-{\mathbf{P}}^{\prime \wedge }\right]=\left[\begin{array}{cccccc}1& 0& 0& 0& Z^{\prime }& -Y^{\prime }\\ 0& 1& 0& Z^{\prime }& 0& X^{\prime }\\ 0& 0& 1& Y^{\prime }& -X^{\prime }& 0\end{array}\right]$$因此可以获得对相机空间位姿的导数$$\frac{\partial \mathbf{e}}{\partial \delta \mathbf{\xi }}=-\left[\begin{array}{cccccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}& -\frac{f_x{X}^{\prime }{Y}^{\prime }}{Z^2}& f_x+\frac{f_x{X}^2}{Z^{\prime 2}}& -\frac{f_x{Y}^{\prime }}{Z^{\prime }}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}& -f_y-\frac{f_y{Y}^{\prime 2}}{Z^2}& \frac{f_y{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }}{Z^{\prime }}\end{array}\right]$$同理我们可以求其对空间点导数$$\frac{\partial \mathbf{e}}{\partial \mathbf{P}}=\frac{\partial \mathbf{e}}{\partial {\mathbf{P}}^{\prime }}\frac{\partial {\mathbf{P}}^{\prime }}{\partial \mathbf{P}}$$已知$${\mathbf{P}}^{\prime }=\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{P}=\mathbf{R}\mathbf{P}+\mathbf{t}$$则${\mathbf{P}}^{\prime }$对求导后有$\mathbf{P}$只剩下$\mathbf{R}$，因此$$\frac{\partial \mathbf{e}}{\partial \mathbf{P}}=-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]\mathbf{R}$$由此我们知道$\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{\xi }}_i}$和$\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{p}}_j}$的维度分别是$2\times 6$和$2\times 3$，那么$\mathbf{J}$矩阵以及其所构成的$\mathbf{H}$矩阵的形状如下：

因为相机空间位姿的数量通常远少于空间点的数量，因此最后$\mathbf{H}$矩阵最后的形式如下：

Step 3：矩阵求解

正因为有了矩阵的稀疏性，因此矩阵的快速求解才有可能，我们把需要求解的方程$\mathbf{H}\Delta \mathbf{x}=\mathbf{g}$按照区域变成如下形式：$$\left[\begin{array}{cc}\mathbf{B}& \mathbf{E}\\ {\mathbf{E}}^T& \mathbf{C}\end{array}\right]\left[\begin{array}{c}\Delta {\mathbf{x}}_c\\ \Delta {\mathbf{x}}_p\end{array}\right]=\left[\begin{array}{c}\mathbf{v}\\ \mathbf{w}\end{array}\right]$$求解方法一共有两种：第一种：Schur消元（也乘Marginalization（边缘化））；第二种：稀疏cholesky分解

Schur消元法
进行对线性方程组进行高斯消元，目标是消去右上角的$E$$$\left[\begin{array}{cc}I& -E{C}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{cc}B& E\\ E^T& C\end{array}\right]\left[\begin{array}{c}\Delta x_c\\ \Delta x_p\end{array}\right]=\left[\begin{array}{cc}I& -E{C}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{c}v\\ w\end{array}\right]$$整理得$$\left[\begin{array}{cc}B-E{C}^{-1}{E}^T& 0\\ E^T& C\end{array}\right]\left[\begin{array}{c}\Delta x_c\\ \Delta x_p\end{array}\right]=\left[\begin{array}{c}v-E{C}^{-1}w\\ w\end{array}\right]$$先求$\Delta x_c$再求$\Delta x_p$，整个过程之所以简单就是因为$C$矩阵是个对角阵，因此$C^{-1}$很好求。最后一个相对较难求解的部分是$$\left[\mathbf{B}-\mathbf{E}{\mathbf{C}}^{-1}{\mathbf{E}}^T\right]\Delta {\mathbf{x}}_c=\mathbf{v}-\mathbf{E}{\mathbf{C}}^{-1}\mathbf{w}$$这个方程只能通过传统的消元或者分解法进行求解，这个稀疏矩阵我们通常成为$S$矩阵，通过$S$矩阵可以获得相机和空间点之间的关系，如下图

表示相机$C_4$和相机 $C_1$ 和 $C_2$ 之间有共同观测的点。而 $S_{34}$ 为零则表示 $C_3$ 和 $C_4$ 之间没有共同观测的路标，另外，不同的SLAM框架这个矩阵的稀疏程度是不一样的，例如ORB_SLAM通过关键帧的选取原则导致这个矩阵是稠密的，而DSO中通过滑动窗口法保证了这个矩阵是稀疏的。
这里还有一点很有意思的解释是，在Schur消元的这个过程中，我们是将求$\left(\Delta {\mathbf{x}}_c,\Delta {\mathbf{x}}_p\right)$的问题转化为了先求$\Delta {\mathbf{x}}_c$，再求$\Delta {\mathbf{x}}_p$的过程，这一步相当于条件概率展开：$$P\left({\mathbf{x}}_c,{\mathbf{x}}_p\right)=P\left({\mathbf{x}}_c\right)\cdot P\left({\mathbf{x}}_p|{\mathbf{x}}_c\right)$$先求出关于$x_c$的边缘分布，这也就是为什么这里的Schur消元又称为边缘化（Marginalization）

稀疏cholesky分解
对线性方程组进行cholesky分解如下：$$\left[\begin{array}{cc}B& E\\ E^T& C\end{array}\right]=\left[\begin{array}{cc}{U}_{11}& U_{12}\\ 0& U_{22}\end{array}\right]\left[\begin{array}{ll}{U}_{11}& 0\\ U_{12}^T& U_{22}\end{array}\right]$$带入得$$\{\begin{array}{l}C=U_{22}{U}_{22}^T\\ E=U_{12}{U}_{22}^T\\ B=U_{11}{U}_{11}^T+U_{12}{U}_{12}^T\end{array}$$因为$C$是对角矩阵，因此容易分解；对E进行分解时，因为$U_{22}$是对角阵且已知因此容易分解；对C进行分解时，因为C矩阵较小，也相对容易分解，这种分解方相较Schur消元法要好的地方时，可以直接求$H$矩阵的逆。

（3）Pose Graph推导及分析

Pose Graph就是不关心空间点的位置，仅仅以相机的位姿作为优化节点的一种图优化方法，在ORB SLAM2里面就Covisibility Graph（共视图）就是一种典型的Pose Graph，操作是在有共视关系的位姿节点建立边，然后一起优化的一种图优化方法，Pose Graph的总体优化目标为$$\underset{\xi }{\min }\frac{1}{2}\sum _{i,j\in \mathcal{E}}{e}_{ij}^T{\mathbf{\Sigma }}_{ij}^{-1}{\mathbf{e}}_{ij}$$我们首先要确定$e_{ij}$怎么定义，第$i$和$j$帧相机的位姿变换的李代数表达方式如下：$$\Delta {\xi }_{ij}=\ln {\left(\exp \left({\left(-{\mathbf{\xi }}_i\right)}^{\wedge }\right)\exp \left({\mathbf{\xi }}_j^{\wedge }\right)\right)}^{\vee }$$
或者李群表达方式：$$\Delta T_{ij}=T_i^{-1}{T}_j$$这里$T_i$和$T_j$分别是第$i$和$j$帧相机的位姿（不一定是相连的），$\Delta T_{ij}$可以理解为由其他帧推过来的变换位姿，按照图优化的思想我们需要建立一个我们用来优化的误差，如下：$$\begin{array}{rl}{\mathbf{e}}_{ij}& =\ln {\left(\Delta {\mathbf{T}}_{ij}^{-1}{\mathbf{T}}_i^{-1}{\mathbf{T}}_j\right)}^{\vee }\\ & =\ln {\left(\exp \left({\left(-{\mathbf{\xi }}_{ij}\right)}^{\wedge }\right)\exp \left({\left(-{\mathbf{\xi }}_i\right)}^{\wedge }\right)\exp \left({\mathbf{\xi }}_j^{\wedge }\right)\right)}^{\vee }\end{array}$$，按照我们第一节给出的第二种较为简便的求导方式，对${\mathbf{\xi }}_i$和${\mathbf{\xi }}_i$分别乘一个左扰动$\delta {\xi }_i$和$\delta {\xi }_j$得：$$\begin{array}{rl}{\hat{e}}_{ij}& =\ln {\left({\mathbf{T}}_{ij}^{-1}{\mathbf{T}}_i^{-1}\exp \left({\left(-\delta {\mathbf{\xi }}_i\right)}^{\wedge }\right)\exp \left(\delta {\mathbf{\xi }}_j^{\wedge }\right){\mathbf{T}}_j\right)}^{\vee }\\ & =\ln {\left(\exp \left({\left(-{\mathbf{\xi }}_{ij}\right)}^{\wedge }\right)\exp \left({\left(-{\mathbf{\xi }}_i\right)}^{\wedge }\right)\exp \left({\left(-\delta {\xi }_i\right)}^{\wedge }\right)\exp \left(\delta {\xi }_j^{\wedge }\right)\exp \left({\mathbf{\xi }}_j^{\wedge }\right)\right)}^{\vee }\end{array}$$然后这里需要用一个性质$$\exp \left({\xi }^{\wedge }\right)\mathbf{T}=\mathbf{T}\exp \left({\left(\mathrm{Ad}\left({\mathbf{T}}^{-1}\right)\mathbf{\xi }\right)}^{\wedge }\right)$$其中$\mathrm{Ad}\left(T\right)$是$T$矩阵的伴随，这个性质的作用是移动扰动项至是式子的右侧，带入有$$\begin{array}{rl}{\hat{e}}_{ij}& =\ln {\left(T_{ij}^{-1}{T}_i^{-1}\exp \left({\left(-\delta {\xi }_i\right)}^{\wedge }\right)\exp \left(\delta {\xi }_j^{\wedge }\right)T_j\right)}^{\vee }\\ & =\ln (T_{ij}^{-1}{T}_i^{-1}{T}_j\exp \left({\left(-\mathrm{Ad}\left(T_j^{-1}\right)\delta {\xi }_i\right)}^{\wedge }\right)\exp {\left({\left(\mathrm{Ad}\left(T_j^{-1}\right)\delta {\xi }_j\right)}^{\wedge }\right)}^{\vee }\\ & \approx \ln (\exp \left(e_{ij}\right)\exp \left({\left(-\mathrm{Ad}\left(T_j^{-1}\right)\delta {\xi }_i\right)}^{\wedge }\right)\exp {\left({\left(\mathrm{Ad}\left(T_j^{-1}\right)\delta {\xi }_j\right)}^{\wedge }\right)}^{\vee }\end{array}$$根据BCH的近似公式$$\begin{array}{rl}\exp \left(\Delta {\xi }^{\wedge }\right)\exp \left({\xi }^{\wedge }\right)& \approx \exp \left({\left({\mathcal{J}}_l^{-1}\Delta \xi +\xi \right)}^{\wedge }\right)\\ \exp \left({\xi }^{\wedge }\right)\exp \left(\Delta {\xi }^{\wedge }\right)& \approx \exp \left({\left({\mathcal{J}}_r^{-1}\Delta \xi +\xi \right)}^{\wedge }\right)\end{array}$$有$${\hat{\mathbf{e}}}_{ij}\approx {\mathbf{e}}_{ij}-{\mathcal{J}}_r^{-1}\left({\mathbf{e}}_{ij}\right)\mathrm{Ad}\left({\mathbf{T}}_j^{-1}\right)\delta {\xi }_i+{\mathcal{J}}_r^{-1}\left({\mathbf{e}}_{ij}\right)\mathrm{Ad}\left({\mathbf{T}}_j^{-1}\right)\delta {\xi }_j$$（这里书上并未给出详细的证明，我不太确定我这里理解是不是对的）${\mathcal{J}}_{\mathbf{r}}$为一个六阶矩阵，形式较为复杂，我们可以取其近似形式$${\mathcal{J}}_r^{-1}\left({\mathbf{e}}_{ij}\right)\approx \mathbf{I}+\frac{1}{2}\left[\begin{array}{cc}{\mathbf{\varphi }}_{\mathbf{e}}^{\wedge }& {\mathbf{\rho }}_{\mathbf{e}}^{\wedge }\\ 0& {\varphi }_{\mathbf{e}}^{\wedge }\end{array}\right]$$有了误差的雅克比矩阵之后我们就可以开始计算$H$矩阵，然后迭代下降了，Pose Graph的推导是有些模糊的，之后抽时间去对着ORB SLAM2的源码看下具体是怎么实现，这篇总结写得够长了，先到这里。

最近还有看到概率机器人上提到的Graph SLAM，视觉SLAM的BA应该就是基于此发展起来的，有问题欢迎交流～