8.全局BA推导

再进行推导之前先回忆一下
我们知道对于某个单个节点 $T_i$ 的 $N$ 条边而言
每条边的误差
$f_{ij}=error_{ij}=z_{ij观测}-z_{ij}=z_{ij观测}-h(\xi_{i},p_j)=z_{ij观测}-h(T_i,p_j)$
那么这个单个节点的 $N$ 条边的一个整体误差总和公式的泰勒一阶近似
为
$\sum_{j=1}^nf_{j}(x+\Delta x)=\sum_{j=1}^n\Big(f_{j}(x)+J_{j}(x)^T\Delta x\Big)$
为了后续的推导，这里要先讲清楚一些说明
首先这里的 $x=[T_i | p_j]$ 是一个 $6+3$ 的维度
且每一个 $J_j(x)^T$ 应该是一个 $2\times(6+3)$ 的维度,这是因为误差 $z_{ij}=\begin{bmatrix}u\\v\end{bmatrix}$ 是个二维像素特征点
于是它由两部分组成，具体的推导可以看第五节

第一部分
$\frac{\partial error_{ij}}{\partial \delta T_i}=\frac{\partial error_{ij}}{\partial p'_j}.\frac{\partial p'_j}{\partial \delta T_i}$
$=-\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}$
可以简计为 $(\frac{\partial error_{ij}}{\partial \delta T_i})_{2\times 6}$
第二部分
$\frac{\partial error_{ij}}{\partial p_j}=-\begin{bmatrix}\frac{f_x}{Z'}&0&-\frac{f_xX'}{Z'^2}\\ 0&\frac{f_y}{Z'}&-\frac{f_yY'}{Z'^2} \end{bmatrix}R$
可简计为 $(\frac{\partial error_{ij}}{\partial p_j})_{2\times 3}$

加在一起就是 $J_j(x)^T=[(\frac{\partial error_{ij}}{\partial \delta T_i})_{2\times 6}|(\frac{\partial error_{ij}}{\partial p_j})_{2\times 3}]$
考虑多个节点的时候，如下图，此时 $T$ 和 $p$ 的数量关系是不存在具体的规律的如下图

image.png

我们回过头再来看单个节点所有边的这个误差求和公式
$\sum_{j=1}^nf_{j}(x+\Delta x)=\sum_{j=1}^n\Big(f_{j}(x)+J_{j}(x)^T\Delta x\Big)$
我能不能把求和项去掉
方法是有的，不如就以上图为例
假设相机 $C_1$ 的姿态为 $T_1$
我们知道对于 $T_1$ 观察 $p_1$ 的情况的误差边雅可比矩阵为

$J_{1-1}=\begin{bmatrix}(\frac{\partial error_{ij}}{\partial \delta T_1})_{2\times 6}&(\frac{\partial error_{i1}}{\partial p_1})_{2\times 3}\end{bmatrix}$

此时我们发现 $T_1$ 是可以看见 $p_1,p_2,p_3,p_4$ 这些点的，于是我们冒出了一个绝妙的点子，不如我们把这些点对应的雅可比矩阵都连起来
$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-1234}$ 这个下标做个说明，前面的 $1$ 指代对 $T_1$ 的偏导，后面的 $1234$ ，是对 $p_1,p_2,p_3,p_4$ 的各自的偏导，这非常重要

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

$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_2$ 节点啊，虽然它不是 $T_1$ 相关的边，但是也是全地图的一个节点，我们发现，它对于T_1也是不存在影响的，于是， $T_1$ 对 $T_2$ 的偏导也全为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}$

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

$\sum_{j=1}^nf_{j}(x+\Delta x)=\sum_{j=1}^n\Big(f_{j}(x)+J_{j}(x)^T\Delta x\Big)$ 的循环体拆掉

我们可以直接写
$f_{i}(x+\Delta x)=f_{i}(x)+J_{i}(x)^T\Delta x\$ 注意这里的函数已经变成了 $f_i(x)$ 也就是说只和单个姿态相关了，姿态内部和其他所有观察点以及所有其他姿态的关系已经统一整理起来了

这里的 $J_{i}(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} \end{bmatrix}$

值的注意的是，此时的 $x$ 变量也变了
此时待优化的变量可写为
$x=\begin{bmatrix}T_1&T_2&p_1&p_2&p_3&p_4&p_5&p_6\end{bmatrix}$

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

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

$\sum_{i=1}^mf_{i}(x+\Delta x)=\sum_{i=1}^mf_{i}(x)+J_{i}(x)^T\Delta x\$
就当前例子而言，因为场景中只包含两个观察位置 $T_1$ , $T_2$ ，所以 $m=2$
在这之前，我们不妨先对 $T_2$ 的雅可比矩阵看图抄写一遍，如果没看错的话应该是长这样
$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}$
之所以也写成这样，是为了维持对优化变量
$x=\begin{bmatrix}T_1&T_2&p_1&p_2&p_3&p_4&p_5&p_6\end{bmatrix}$ 的统一性

很好，现在我们也可以拆掉对于所有节点相关的求和循环了
$f(x+\Delta x)=f(x)+J(x)^T\Delta x\$
此时的优化变量不变
依然是 $x=\begin{bmatrix}T_1&T_2&p_1&p_2&p_3&p_4&p_5&p_6\end{bmatrix}$
而 $J(x)^T$ 变成了拼接的一个矩阵，这时候不是左右拼接，而是变成了上下拼接
我们把它写出来
$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}$

