8.全局BA推导

再进行推导之前先回忆一下
我们知道对于某个单个节点的条边而言
每条边的误差

那么这个单个节点的条边的一个整体误差总和公式的泰勒一阶近似
为

为了后续的推导，这里要先讲清楚一些说明
首先这里的是一个的维度
且每一个应该是一个的维度,这是因为误差是个二维像素特征点
于是它由两部分组成，具体的推导可以看第五节

第一部分

$=-\begin{bmatrix} \frac{f_x}{Z'}&0&-\frac{f_xX'}{Z'^2}&-\frac{f_xX'Y'}{Z'^2}&f_x+\frac{f_xX'^2}{Z'^2}&-\frac{f_xY'}{Z'}\\ 0&\frac{f_y}{Z'}&-\frac{f_yY'}{Z'^2}&-f_y-\frac{f_yY'^2}{Z'^2}&\frac{f_yX'Y'}{Z'^2}&\frac{f_yX'}{Z'} \end{bmatrix}$
可以简计为
第二部分

可简计为

加在一起就是
考虑多个节点的时候，如下图，此时和的数量关系是不存在具体的规律的如下图

image.png

我们回过头再来看单个节点所有边的这个误差求和公式

我能不能把求和项去掉
方法是有的，不如就以上图为例
假设相机的姿态为
我们知道对于观察的情况的误差边雅可比矩阵为

此时我们发现是可以看见这些点的，于是我们冒出了一个绝妙的点子，不如我们把这些点对应的雅可比矩阵都连起来
$J_{1-1234}= \begin{bmatrix}(\frac{\partial error_{ij}}{\partial \delta T_1})_{2\times 6} &(\frac{\partial error_{i1}}{\partial p_1})_{2\times 3} &(\frac{\partial error_{i2}}{\partial p_2})_{2\times 3} &(\frac{\partial error_{i3}}{\partial p_3})_{2\times 3} &(\frac{\partial error_{i4}}{\partial p_4})_{2\times 3} \end{bmatrix}$

这里需要对这个下标做个说明，前面的指代对的偏导，后面的，是对的各自的偏导，这非常重要

进一步想，我们发现是看不见的，那能不能也把偏导数一起写进来，其实也是可以的，我们知道因为看不见，那么意味着这两个点对于整体边的误差影响是，为了不失一般性，我们向下面这样扩充

$J_{1-123456}= \begin{bmatrix}(\frac{\partial error_{ij}}{\partial \delta T_1})_{2\times 6} &(\frac{\partial error_{i1}}{\partial p_1})_{2\times 3} &(\frac{\partial error_{i2}}{\partial p_2})_{2\times 3} &(\frac{\partial error_{i3}}{\partial p_3})_{2\times 3} &(\frac{\partial error_{i4}}{\partial p_4})_{2\times 3} &(0)_{2\times 3} &(0)_{2\times 3} \end{bmatrix}$

再进一步，我们说，这里好像还有一个节点啊，虽然它不是相关的边，但是也是全地图的一个节点，我们发现，它对于T_1也是不存在影响的，于是，对的偏导也全为0

$J_{1-123456}= \begin{bmatrix}(\frac{\partial error_{ij}}{\partial \delta T_1})_{2\times 6} &(0)_{2\times 6} &(\frac{\partial error_{i1}}{\partial p_1})_{2\times 3} &(\frac{\partial error_{i2}}{\partial p_2})_{2\times 3} &(\frac{\partial error_{i3}}{\partial p_3})_{2\times 3} &(\frac{\partial error_{i4}}{\partial p_4})_{2\times 3} &(0)_{2\times 3} &(0)_{2\times 3} \end{bmatrix}$

此时这里单个节点对于全体地图所有姿态和观察点的雅各比矩阵，就是这么长了
到这一步，我们已经把单节点的

的循环体拆掉

我们可以直接写
注意这里的函数已经变成了也就是说只和单个姿态相关了，姿态内部和其他所有观察点以及所有其他姿态的关系已经统一整理起来了

这里的形如 $\begin{bmatrix}(\frac{\partial error_{ij}}{\partial \delta T_1})_{2\times 6} &(0)_{2\times 6} &(\frac{\partial error_{i1}}{\partial p_1})_{2\times 3} &(\frac{\partial error_{i2}}{\partial p_2})_{2\times 3} &(\frac{\partial error_{i3}}{\partial p_3})_{2\times 3} &(\frac{\partial error_{i4}}{\partial p_4})_{2\times 3} &(0)_{2\times 3} &(0)_{2\times 3} \end{bmatrix}$

