8.全局BA推導(dǎo)

再進(jìn)行推導(dǎo)之前先回憶一下
我們知道對(duì)于某個(gè)單個(gè)節(jié)點(diǎn) $T_i$ 的 $N$ 條邊而言
每條邊的誤差
$f_{ij}=error_{ij}=z_{ij觀測(cè)}-z_{ij}=z_{ij觀測(cè)}-h(\xi_{i},p_j)=z_{ij觀測(cè)}-h(T_i,p_j)$
那么這個(gè)單個(gè)節(jié)點(diǎn)的 $N$ 條邊的一個(gè)整體誤差總和公式的泰勒一階近似
為
$\sum_{j=1}^nf_{j}(x+\Delta x)=\sum_{j=1}^n\Big(f_{j}(x)+J_{j}(x)^T\Delta x\Big)$
為了后續(xù)的推導(dǎo)述呐，這里要先講清楚一些說(shuō)明
首先這里的 $x=[T_i | p_j]$ 是一個(gè) $6+3$ 的維度
且每一個(gè) $J_j(x)^T$ 應(yīng)該是一個(gè) $2\times(6+3)$ 的維度,這是因?yàn)檎`差 $z_{ij}=\begin{bmatrix}u\\v\end{bmatrix}$ 是個(gè)二維像素特征點(diǎn)
于是它由兩部分組成，具體的推導(dǎo)可以看第五節(jié)

第一部分
$\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}$
可以簡(jiǎn)計(jì)為 $(\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$
可簡(jiǎn)計(jì)為 $(\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}]$
考慮多個(gè)節(jié)點(diǎn)的時(shí)候，如下圖压语，此時(shí) $T$ 和 $p$ 的數(shù)量關(guān)系是不存在具體的規(guī)律的如下圖

image.png

我們回過(guò)頭再來(lái)看單個(gè)節(jié)點(diǎn)所有邊的這個(gè)誤差求和公式
$\sum_{j=1}^nf_{j}(x+\Delta x)=\sum_{j=1}^n\Big(f_{j}(x)+J_{j}(x)^T\Delta x\Big)$
我能不能把求和項(xiàng)去掉
方法是有的，不如就以上圖為例
假設(shè)相機(jī) $C_1$ 的姿態(tài)為 $T_1$
我們知道對(duì)于 $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}$

此時(shí)我們發(fā)現(xiàn) $T_1$ 是可以看見(jiàn) $p_1,p_2,p_3,p_4$ 這些點(diǎn)的二汛，于是我們冒出了一個(gè)絕妙的點(diǎn)子蔼紧，不如我們把這些點(diǎn)對(duì)應(yīng)的雅可比矩陣都連起來(lái)
$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}$

這里需要對(duì) $J_{1-1234}$ 這個(gè)下標(biāo)做個(gè)說(shuō)明，前面的 $1$ 指代對(duì) $T_1$ 的偏導(dǎo)，后面的 $1234$ 苏遥，是對(duì) $p_1,p_2,p_3,p_4$ 的各自的偏導(dǎo)饼拍，這非常重要

進(jìn)一步想，我們發(fā)現(xiàn) $T_1$ 是看不見(jiàn) $p_5,p_6$ 的田炭，那能不能也把偏導(dǎo)數(shù)一起寫(xiě)進(jìn)來(lái)师抄，其實(shí)也是可以的，我們知道因?yàn)?img class="math-inline" src="https://math.jianshu.com/math?formula=T_1" alt="T_1" mathimg="1">看不見(jiàn) $p_5,p_6$ 教硫，那么意味著這兩個(gè)點(diǎn)對(duì)于 $T_1$ 整體邊的誤差影響是 $0$ 司澎，為了不失一般性，我們向下面這樣擴(kuò)充

$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}$

再進(jìn)一步栋豫，我們說(shuō)挤安，這里好像還有一個(gè) $T_2$ 節(jié)點(diǎn)啊，雖然它不是 $T_1$ 相關(guān)的邊丧鸯，但是也是全地圖的一個(gè)節(jié)點(diǎn)蛤铜，我們發(fā)現(xiàn)，它對(duì)于T_1也是不存在影響的丛肢，于是围肥， $T_1$ 對(duì) $T_2$ 的偏導(dǎo)也全為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}$

此時(shí)這里單個(gè)節(jié)點(diǎn)對(duì)于全體地圖所有姿態(tài)和觀察點(diǎn)的雅各比矩陣，就是這么長(zhǎng)了
到這一步蜂怎，我們已經(jīng)把單節(jié)點(diǎn)的

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

我們可以直接寫(xiě)
$f_{i}(x+\Delta x)=f_{i}(x)+J_{i}(x)^T\Delta x\$ 注意這里的函數(shù)已經(jīng)變成了 $f_i(x)$ 也就是說(shuō)只和單個(gè)姿態(tài)相關(guān)了穆刻，姿態(tài)內(nèi)部和其他所有觀察點(diǎn)以及所有其他姿態(tài)的關(guān)系已經(jīng)統(tǒng)一整理起來(lái)了

這里的 $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}$

值的注意的是，此時(shí)的 $x$ 變量也變了
此時(shí)待優(yōu)化的變量可寫(xiě)為
$x=\begin{bmatrix}T_1&T_2&p_1&p_2&p_3&p_4&p_5&p_6\end{bmatrix}$

這應(yīng)該是很自然的事情了吧杠步，都不需要多解釋了

事情還沒(méi)完氢伟，剛才說(shuō)了，這只是其中的單個(gè)節(jié)點(diǎn)
對(duì)于上面的方程幽歼，我們還需要再套一層,以表示所有的節(jié)點(diǎn)

$\sum_{i=1}^mf_{i}(x+\Delta x)=\sum_{i=1}^mf_{i}(x)+J_{i}(x)^T\Delta x\$
就當(dāng)前例子而言朵锣，因?yàn)閳?chǎng)景中只包含兩個(gè)觀察位置 $T_1$ , $T_2$ ，所以 $m=2$
在這之前甸私，我們不妨先對(duì) $T_2$ 的雅可比矩陣看圖抄寫(xiě)一遍诚些，如果沒(méi)看錯(cuò)的話(huà)應(yīng)該是長(zhǎng)這樣
$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}$
之所以也寫(xiě)成這樣，是為了維持對(duì)優(yōu)化變量
$x=\begin{bmatrix}T_1&T_2&p_1&p_2&p_3&p_4&p_5&p_6\end{bmatrix}$ 的統(tǒng)一性

很好皇型，現(xiàn)在我們也可以拆掉對(duì)于所有節(jié)點(diǎn)相關(guān)的求和循環(huán)了
$f(x+\Delta x)=f(x)+J(x)^T\Delta x\$
此時(shí)的優(yōu)化變量不變
依然是 $x=\begin{bmatrix}T_1&T_2&p_1&p_2&p_3&p_4&p_5&p_6\end{bmatrix}$
而 $J(x)^T$ 變成了拼接的一個(gè)矩陣诬烹，這時(shí)候不是左右拼接，而是變成了上下拼接
我們把它寫(xiě)出來(lái)
$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}$

讀者可能已經(jīng)發(fā)現(xiàn)了一點(diǎn)規(guī)律弃鸦，那么現(xiàn)在的問(wèn)題是绞吁，假如說(shuō)整個(gè)地圖場(chǎng)景中有 $M$ 個(gè) $T$ ,以及 $N$ 個(gè) $p$
難道也是這么排，
然而事實(shí)就是如此寡键，你會(huì)注意到這非常的浪費(fèi)空間掀泳，小編也很無(wú)奈呢瞪浸，不過(guò)在這之前先把事情交代完
對(duì)于任意的 $M$ 個(gè) $T$ ,以及 $N$ 個(gè) $p$ 的情況
我們定義優(yōu)化變量
$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}$
我們簡(jiǎn)化為下面的情況蕾殴，
$x=\begin{bmatrix}x_c&x_p\end{bmatrix}$

而對(duì)于雅可比矩陣也是同理载迄，可以分成兩塊
前面部分是姿態(tài)矩陣的偏導(dǎo)數(shù)捉兴，且全在對(duì)角線(xiàn)上，后邊部分是所有可看見(jiàn)的世界坐標(biāo)點(diǎn) $p$ 的偏導(dǎo)數(shù)矩陣
我們可以簡(jiǎn)單的計(jì)作
$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$

接下來(lái)我們知道马僻，后續(xù)需要迭代求解全局BA庄拇，總是逃不過(guò)一個(gè)牛頓迭代增量公式的，
可以通過(guò)

$\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$
求全微分令各方向偏導(dǎo)數(shù)為零求極小值

這里就不算了韭邓，前面算了太多遍措近，我直接寫(xiě)出高斯牛頓迭代公式
$J(x)J(x)^T\Delta X=-J(x)f(x)$ 這個(gè)等式中 $J(x)$ 默認(rèn)是豎著放的，所以才這么寫(xiě)

而在我們這里的例子中 $J(x)=[F|E]$ 默認(rèn)定義就是橫著放的女淑，所以此處需要改成
$J(x)^TJ(x)\Delta X=-J(x)^Tf(x)$

也就是說(shuō)這里的 $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}$

為了較好的展示矩陣長(zhǎng)什么樣子瞭郑，這里假設(shè)全局地圖里情況如圖

image.png

那么此時(shí)得到的雅可比矩陣和 $H$ 矩陣如下

image.png

現(xiàn)實(shí)中一般相機(jī)位姿的數(shù)量 $m$ ，以及路標(biāo)點(diǎn)數(shù)量 $n$ 鸭你，會(huì)存在如下關(guān)系屈张，即 $n >> m$ ,那么此時(shí)的 $H$ 矩陣很大概率長(zhǎng)這個(gè)樣子

image.png

$\frac{1}{2}||f(x+\Delta x)||^2=\frac{1}{2}||f(x)+F\Delta x_c +E\Delta x_p||^2$
可以看見(jiàn)這樣的矩陣是一個(gè)很稀疏的矩陣，而且有明顯特征袱巨，對(duì)這樣的
$H\Delta x=g$ ,就可以做一些更快速的做法
在slam中常用的做法是 $schur$ 消元阁谆，也稱(chēng) $(Marginalization)$ 邊緣化
我們注意到
H矩陣的四個(gè)塊中，左上角愉老，和右下角场绿，都是對(duì)角塊矩陣，于是我們重新命名這幾個(gè)塊如下

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

image.png

于是線(xiàn)性方程組

H\Delta x=g

可以寫(xiě)成

image.png

?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請(qǐng)聯(lián)系作者

人面猴
序言：七十年代末嫉入，一起剝皮案震驚了整個(gè)濱河市焰盗，隨后出現(xiàn)的幾起案子，更是在濱河造成了極大的恐慌劝贸，老刑警劉巖姨谷，帶你破解...
沈念sama閱讀 206,214評(píng)論 6贊 481
死咒
序言：濱河連續(xù)發(fā)生了三起死亡事件，死亡現(xiàn)場(chǎng)離奇詭異映九，居然都是意外死亡，警方通過(guò)查閱死者的電腦和手機(jī)瞎颗，發(fā)現(xiàn)死者居然都...
沈念sama閱讀 88,307評(píng)論 2贊 382
救了他兩次的神仙讓他今天三更去死
文/潘曉璐我一進(jìn)店門(mén)件甥，熙熙樓的掌柜王于貴愁眉苦臉地迎上來(lái)，“玉大人哼拔，你說(shuō)我怎么就攤上這事引有。” “怎么了倦逐？”我有些...
開(kāi)封第一講書(shū)人閱讀 152,543評(píng)論 0贊 341
道士緝兇錄：失蹤的賣(mài)姜人
文/不壞的土叔我叫張陵譬正，是天一觀的道長(zhǎng)。經(jīng)常有香客問(wèn)我，道長(zhǎng)曾我，這世上最難降的妖魔是什么粉怕？我笑而不...
開(kāi)封第一講書(shū)人閱讀 55,221評(píng)論 1贊 279
?港島之戀（遺憾婚禮）
正文為了忘掉前任，我火速辦了婚禮抒巢，結(jié)果婚禮上贫贝，老公的妹妹穿的比我還像新娘。我一直安慰自己蛉谜，他們只是感情好稚晚，可當(dāng)我...
茶點(diǎn)故事閱讀 64,224評(píng)論 5贊 371
惡毒庶女頂嫁案：這布局不是一般人想出來(lái)的
文/花漫我一把揭開(kāi)白布。她就那樣靜靜地躺著型诚，像睡著了一般客燕。火紅的嫁衣襯著肌膚如雪。梳的紋絲不亂的頭發(fā)上狰贯，一...
開(kāi)封第一講書(shū)人閱讀 49,007評(píng)論 1贊 284
城市分裂傳說(shuō)
那天幸逆，我揣著相機(jī)與錄音，去河邊找鬼暮现。笑死还绘，一個(gè)胖子當(dāng)著我的面吹牛，可吹牛的內(nèi)容都是我干的栖袋。我是一名探鬼主播拍顷，決...
沈念sama閱讀 38,313評(píng)論 3贊 399
雙鴛鴦連環(huán)套：你想象不到人心有多黑
文/蒼蘭香墨我猛地睜開(kāi)眼，長(zhǎng)吁一口氣：“原來(lái)是場(chǎng)噩夢(mèng)啊……” “哼塘幅！你這毒婦竟也來(lái)了昔案？” 一聲冷哼從身側(cè)響起，我...
開(kāi)封第一講書(shū)人閱讀 36,956評(píng)論 0贊 259
萬(wàn)榮殺人案實(shí)錄
序言：老撾萬(wàn)榮一對(duì)情侶失蹤电媳，失蹤者是張志新（化名）和其女友劉穎踏揣，沒(méi)想到半個(gè)月后，有當(dāng)?shù)厝嗽跇?shù)林里發(fā)現(xiàn)了一具尸體匾乓，經(jīng)...
沈念sama閱讀 43,441評(píng)論 1贊 300
?護(hù)林員之死
正文獨(dú)居荒郊野嶺守林人離奇死亡捞稿，尸身上長(zhǎng)有42處帶血的膿包…… 初始之章·張勛以下內(nèi)容為張勛視角年9月15日...
茶點(diǎn)故事閱讀 35,925評(píng)論 2贊 323
?白月光啟示錄
正文我和宋清朗相戀三年，在試婚紗的時(shí)候發(fā)現(xiàn)自己被綠了拼缝。大學(xué)時(shí)的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片娱局。...
茶點(diǎn)故事閱讀 38,018評(píng)論 1贊 333
活死人
序言：一個(gè)原本活蹦亂跳的男人離奇死亡，死狀恐怖咧七，靈堂內(nèi)的尸體忽然破棺而出衰齐，到底是詐尸還是另有隱情，我是刑警寧澤继阻，帶...
沈念sama閱讀 33,685評(píng)論 4贊 322
?日本核電站爆炸內(nèi)幕
正文年R本政府宣布耻涛，位于F島的核電站废酷，受9級(jí)特大地震影響，放射性物質(zhì)發(fā)生泄漏抹缕。R本人自食惡果不足惜澈蟆，卻給世界環(huán)境...
茶點(diǎn)故事閱讀 39,234評(píng)論 3贊 307
男人毒藥：我在死后第九天來(lái)索命
文/蒙蒙一、第九天我趴在偏房一處隱蔽的房頂上張望歉嗓。院中可真熱鬧丰介，春花似錦、人聲如沸鉴分。這莊子的主人今日做“春日...
開(kāi)封第一講書(shū)人閱讀 30,240評(píng)論 0贊 19
一樁弒父案，背后竟有這般陰謀
文/蒼蘭香墨我抬頭看了看天上的太陽(yáng)志珍。三九已至橙垢，卻和暖如春，著一層夾襖步出監(jiān)牢的瞬間伦糯，已是汗流浹背柜某。一陣腳步聲響...
開(kāi)封第一講書(shū)人閱讀 31,464評(píng)論 1贊 261
情欲美人皮
我被黑心中介騙來(lái)泰國(guó)打工，沒(méi)想到剛下飛機(jī)就差點(diǎn)兒被人妖公主榨干…… 1. 我叫王不留敛纲，地道東北人喂击。一個(gè)月前我還...
沈念sama閱讀 45,467評(píng)論 2贊 352
代替公主和親
正文我出身青樓，卻偏偏與公主長(zhǎng)得像淤翔，于是被迫代替她去往敵國(guó)和親翰绊。傳聞我的和親對(duì)象是個(gè)殘疾皇子，可洞房花燭夜當(dāng)晚...
茶點(diǎn)故事閱讀 42,762評(píng)論 2贊 345

8.全局BA推導(dǎo)

推薦閱讀更多精彩內(nèi)容