读者可能已经发现了一点规律，那么现在的问题是，假如说整个地图场景中有 $M$ 个 $T$ ,以及 $N$ 个 $p$
难道也是这么排，
然而事实就是如此，你会注意到这非常的浪费空间，小编也很无奈呢，不过在这之前先把事情交代完
对于任意的 $M$ 个 $T$ ,以及 $N$ 个 $p$ 的情况
我们定义优化变量
$x=\begin{bmatrix}T_1&\dots&T_m&p_1&\dots&p_n\end{bmatrix}$
我们定义
$x_c=\begin{bmatrix}T_1&\dots&T_m\end{bmatrix}$
$x_p=\begin{bmatrix}p_1&\dots&p_n\end{bmatrix}$
我们简化为下面的情况，
$x=\begin{bmatrix}x_c&x_p\end{bmatrix}$

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

那么我们整理
$f(x+\Delta x)=f(x)+J(x)^T\Delta x\$
$f(x+\Delta x)=f(x)+[F | E]. \begin{bmatrix}\Delta x_c\\\Delta x_p\end{bmatrix}$
$f(x+\Delta x)=f(x)+F\Delta x_c +E\Delta x_p$

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

$\frac{1}{2}||f(x+\Delta x)||^2=\frac{1}{2}||f(x)+J(x)^T\Delta x||^2$
或
$\frac{1}{2}||f(x+\Delta x)||^2=\frac{1}{2}||f(x)+F\Delta x_c +E\Delta x_p||^2$
求全微分令各方向偏导数为零求极小值

这里就不算了，前面算了太多遍，我直接写出高斯牛顿迭代公式
$J(x)J(x)^T\Delta X=-J(x)f(x)$ 这个等式中 $J(x)$ 默认是竖着放的，所以才这么写

而在我们这里的例子中 $J(x)=[F|E]$ 默认定义就是横着放的，所以此处需要改成
$J(x)^TJ(x)\Delta X=-J(x)^Tf(x)$

也就是说这里的 $H=J(x)^TJ(x)=\begin{bmatrix}F\\E\end{bmatrix}\begin{bmatrix}F&E\end{bmatrix}=\begin{bmatrix}F^TF&F^TE\\E^TF&E^TE\end{bmatrix}$

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

image.png

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

image.png

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

image.png

$\frac{1}{2}||f(x+\Delta x)||^2=\frac{1}{2}||f(x)+F\Delta x_c +E\Delta x_p||^2$
可以看见这样的矩阵是一个很稀疏的矩阵，而且有明显特征，对这样的
$H\Delta x=g$ ,就可以做一些更快速的做法
在slam中常用的做法是 $schur$ 消元，也称 $(Marginalization)$ 边缘化
我们注意到
H矩阵的四个块中，左上角，和右下角，都是对角块矩阵，于是我们重新命名这几个块如下

$H=\begin{bmatrix}F^TF&F^TE\\E^TF&E^TE\end{bmatrix}=\begin{bmatrix}B&E\\E^T&C\end{bmatrix}$

image.png

于是线性方程组

H\Delta x=g

可以写成

image.png

人面猴
序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
沈念sama阅读 202,529评论 5赞 475
死咒
序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
沈念sama阅读 85,015评论 2赞 379
救了他两次的神仙让他今天三更去死
文/潘晓璐我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
开封第一讲书人阅读 149,409评论 0赞 335
道士缉凶录：失踪的卖姜人
文/不坏的土叔我叫张陵，是天一观的道长。经常有香客问我，道长，这世上最难降的妖魔是什么？我笑而不...
开封第一讲书人阅读 54,385评论 1赞 273
港岛之恋（遗憾婚礼）
正文为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
茶点故事阅读 63,387评论 5赞 364
恶毒庶女顶嫁案：这布局不是一般人想出来的
文/花漫我一把揭开白布。她就那样静静地躺着，像睡着了一般。火红的嫁衣衬着肌肤如雪。梳的纹丝不乱的头发上，一...
开封第一讲书人阅读 48,466评论 1赞 281
城市分裂传说
那天，我揣着相机与录音，去河边找鬼。笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。我是一名探鬼主播，决...
沈念sama阅读 37,880评论 3赞 395
双鸳鸯连环套：你想象不到人心有多黑
文/苍兰香墨我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
开封第一讲书人阅读 36,528评论 0赞 256
万荣杀人案实录
序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
沈念sama阅读 40,727评论 1赞 295
护林员之死
正文独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋以下内容为张勋视角年9月15日...
茶点故事阅读 35,528评论 2赞 319
白月光启示录
正文我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
茶点故事阅读 37,602评论 1赞 329
活死人
序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
沈念sama阅读 33,302评论 4赞 318
日本核电站爆炸内幕
正文年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
茶点故事阅读 38,873评论 3赞 306
男人毒药：我在死后第九天来索命
文/蒙蒙一、第九天我趴在偏房一处隐蔽的房顶上张望。院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
开封第一讲书人阅读 29,890评论 0赞 19
一桩弑父案，背后竟有这般阴谋
文/苍兰香墨我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。一阵脚步声响...
开封第一讲书人阅读 31,132评论 1赞 259
情欲美人皮
我被黑心中介骗来泰国打工，没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。一个月前我还...
沈念sama阅读 42,777评论 2赞 349
代替公主和亲
正文我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
茶点故事阅读 42,310评论 2赞 342

8.全局BA推导

推荐阅读更多精彩内容