值的注意的是，此时的变量也变了
此时待优化的变量可写为

这应该是很自然的事情了吧，都不需要多解释了

事情还没完，刚才说了，这只是其中的单个节点
对于上面的方程，我们还需要再套一层,以表示所有的节点

就当前例子而言，因为场景中只包含两个观察位置,，所以
在这之前，我们不妨先对的雅可比矩阵看图抄写一遍，如果没看错的话应该是长这样
$J_{2-123456}= \begin{bmatrix}(0)_{2\times 6} &(\frac{\partial error_{ij}}{\partial \delta T_2})_{2\times 6} &(0)_{2\times 3} &(0)_{2\times 3} &(\frac{\partial error_{i3}}{\partial p_3})_{2\times 3} &(\frac{\partial error_{i4}}{\partial p_4})_{2\times 3} &(\frac{\partial error_{i5}}{\partial p_5})_{2\times 3} &(\frac{\partial error_{i6}}{\partial p_6})_{2\times 3} \end{bmatrix}$
之所以也写成这样，是为了维持对优化变量
的统一性

很好，现在我们也可以拆掉对于所有节点相关的求和循环了

此时的优化变量不变
依然是
而变成了拼接的一个矩阵，这时候不是左右拼接，而是变成了上下拼接
我们把它写出来
$J(x)^T=\begin{bmatrix} (\frac{\partial error_{ij}}{\partial \delta T_1})_{2\times 6} &(0)_{2\times 6} &(\frac{\partial error_{i1}}{\partial p_1})_{2\times 3} &(\frac{\partial error_{i2}}{\partial p_2})_{2\times 3} &(\frac{\partial error_{i3}}{\partial p_3})_{2\times 3} &(\frac{\partial error_{i4}}{\partial p_4})_{2\times 3} &(0)_{2\times 3} &(0)_{2\times 3} \\(0)_{2\times 6} &(\frac{\partial error_{ij}}{\partial \delta T_2})_{2\times 6} &(0)_{2\times 3} &(0)_{2\times 3} &(\frac{\partial error_{i3}}{\partial p_3})_{2\times 3} &(\frac{\partial error_{i4}}{\partial p_4})_{2\times 3} &(\frac{\partial error_{i5}}{\partial p_5})_{2\times 3} &(\frac{\partial error_{i6}}{\partial p_6})_{2\times 3} \end{bmatrix}$

读者可能已经发现了一点规律，那么现在的问题是，假如说整个地图场景中有个,以及个
难道也是这么排，
然而事实就是如此，你会注意到这非常的浪费空间，小编也很无奈呢，不过在这之前先把事情交代完
对于任意的个,以及个的情况
我们定义优化变量

我们定义

我们简化为下面的情况，

而对于雅可比矩阵也是同理，可以分成两块
前面部分是姿态矩阵的偏导数，且全在对角线上，后边部分是所有可看见的世界坐标点的偏导数矩阵
我们可以简单的计作

那么我们整理

接下来我们知道，后续需要迭代求解全局BA，总是逃不过一个牛顿迭代增量公式的，
可以通过

或

求全微分令各方向偏导数为零求极小值

这里就不算了，前面算了太多遍，我直接写出高斯牛顿迭代公式
这个等式中默认是竖着放的，所以才这么写

而在我们这里的例子中默认定义就是横着放的，所以此处需要改成

也就是说这里的

为了较好的展示矩阵长什么样子，这里假设全局地图里情况如图

image.png

那么此时得到的雅可比矩阵和矩阵如下

image.png

现实中一般相机位姿的数量，以及路标点数量，会存在如下关系，即,那么此时的矩阵很大概率长这个样子

image.png

可以看见这样的矩阵是一个很稀疏的矩阵，而且有明显特征，对这样的
,就可以做一些更快速的做法
在slam中常用的做法是消元，也称边缘化
我们注意到
H矩阵的四个块中，左上角，和右下角，都是对角块矩阵，于是我们重新命名这几个块如下

image.png

于是线性方程组可以写成

image.png

8.全局BA推导

你可能感兴趣的:(8.全局BA推导)