ice-ba分析及gba代碼解析by崔華坤-5

1、ICE-BA分析及 GBA 代碼崔華坤 2018.10.30目錄ICE-BA分析及 GBA 代碼. 1一、BA 求解31.11.21.31.4建立 Normal Equation3建立 Schur Complement3求解 camera 位姿3求解路標(biāo)點(diǎn)31.5 ICE-BA 與傳統(tǒng) Batch BA 的區(qū)別4二、圖解 Hes矩陣42.1 xyz 參數(shù)化的視覺約束4逆深度參數(shù)化的視覺約束6視覺和 IMU 聯(lián)合約束72.4 H|b矩陣的數(shù)學(xué)形式8三、因子圖分析法 Factraph9觀測因子視覺約束9運(yùn)動因子IMU 約束10VIO 約束的因子圖表示11先驗(yàn)因子11四、ICE-BA 后端非線性優(yōu)化

2、的數(shù)學(xué)說明124.14.2狀態(tài)向量12目標(biāo)函數(shù)134.3視覺約束144.4 IMU 約束144.5 Marginalization 約束151五、ICE-BA 中的一些變量195.1 m_Zs 和 m_zs19六、GBA 代碼. 206.1更新視覺約束 UpdateFactorsFeature216.26.3更新 IMU 約束 UpdateFactorsIMU24更新位姿先驗(yàn) UpdateFactorsPriorCamerae276.46.5更新運(yùn)動先驗(yàn) UpdateFactorsPriorCameraMotion28更新路標(biāo)點(diǎn)先驗(yàn) UpdateFactorsPriorDepth296.66.

3、76.8更新 Schur complement30求解相機(jī)位姿32求解路標(biāo)點(diǎn)34五、參考文獻(xiàn)34六、附錄346.1 Marginalization 中的證明346.2圖 12 中的 Hes和 b 矩陣數(shù)學(xué)形式352一、BA 求解SLAM 優(yōu)化求解分為如下四步：是一個 BA（Bundle Adjustment）問題，BA 的求解過1.1 建立 Normal Equation以 Gauewton 法為例，求解優(yōu)化變量的增量方程（又稱 Normal Equation）可寫為： = (1)其中 J 為殘差 e 關(guān)于整體優(yōu)化變量 X 的 Jacobian，上式可簡寫 = ，其中H 為近似的 Hes矩陣，

4、公式(1)可寫成矩陣形式： (2) = 其中 U 是相機(jī)位姿間的關(guān)系，V 是路標(biāo)點(diǎn)間的關(guān)系，通常 U、V 是對角陣，W 是相機(jī)與路標(biāo)點(diǎn)的關(guān)系。1.2 建立 Schur Complement求解增量方程時常用的方法是 Schur Complement，即先邊緣化掉路標(biāo)點(diǎn)，：( 1) = ( 1)(3)上式可簡寫為S = ，S 是表示相機(jī)之間共視關(guān)系的矩陣，對于滑動窗口來說，通常 S 是稠密的，即兩兩相機(jī)之間都能看到相同的路標(biāo)點(diǎn)，如 ICE-BA 中的 LBA；而 KeyFrame 之間的共視點(diǎn)通常比較少，除了相鄰的關(guān)鍵幀，以及閉環(huán)兩端的關(guān)鍵幀外，其他關(guān)鍵幀之間往往并沒有公共點(diǎn)，因此 S 通常是稀

5、疏的，如 ICE-BA 中的 GBA。值得注意的是，建立 Schur Complement 方程這一步是 BA 求解中最耗時的部分。1.3 求解 camera 位姿利用 S 矩陣的稀疏性，ICE-BA 用預(yù)處理共軛梯度（Preconditioned Conjugate Gradient，簡稱 PCG）算法，高效求解公式(3)的舒爾補(bǔ)方程，得到。1.4 求解路標(biāo)點(diǎn)將代入到下式，求解：3 = 1( )1.5 ICE-BA 與傳統(tǒng) Batch BA 的區(qū)別(4)ICE-BA 認(rèn)為在建立公式（1）的增量方程時，對之前已經(jīng)算過的那些狀態(tài)向量再次計(jì)算其 Jacobian 對精度不會有太大改善，因此沒必要重

6、新線性化（線性化就是計(jì)算 Jacobian），因?yàn)榧词怪匦掠?jì)算，得到的 Jacobian 也沒什么變化。并且，在公式（3）中也沒必要對整體H|b矩陣進(jìn)行消元，而是只對變化的進(jìn)行更新。最后，在計(jì)算公式（4）時，只計(jì)算變化的路標(biāo)點(diǎn)即可。對于沒有變化的狀態(tài)向量的臨時值則一直保存下來，避免重新計(jì)算?？偨Y(jié)來說，在 ICE-BA 中更新 Factor 有三種情況：1.2.該 Factor 在本次迭代中沒有變化，則不更新；該 Factor 是新 Factor，則在原來基礎(chǔ)上 +=新 Factor。如到一個新的路標(biāo)點(diǎn)，則對此新路標(biāo)點(diǎn)的觀測約束就是一個新的 Factor；該 Factor 已存在但需要更新，則先

7、減去舊 Factor，再加上新 Factor。3.二、圖解 Hes矩陣2.1 xyz 參數(shù)化的視覺約束當(dāng)路標(biāo)點(diǎn)用 xyz 這種參數(shù)化形式時，圖 1 給出了一個實(shí)例，其中綠色的圓圈為相機(jī)位姿61 = |，黃色五角星為路標(biāo)點(diǎn)31 = (, , )，黑色線條表示一個相機(jī)對一個路標(biāo)點(diǎn)的觀測，誤差為重投影誤差21，其中 i 為相機(jī) id，j為路標(biāo)點(diǎn) id。2102301圖 1 路標(biāo)點(diǎn)為 xyz 參數(shù)化的例子4根據(jù)上圖Jacobian 矩陣如下所示，其中橫軸為優(yōu)化變量，豎軸為六次視覺觀測，每個綠色小方塊表示一次視覺重投影誤差對相機(jī)位姿的 Jacobian，大小為 26，黃色小方塊表示一次視覺重投影誤差對路

8、標(biāo)點(diǎn)的 Jacobian，大小為 23：12(18+12)=圖 2 Jacobian 矩陣那么，公式(2)可用下圖表示，其中 U、V 為對角陣，W 為相機(jī)與路標(biāo)點(diǎn)的觀測關(guān)系：T0T1T2x0 x1x2x3T0UWT1T2=x0WTx1Vx2x3圖 3 Normal Equation同樣，公式(3)可用下圖表示，其中 S 為共視矩陣：-UWT=V-W5T0T1T2T0T1ST2T0uT1T2x0vx1x2x3T0CT1T2x0Xx1x2x3T0T1T2x0 x1x2x3r00r01r11JCJr12Xr13r23=-WV-圖 4 Marginalization2.2 逆深度參數(shù)化的視覺約束因 I

9、CE-BA 中采用逆深度參數(shù)化，因此下面將詳細(xì)進(jìn)行說明。每一次視覺觀測都將與三個物理量有關(guān)：路標(biāo)點(diǎn)的逆深度，第一次觀測到該路標(biāo)點(diǎn)的相機(jī)（ICE-BA 中稱之為 source camera（cx），和當(dāng)前觀測到該路標(biāo)點(diǎn)的相機(jī)（measurementcamera（cz）。如下圖所示，灰線表示第一次觀測到該路標(biāo)點(diǎn)的相機(jī)與路標(biāo)點(diǎn)的關(guān)系；橙線表示一次觀測，其中 i 為當(dāng)前觀測到該路標(biāo)點(diǎn)的相機(jī) cz 的 id，j 為路標(biāo)點(diǎn) id，下面都將以下圖為例進(jìn)行說明。210rv10rv22rv23rv112301圖 5逆深度參數(shù)化例子那么 Jacobian 可用下圖表示，注意與上一個例子中的區(qū)別是每次視覺殘差都與兩

10、個相機(jī)和一個路標(biāo)點(diǎn)相關(guān)，其中黃色小方塊所表示的 Jacobian 參數(shù)塊尺寸為 21：圖 6 逆深度參數(shù)下的 Jacobian 矩陣6T0T1T2x0 x1x2x3rv10rv11rv22rv23vT0uT1T2T0gT1T2可計(jì)算得到 Hes如下所示（2.4 部分中將詳細(xì)介紹下圖的數(shù)學(xué)求解過程）：圖 7 逆深度參數(shù)下的 Hes矩陣2.3 視覺和 IMU 聯(lián)合約束VIO 中除了視覺約束還需要考慮IMU 約束，IMU 引入了相鄰相機(jī)間的位置、旋轉(zhuǎn)、速度、Bias 的約束。那么，對于如下圖中藍(lán)色線所示：在圖 5 的基礎(chǔ)上添加兩個 IMU 約束，201rb01rb12rv10rv22rv23rv11

11、2301圖 8 VIO 中的 IMU 約束上圖中 IMU 約束的 Jacobian 如下所示，其中藍(lán)色小方塊為 IMU 殘差關(guān)于相機(jī)位姿的 Jacobian，大小為 156；紅色小方塊為 IMU 殘差關(guān)于相機(jī)運(yùn)動的Jacobian，大小為 159：圖 9 IMU 約束的 Jacobian 矩陣將 IMU 約束和視覺約束合并后的 Jacobian 為：7T0M0T1M1T2M2x0 x1x2x3rb01rb12T0T1T2x0 x1x2x3T0T1T2x0 x1x2x3圖 10 VIO 的視覺和 IMU 共同約束下的 Jacobian 矩陣那么，計(jì)算出公式（1）中的 Hes和 b 矩陣如下圖所示

12、，其中綠色小塊同時受視覺和 IMU 共同約束影響，黃色小塊僅受 IMU 約束影響，紅色和藍(lán)色小塊僅受視覺約束影響，下面對下圖進(jìn)行詳細(xì)的數(shù)學(xué)說明。圖 11 VIO 的視覺和 IMU 共同約束下的 Hes和 b 矩陣2.4 H|b矩陣的數(shù)學(xué)形式，暫且忽略目標(biāo)函數(shù)中的先驗(yàn)和 marg 約束，并且為了文章為了簡化簡潔，H|b矩陣的詳細(xì)數(shù)學(xué)形式放到附錄 6.2 中。在下面分析增量式 BA過程中，以某一次新的觀測，來分析對這里數(shù)學(xué)形式的影響。但是，分析附錄的數(shù)學(xué)形式會發(fā)現(xiàn)，對于這個僅有 3 個相機(jī) 2 個路標(biāo)點(diǎn)的系統(tǒng)來說，數(shù)學(xué)形式就異常復(fù)雜，不但要分析上面的H|b矩陣中哪一項(xiàng)有值，而且要分析每一項(xiàng)都受哪些

13、約束共同影響，比如11 就有六項(xiàng)約束共同影響： 10 10 11 11 22 22 23= () () + () () + () () + () 23()1111111111 01 01 12+ () () + () 12()1111可以發(fā)現(xiàn)，通過上面的分析方法很難清晰地分析出來，下面給出一種清晰的思路因子圖 Factraph。8T0M0T1M1T2M2x0 x1x2x3bT0M0T1M1T2M2x0 x1x2x3T0M0T1M1T2M2x0 x1x2x3rv10rv11rv22rv23rb01rb12三、因子圖分析法 Factraph因子圖由兩種節(jié)點(diǎn)組成：變量節(jié)點(diǎn)和因子節(jié)點(diǎn)。其中，變量節(jié)點(diǎn)是

14、指所有的優(yōu)化變量（用圓圈表示），因子節(jié)點(diǎn)是指優(yōu)化變量之間的各種約束關(guān)系，即一個因子對應(yīng)一項(xiàng)約束（用方塊表示）。ICE-BA 中的約束包括：視覺約束、IMU 約束、先驗(yàn)約束、相對邊緣化約束。下面重點(diǎn)分析前三種約束，相對邊緣化約束將放在 4.5 中。3.1 觀測因子視覺約束從簡單例子入手，以兩個相機(jī)看到一個路標(biāo)點(diǎn)為例，那么因子圖可表示如下：11T0T11T0T111X0222X0T0T11圖 12 Factraph 表示其中，上圖中的三個圖便輕松地表示了從視覺約束，到 Hes矩陣，再到Schur Complement 的過程，非常簡潔?。ㄟ@里感謝博士帶我進(jìn)入 Factor-Graph 的海洋！），

15、下面詳細(xì)介紹：1. 左圖中一個小方塊表示一個約束（又稱一個因子），這里的紅色因子表示兩個相機(jī)（0和1）共同觀測到一個路標(biāo)點(diǎn)0的視覺約束，其中相機(jī)和路標(biāo)點(diǎn)為優(yōu)化變量；2. 中間圖表示該紅色因子對 H 矩陣的影響：即與小方塊相連的所有變量的兩兩之間，和自己與自己之間，都添加一個邊，邊上的數(shù)字表示有幾個約束會同時影響 H 矩陣中該處的值，其中一個紅色視覺因子，會對 H 矩陣產(chǎn)生六處影響，對應(yīng)到 H 矩陣中如下所示，其中下圖中 H 矩陣對角線上的項(xiàng)對應(yīng)到因子圖中每個變量自身和自身的那個圓圈邊，而非對角線的項(xiàng)對應(yīng)到因子圖中兩兩之間的邊。9圖 13 與上面因子圖對應(yīng)的 Hes矩陣3. 若建立 Schur

16、Complement，只需要將圖 12 中的路標(biāo)點(diǎn)0作為一個約束去掉即可，那么0會對與它相連的變量添加上兩兩約束及自身和自身的約束，如右圖所示。3.2 運(yùn)動因子IMU 約束下面給出 IMU 約束對應(yīng)的因子圖：M1M2M2M1T1T2T1T211圖 14運(yùn)動因子上圖中藍(lán)色小塊為 IMU 預(yù)積分引入的約束項(xiàng)，此項(xiàng)因子對 Hes矩陣的影響如右圖所示。因子圖與 Hes矩陣的對應(yīng)關(guān)系如下：10T0T1x0T0T1x0圖 15 運(yùn)動因子對應(yīng)的 Hes矩陣3.3 VIO 約束的因子圖表示對圖 8可以用因子圖的形式來表示，其中一個小方塊表示一個約束，包括 2 個藍(lán)色的 IMU 約束和 4 個紅色的視覺約束；與

17、小方塊連接的線是表示與該約束相關(guān)的優(yōu)化變量；當(dāng)去掉一個小方塊時，需要在所有連接的優(yōu)化變量上和兩兩之間上都添加一個邊，這個邊表示該約束在整體 Hes矩陣中的作用，會在 4.1 和 4.2 節(jié)的視覺和 IMU 約束中詳細(xì)說明。M0M1M2rb01rb12T0T1T2rv11rv22rv10rv23X0X1X2X3圖 16 Graph 表示的 VIO 約束關(guān)系圖3.4 先驗(yàn)因子先驗(yàn)約束比較簡單，比如固定住第一幀的位姿：位置為 0，朝向?yàn)閷R重力方向，那么就引入了對第一幀位姿的先驗(yàn)約束。如下圖所示，此例中的先驗(yàn)因子為單元因子，即只對一個優(yōu)化變量有影響：11T1M1T2M2T1M1T2M2圖 17 先驗(yàn)

18、因子四、ICE-BA 后端非線性優(yōu)化的數(shù)學(xué)說明4.1 狀態(tài)向量為了深入理解圖 11 的大矩陣H|b，同時也為了便于理解接下來的增量式 BA思路，這里有必要對其中的 Hes矩陣的數(shù)學(xué)求解過程進(jìn)行詳細(xì)說明，首先我們從 ICE-BA 的狀態(tài)向量，即優(yōu)化的物理量進(jìn)行說明：狀態(tài)向量共包括滑動窗口內(nèi)的 n 個所有相機(jī)的狀態(tài)（包括位置、朝向、速度、加速度計(jì) bias 和陀螺儀 bias）和 m 個 3D 點(diǎn)的逆深度，尺寸為 15n+m： = 0, 1, 1, 0, 1, 1(5) = (, ) = , , , , 其中，表示第 k 幀相機(jī)狀態(tài)，表示位置和朝向，表示速度和 bias，下面對1中的坐標(biāo)系進(jìn)行說明

19、： = 表示從世界系到第幀相機(jī)系的變換 = 表示從世界系到第 k 幀相機(jī)系的旋轉(zhuǎn) = 表示從第 k 幀相機(jī)系到世界系的平移，即第 k 幀相機(jī)在世界系中的坐標(biāo) = 表示第 k 幀相機(jī)在世界系中的速度其中，從世界系到第 k 幀相機(jī)系的平移，可計(jì)算得：(6)w = 那么，對于世界系中的任意點(diǎn)，它在第 k 幀相機(jī)系下的坐標(biāo)為：(7)= = 為了避免，下文均采用等式右邊的改寫方式。12T0T1x0T0T1x0另外，上面提到的滑動窗口值得解釋下：如下圖所示，在 LBA 中，滑動窗口是指時間上最鄰近的多幀圖像，如藍(lán)色虛框中的 7 個 Frame，LBA 中的 Frame 不一定是關(guān)鍵幀（KF），而是按時序從

20、前端依次進(jìn)來的幀，并且通常為均勻分布。但 GBA 中包含了所有的 KF，而優(yōu)化后的 KF 位姿會更新給 LBA；LBA 中也含有 GBA 中所有的 KF，但是會固定住不變，等待 GBA 為其更新。經(jīng)過 GBA 優(yōu)化后的 KF 可以使得 LBA 更準(zhǔn)確地求解位姿。值得說明的是，LBA 中會將 KF 復(fù)制一份作為普通的 Frame 放到滑窗中進(jìn)行優(yōu)化（比如 LBA 中的 h7、h9、h10 和 h12對應(yīng) GBA 的 k5、k6、k7、k8），但是因?yàn)樯蟮钠?。品?KF 一模一樣，因此位姿不會發(fā)圖 18 LBA 和 GBA4.2 目標(biāo)函數(shù)1給出了 LBA 的整體目標(biāo)函數(shù)，包括視覺約束、IMU

21、約束和先驗(yàn)約束。+ (argmin ( , , )(8)+ ( , )0,+1+100,+1GBA 在上面基礎(chǔ)上多了一項(xiàng) marg 約束，如下所示：+ (argmin ( , , )+ ( , )0,+1+100,+1+( )(9)將上式整理成 Norma Equation 形式：13111( + + ,+1,+1,+1+ 1) 000111= + + +,+1,+1,+1 1(10)000簡寫為：( + + + ) = + + + (11)其中，每一項(xiàng) H 的尺寸均為所有優(yōu)化變量的尺寸(15n+m)(15n+m)，每一項(xiàng) b 的尺寸均為(15n+m)1。注意在 GBA 中更新時，各項(xiàng)約束的影

22、響不一定存放在哪個 H 矩陣，畢竟可能有多個約束會共同影響一個地方的值，比如 IMU 約束除了會放在中，也有可能存放在中。4.3 視覺約束1）殘差：視覺殘差是重投影誤差，對于第 j 個路標(biāo)點(diǎn) P，將 P 從第一次到它的相機(jī)（設(shè)為第幀）坐標(biāo)系下的歸一化相機(jī)系中的值 ，轉(zhuǎn)換到當(dāng)前的第 i 個相機(jī)坐標(biāo)系下的像素坐標(biāo) ，與第 i 個相機(jī)實(shí)際看到的像素坐標(biāo) 相減，定義為視覺誤差項(xiàng)為（對應(yīng)1中的公式（1）：1) ( , , ) = (, 1(10), ) = ( 2）優(yōu)化變量 , ， , ，4.4 IMU 約束1）殘差：兩幀之間的PVQ 和 bias 的變化量的差： (, ) =(11),14 ( ( )

23、 ( ) ( + ( )ij= 1 ) + ( ) (2(2 j i 2）優(yōu)化變量 , ， , , ， , ， , , 4.5 Marginalization 約束當(dāng) LBA 滑窗中 Frame 數(shù)量大于閾值時，需要將滑窗中最老幀給邊緣化掉，1中的相對邊緣化（Relative Marginalization）。ICE-BA 的相對邊緣化是在即參考坐標(biāo)系下進(jìn)行的，參考坐標(biāo)系是與最老幀共視中最近的 KF，這里的最近可以是時間上最近，也可以是距離上最近。marg 后會對 GBA 中跟最老幀有共視關(guān)系的所有 KF 都產(chǎn)生一個線性約束。這樣的話，GBA 可以根據(jù)這些 marg 約束對KF 的位姿進(jìn)行更準(zhǔn)

24、確地優(yōu)化，從而將優(yōu)化的 KF 位姿更新給 LBA，進(jìn)而可以有LBA 不斷的 marg 造成的誤差累積。效1）marg 視覺約束后產(chǎn)生的先驗(yàn)按照1的第 5 部分，相對邊緣化約束的參考坐標(biāo)系不再是前面的世界系w，而是與當(dāng)前幀共視且距離最近的一個 KF 的相機(jī)系（設(shè)為第0幀）。那么，公式（10）的視覺約束可改寫為：1) (1(12), , ) = (000 0 其中，為 GBA 中與當(dāng)前幀共視的 source keyframe。2）marg 掉 IMU 約束后產(chǎn)生的先驗(yàn)根據(jù)上一步中的參考系從世界系變成距離當(dāng)前幀最近的一個 KF 的相機(jī)系（設(shè)為第0幀）。那么，公式（11）的 IMU 約束可改寫為：15

25、 0(13)(, , ) =, ( ( ) 0 000 (0 ) ( ij + ( )= 1 2 + ( ) 20( ) (000 ji 其中上式中第三項(xiàng)證明可查看附錄 6.1。3）舉例說明 marg 過程（下面僅引入了視覺約束，沒看到 IMU 約束在哪？）為了直觀理解，下面舉例來說明 marg 的影響。其中，紫色虛框?yàn)?GBA，藍(lán)色虛框?yàn)?LBA。LBA 的最老幀1 需要被邊緣化掉，1 、2 、3 是1 的 sourceframe，3 是距離1 最近的 KF，并作為 marg 過程中的參考幀。圖 19 marg 的例子將上面例子畫出因子圖形式如下，并去掉因子，轉(zhuǎn)成對 H 矩陣的影響圖：161

26、X0X0111Tk1113Th1Tk11Tk21X11Tk2Th1111X11Tk31Tk3X2X21圖 20 marg 的因子圖表示為了形象說明，上面過程對應(yīng)到 J 和 H 矩陣可如下所示，可見 H 矩陣的每一項(xiàng)都可與因子圖中的每個邊一一對應(yīng)：圖 21 Jacobian 和 Hes矩陣當(dāng) marg 掉1 時，會對 GBA 中的1 、2 、3 產(chǎn)生視覺先驗(yàn)約束，即將上圖中1 邊緣化掉，下圖中左圖的因子圖，其中邊上標(biāo)注“1”的為 marg 所引入的約束；若繼續(xù) marg 掉路標(biāo)點(diǎn)，則到右圖的因子圖，右圖對應(yīng)了 S 矩陣。17Tk1Tk2Tk3Tk4Th1Tk5X0X1X2rv10rv11rv12

27、Tk1Tk2Tk3Tk4Th1Tk5X0X1X2bTk1Tk2Tk3Tk4Th1Tk5X0X1X2Tk24Tk3Tk32圖 22 marg 掉最老幀后的因子圖（左），和繼續(xù) marg 掉路標(biāo)點(diǎn)的因子圖（右）對應(yīng)到 H 矩陣的圖形上，可將H|b矩陣中與h1 有關(guān)的移到一邊，用 Schur 補(bǔ)進(jìn)行 marg，那么邊緣化后的新H|b矩陣如下圖所示：圖 23 將h1 移動到一邊的H|b（左圖）marg 后的H|b（中圖），及 marg 掉路標(biāo)點(diǎn)的 S 矩陣（右圖）對比 marg 前后的矩陣可以發(fā)現(xiàn)，marg 掉h1 會導(dǎo)致 Hes稀疏性被破壞，產(chǎn)生了許多新的非零塊，增加的束可以用下圖中的紅線來表示：T

28、k1Tk2Tk3Tk4Tk1Tk2k3Tk4Tk1Tk2Tk3Tk5Tk1X0Tk2X1Tk3X2Tk1Tk2Tk3Tk4Tk5X0Tk1Tk2Tk3Tk4Tk5X0X1X2Th1圖 24 marg 產(chǎn)生的束五、ICE-BA 中的一些變量下面對 GBA 代碼進(jìn)行Unitary：對角線的數(shù)據(jù) Binary: 非對角線的數(shù)據(jù)，首先對 ICE-BA 中的幾個變量進(jìn)行說明：X: sourceZ: measurementiKF2X：matched KF to source feature indexiKF2di：截止到第 i 個 KF 之前的所有 KF 一生的 mappo個數(shù)5.1 m_Zs 和 m_z

29、s假設(shè)當(dāng)前有 10 個 MapPos，id=0,1,2,.,8,9，其中前 3 個 0,1,2 來自于 KF0，在 KF0 中的 m_ix 分別為 m_ix=0,1,2；后 7 個 3,4,.,9 來自 KF1，在 KF1 中的 m_ix分別為 m_ix=0,1,.6，如下圖所示：19KF0 m_ixKF1 m_ixMapPos idm_iz某幀觀測到的點(diǎn)01234m_Zs0m_Zs1圖 25 m_Zs 和 m_zs 的關(guān)系某一幀看到 5 個 feature measurement，對應(yīng)到 MapPos 的 id=1,2,5,7,9，那么前 2 個 feature measurement 來自

30、于 KF0，在 KF0 中的 m_ix=1,2；后 3 個 featuremeasurement 來自于 KF1，在 KF1 中的 m_ix=2,4,6。m_iz1 和 m_iz2 表示當(dāng)前觀測幀看到的點(diǎn)的 id，m_zs表示當(dāng)前幀觀測到的所有路標(biāo)點(diǎn)，m_ix 表示該路標(biāo)點(diǎn)在第一次觀測的 source KF 中的序號。m_Zs0.m_iKF,m_iz1,m_iz2=0,0,2m_Zs1.m_iKF,m_iz1,m_iz2=1,2,5m_zs0.m_ix=1m_zs2.m_ix=2m_zs1.m_ix=2m_zs3.m_ix=4m_zs4.m_ix=6六、GBA 代碼GBA 線程主函數(shù)為 Glo

31、balBundleAdjustor:Run()，流程如下：2097521圖 26 GBA 流程圖其中，UpdateFactors 對應(yīng) BA 求解中的建立增量方程，得到H|b矩陣，具體實(shí)現(xiàn)在 GlobalBundleAdjustN 中的 UpdateFactors()，主要包含更新視覺和 IMU約束、更新先驗(yàn)和固定一些優(yōu)化變量；plement 是計(jì)算 Schurplement 是計(jì)算相機(jī)位姿及Motion 等；Complement 中的S|g矩陣SolveBackSubstitution 是求解路標(biāo)點(diǎn)。下面對各個函數(shù)進(jìn)行分別說明：6.1 更新視覺約束 UpdateFactorsFeature對

32、應(yīng)代碼為 GlobalBundleAdjustN:UpdateFactorsFeature()，為了便于形象假設(shè)圖 14 中相機(jī) T2 為新來的，這時它帶來了兩個新理解，這里舉例說明，的視覺觀測22 、23 和一個新的 IMU 約束12 ，這里以22 這次視覺觀測為例進(jìn)行分析，22 涉及 T2、T1 和 X2，其中 T1 為 source frame cx，T2 為 measurementframe cz。再回頭分析 2.4 節(jié)中的 Hes矩陣和 b 矩陣的數(shù)學(xué)形式，可以發(fā)現(xiàn)，22 會對圖 11 中的如下參數(shù)塊有影響，其中紅色為22 的影響： 10 10 11 11 22 22 23= ()

33、() + () () + () () + () 23()1111111111 01 01 12+ () () + () 12()1111 22 22 23 23 12= () () + () () + () 12()2121212121SolveBacSolveSch 22= () 22()121222222323121222= () () + () () + () ()(6)222222 22= () 22()2222 22= () 22()2222 10 11= () + () 22+ () 23+ () + ( 01 ) 1101122230111111 12+ () 121 22= (

34、) + ( 23 ) 12+ () 2222312222 22= () 2222其中，22 對 H 矩陣的 6 項(xiàng)約束，下面用因子圖的方式進(jìn)行說明：這里只考慮22 的影響，可以發(fā)現(xiàn)會對六個邊有影響，并且發(fā)現(xiàn)跟上面的數(shù)學(xué)形式一一對應(yīng)！這里忽略了其他的小塊，其中邊上的數(shù)字表示有幾個約束對這個邊有影響，這里只考慮22 的 1 個約束。M0M1M2M0M1M2rb01rb12111T0T1T2T0T1T2rv10rv11rv22rv23X2X0X1X3X0X1X2X3圖 27 22 對 H 矩陣的影響（左邊為帶小塊的形式，右邊為去掉22 小塊的影響）上述九項(xiàng)在圖 11 H|b矩陣的影響如下圖所示，其中

35、的 9 個彩色塊即為對應(yīng)的影響，對應(yīng)到代碼中，大矩陣H|b中的綠色參數(shù)塊存放在 m_SAcus 數(shù)組中，表示 U 對角線上跟相機(jī)位姿相關(guān)的塊及對應(yīng)的 b，通過上面分析已經(jīng)知道這些參數(shù)將同時受到視覺和IMU 的影響；藍(lán)色塊存放在觀測幀T2 的m_SAcxzs 數(shù)組中，表示 U 中觀測幀 T2 所看到的所有視覺觀測；橙色塊存放在原始幀 T1 的 m_Axs22數(shù)組中的 m_Sadx 中，表示該原始幀 T1 負(fù)責(zé)對于路標(biāo)點(diǎn) X2 的可能被除 T2 外的其他觀測幀看到，也是由該路標(biāo)點(diǎn)的原始幀 T1 來，比如 X2 有。紅色塊存放在觀測幀 T2 的 m_Azs1 數(shù)組中的 m_adcz 中，該參數(shù)塊只與

36、22 有關(guān)，不會受到其他觀測的影響。SAcxzSAcxxT0M0T1M1T2M2x0 x1x2x3bT0M0SAczzWUT1M1mSadxT2M2x0(Cz)KF.m_SAcxzsTVWx1m_SAcusx2x3(Cx)_KF.m_Axs.m_Sadx(Cz)KF.m_Azs1.m_adcz圖 28當(dāng)圖 9 中新來一幀 T2 時，新的視覺觀測22 對H|b的 9 處影響下圖形象給出了一個視覺約束因子對于H|b矩陣的影響及對應(yīng)關(guān)系：T2.m_SAcxzsm_SAcus1m_SAcus2T2.m_Azs1.m_adczT1.m_Axs.m_Sadx圖 29 因子圖與H|b的對應(yīng)關(guān)系值得注意的是，

37、22 對于大矩陣H|b的影響存放在觀測幀的 m_Azs223T2和T0M0T1M1T2M2x0 x1x2x3bT0M0T1M1T2M2x0 x1x2x3m_Azs1.m_adcz 中，并通過 FTR:GetFactor 函數(shù)計(jì)算公式（6）中的紅色部分。為了與 GetFactor 中的 Factor:Full:U-m_A（尺寸為 1314）一致，中的彩色塊單獨(dú)拿出來，并將路標(biāo)點(diǎn)放到左上角，則如下圖所示：圖 2461 m_Azs2. m_adx.m_adc61 m_Azs1.m_adcz11 m_Azs2.m_adx.m_add.m_b11 m_Azs2.m_adx.madd.m_am_Azs2.

38、m_Acxx.m_bm_Azs2.m_Aczz.m_b圖 30 GetFactor 中的 U-m_A，尺寸為 13146.2 更新 IMU 約束 UpdateFactorsIMU對應(yīng)代碼為：GlobalBundleAdjustN:UpdateFactorsIMU()，同樣的，對于圖23 來說，考慮新來的 T2 帶來了一個新的 IMU 約束12 ，那么12 涉及 T1、M1 和 T2、M2。同樣的，12 對附錄 H 中的 14 處影響如下藍(lán)色所示： 10 10 11 11 22 22 23= () () + () () + () () + () 23()1111111111 01 01 12+

39、() () + () 12()1111 01 01 12= () () + ()( 12 )111111 22 22 23 23 12= () () + () () + () 12()21212121 12= () 12()1212 01 01 12= () () + () 12()111111 12= () 12()121224x2T1T2bx2T1m_Azs2. m_Acxx.m_Am_Azs2. m_AcxzT2m_Azs2. m_Aczz.m_A 12= ()( 12 )1212 22 22 23 23 12= () () + () () + () 12()22222222 12= (

40、) 12()2222 12= ()( 12 )2222 10= () + ( 11 ) 22+ () 23+ () + ( 01 ) 1101122230111111 12+ () 121 01= () 12+ ()1101121 22 23 12= () + () + () 2222312222 12= () 212212 對 H 矩陣的 10 處影響用 Factraph 的形式可如下表示：M1M2M022T0T1T211X2X0X1X31圖 3112 對 H 矩陣的影響12 對大矩陣H|b的 14 處影響可以用下圖的彩色塊表示，灰色塊是不受影響的。其中綠色塊為跟上面視覺約束相同的 m_SA

41、cus，這里不再類述；黃色塊為跟 IMU 的 V、bias 有關(guān)的參數(shù)塊，數(shù)據(jù)存放在 m_SAcmsLM 數(shù)組中的 m_Au 中（包括 m_Amm 和 m_Acm），其中綠色塊和黃色塊都是可能受到其他約束影響，比如黃色塊還會受到01 的影響；紅色塊存放在 m_SAcmsLMim2.m_Ab 中，表示2512 對兩幀之間引入的約束影響，值得注意的是，紅色塊不會受其他 IMU 約束的影響，其中，12 也會受到上面視覺約束影響，但這里只保存 IMU 約束對這個參數(shù)塊的影響，這個變量是在 m_DsLMim2.GetFactor 中計(jì)算的。m_SAcus1T0M0T1M1T2M2x0 x1x2x3bT0

42、M0m_SAcus2T1WUM1T2M2Tx0VWm_SAcmsLM2.m_Abx1x2m_SAcusx3m_SAcmsLM.m_Aum_SAcmsLM2.m_Aum_SAcmsLM1.m_Au圖 32當(dāng)新來一幀 T2 時，新的視覺觀測12 對增量方程的 14 處影響下圖形象給出了一個 IMU 約束因子對H|b矩陣的影響及對應(yīng)關(guān)系：m_SAcmsLM1.m_Aum_SAcmsLM2.m_AbT0M0T1M1T2M2x0 x1x2x3bT0M0m_SAcmsLM2.m_AuT1M1T2m_SAcus2M2x0 x1x2x3m_SAcus1m_SAcmsLM2.m_Ab圖 33 IMU 約束因子對

43、H|b矩陣的影響12 對H|b的影響是對應(yīng)在 m_AdsLMim2和 m_SAcmsLMim2.m_Ab 這兩個變量，可以用下圖來表示12 的影響，對應(yīng)到代碼 IMU:GetFactor()中。其中，26黃色塊為 m_AdsLMim2 -m_A11，表示12 對前一個相機(jī)狀態(tài)的影響；綠色塊為 m_AdsLMim2 -m_A22，表示12 對后一個相機(jī)狀態(tài)的影響；紅色為 m_SAcmsLMim2.m_Ab ，表示 12 對兩個 相機(jī)之間的影響 。 代碼中 IMU:Delta:Factor:Auxiliary:Global U 的 m_A55，是對應(yīng)到下圖中除 b 列之外的所有彩色塊，共 55 個

44、小塊，每個小塊尺寸為 33；m_b10是對應(yīng)最后一列的 10 個彩色塊，即 b 列，每個小塊的尺寸也是 33。m_AdsLMim2-m_A11m_SAcmsLMim2.m_Abm_AdsLMim2-m_A22圖 34 12 對于大矩陣H|b的 14 處新影響6.3 更新位姿先驗(yàn) UpdateFactorsPriorCamerae對應(yīng)代碼為 GlobalBundleAdjustor:UpdateFactorsPriorCamerae()。根據(jù)上面4.5 節(jié)分析的 Relative Marg，當(dāng) marg 掉最老幀時，會對 GBA 中與最老幀共視的KF 產(chǎn)生視覺約束，直接用上面的例子對該函數(shù)進(jìn)行說

45、明。首先，重新觀察 4.5 節(jié)的例子中的最后因子圖和 S 矩陣，并令 Tk2 為參考關(guān)鍵幀：27T1M1T2M2bp1r1v1ba1bw1p2r2v2ba2bw2T1p1r1M1v1ba1bw1T2p2r2M2v2ba2bw2m_SAcus1Tk2.m_SAps1Tk3.m_SAps1m_SAcusm_SAcus2m_SApsTk3.m_SAps2m_SAcus3圖 35 marg 引入視覺先驗(yàn)的例子該函數(shù)中，將影響H|b矩陣中對角線的值依舊存放在 m_SAcus 數(shù)組中；將影響S|g矩陣中非對角線的值存放在相應(yīng) KF 的 m_SAps 數(shù)組中。其中，某一次邊緣化對整體S|g的影響都臨時存放在

46、 A = m_ApsiZ這個數(shù)組中，并通過 Z.GetFactor()來計(jì)算 A。A 的數(shù)據(jù)結(jié)構(gòu)如下所示：共包含(N+1) (N+1)個小格，每個小格的尺寸為 66，第 0 行存放著參考關(guān)鍵幀 Tk2 跟其他共視幀 Tk1 和 Tk3 之間的約束關(guān)系，第 1N 行存放著除參考 KF 之外的其他共視幀兩兩間的約束關(guān)系。圖 36 A = m_ApsiZ的數(shù)據(jù)結(jié)構(gòu)6.4 更新運(yùn)動先驗(yàn) UpdateFactorsPriorCameraMotion對應(yīng)代碼為GlobalBundleAdjustor:UpdateFactorsPriorCameraMotion()，其中通過 m_ZpLM.GetFacto

47、r 來計(jì)算運(yùn)動先驗(yàn)，并存放在臨時變量 m_ApLM 中，最后來更新全局變量 m_SAcusm_ZpLM.m_iKF和 m_SAcmsLMim.m_Au。其中，im為參考 KF 的 id，具體有：28m_Am_b012N012NTk1Tk2Tk3Tk1Tk2Tk3im = m_ZpLM.m_iKF - m_Cs.Size() + m_Csize()其中三個變量的意義為：m_ZpLM.m_iKF 為當(dāng)前參考關(guān)鍵幀、m_Cs.Size()為GBA 中 KF 數(shù)量、m_Cs所有的 KF，因此 m_Csize()為 LBA 的 KF 數(shù)量。通常 LBA 中含有 GBA 中ize()=m_Cs.Size(

48、)。m_SAcusm_SAcmsLM.m_Au.m_Acmm_SAcmsLM.m_Au.m_Amm圖 37運(yùn)動先驗(yàn)對于大矩陣H|b的影響說是將 LBA 中（從因子圖上或數(shù)學(xué)意義上，未能解釋該函數(shù)意義，據(jù)對于參考關(guān)鍵幀的 Motion 先驗(yàn)加到了 GBA 中，待理解。另外，為何只跟 R 有關(guān)，與 p 無關(guān)？）6.5 更新路標(biāo)點(diǎn)先驗(yàn) UpdateFactorsPriorDepth對應(yīng)代碼為：UpdateFactorsPriorDepth()，用來對所有路標(biāo)點(diǎn)的逆深度進(jìn)行約束，令每個路標(biāo)點(diǎn)的逆深度在該 source KF 所管轄的所有路標(biāo)點(diǎn)的平均逆深度附近（還未完全對應(yīng)代碼，沒看明白 KF.m_d

49、是什么？），如下圖所示，僅對KF.m_Axsix.m_Sadx.m_add 有影響。X0T01X0T0圖 38 路標(biāo)點(diǎn)的先驗(yàn)及影響29T0M0T1M1T2M2bp0r0v0ba0bw0p1r1v1ba1bw1p2r2v2ba2bw2T0p0r0M0v0ba0bw0T1p1r1M1v1ba1bw1T2p2r2M2v2ba2bw26.6 更新 Schur complement1. 數(shù)學(xué)知識根據(jù) 1.2 節(jié)中建立 Schur Complement：( 1) = ( 1)S = 其中， = 1。那么，對于 S 矩陣中某個位置(i, j)的值為：1()1= = 其中， 表示第 i 個 KF 跟第 j 個

50、 KF 的共視點(diǎn)。舉例說明，假設(shè) T1和 T2 的共視點(diǎn)為 x1 和 x3，那么，2,4可寫成：2,4 = 2,4 (2,11,114,1 + 2,33,314,3 )為了形象，給出圖示：圖 39 Schur Complement 例子2. 因子圖解釋可以把路標(biāo)點(diǎn)當(dāng)作一項(xiàng)約束因子進(jìn)行 marg 掉，那么，用因子圖表示，就會對與路標(biāo)點(diǎn)相連的相機(jī)位姿的兩兩之間和自身與自身之間，添加上新的約束。下圖中，假設(shè)路標(biāo)點(diǎn) X0 的 source KF 為 T0，X1 的 source KF 為 T1，這里忽略了其他的如 IMU 等約束，右圖給出了 marg 掉路標(biāo)點(diǎn)后的添加的束。30T0M0T1M1T2M2

51、x0 x1x2x3T0M0T1M1T2M2x0 x1x2x3T0X0T0T1T1X1T2T2T3T3圖 40因子圖：H矩陣（左圖）、marg 掉路標(biāo)點(diǎn)后的S矩陣（右圖）3. 代碼plement()。到這里已經(jīng)得到了H|b矩陣，對應(yīng)代碼為下面要計(jì)算S|g矩陣。其中H|b矩陣是 GBA 中所有 KF 的位姿和 Motion，與所有路標(biāo)點(diǎn)的相互關(guān)系。代碼中分成三類情況進(jìn)行計(jì)算：1）對所有的 source KF，分析該 KF 所的所有路標(biāo)點(diǎn) m_xs 被 marg 掉后，對該 KF 在S|g對角線上的影響。如下圖所示，X0 和 X1 的 source KF 為 T0，當(dāng) marg 掉 X0 和 X1

52、時，會對 T0 在S|g對角線的值 m_SMcus0產(chǎn)生影響，而對其他處的影響，如 T1、T0 T1 及 T2、T0 T2 則會在下一類情況中更新。m_SMcus0圖 41 marg 路標(biāo)點(diǎn)對 Source KF 在S|g矩陣中的影響2）對所有的 measurement KF，分析該 KF 所觀測到的所有路標(biāo)點(diǎn) m_Zs 被marg 掉后對該 KF 和該 KF 與 source KF 之間的影響，并將該影響存放在該 KF31T0M0T1M1T2M2T0M0T1M1T2M2的 KF.m_Zm.m_SMczmsm_ik，其中 m_ik 為 source KF id。仍以上圖為例，T1 看到了 T0

53、 的 X0 路標(biāo)點(diǎn)，當(dāng) X0 被 marg 掉之后，會對 T1、T0 T1 產(chǎn)生如下影響：T1.m_Zm.m_SMczms0圖 42 marg 路標(biāo)點(diǎn) X0 對 measurement KF T1 在S|g矩陣中的影響3）下圖情況：路標(biāo)點(diǎn) X0 的 source KF 為 T0，T1 和 T2 均能看到 X0 點(diǎn)。當(dāng) Marg掉時，會在和X0T1T2之間添加約束，這個約束對應(yīng)在代碼中什么地方？m_iKFsMatch 意義：幀的 id著當(dāng)該 KF 為 source KF 時，所有跟該 KF 共視的關(guān)鍵T1T1T0X0T0T2T2圖 43 兩個 measurement KF 看到同一個路標(biāo)點(diǎn)的情況

54、6.7 求解相機(jī)位姿得到了 Schur 補(bǔ)方程S = 后，便可以對相機(jī)參數(shù)，即 PVQ 和 bias 的增量進(jìn)行求解，代碼對應(yīng)為：GlobalBundleAdjusBA 中采用 PCG 進(jìn)行求解，下面對 PCG 做簡單介紹：1. PCG（預(yù)處理的共軛梯度法）plement()。ICE-共軛梯度法（Conjugate gradient，CG）是一種求解大型、稀疏、對稱、正定32T0M0T1M1T2M2T0M0T1M1T2M2線性方程組 Ax=b 的迭代方法，其中，A 為 nn 的對稱正定矩陣，b 為 n1 的列向量。該方程即對應(yīng)著上面的 Schur Complement 方程S = 。共軛梯度法是介于梯度下降法與法之間的一個方法，是一個一階方法，它克服了梯度下降法收斂慢的缺點(diǎn)，又避免了和計(jì)算法所需要的二階導(dǎo)數(shù)信息。共軛梯度法的就是找到 n 個兩兩共軛的共軛方向，每次沿著一個方向優(yōu)化得到該方向上的極小值，后面再沿其它方向求極小值的時候，不會影響前面已經(jīng)得到的沿哪些方向上的極小值，所以理論上對 n 個方向都求出極小值就得到了 n 維問題的極小值。在 n 維的優(yōu)化問題軛梯度法最多 n 次迭代就能找到最優(yōu)解（是找到，不是接近）