重讀《視覺SLAM十四講》ch10 後端2

前言

本章關於滑動視窗部分比較難懂，我覺得可能是因為篇幅所限，而在後面ch13中也取了最簡單的滑動視窗形式。因此我決定在本文中只留下簡單易懂的部分，這樣不影響後面內容的學習，書中複雜的延伸內容應該在其他地方更加系統的學習。

一、滑動視窗濾波和最佳化

1。1 實際環境下我的BA結構

隨著路標點和幀（每幀都有一個相機位姿）的增多，後端最佳化（BA）的問題規模越來越大，如果不對BA規模進行控制，算力逐漸吃緊，無法保證SLAM實時性。

控制計算規模的做法很多，如選取關鍵幀，非關鍵幀僅用於定位（估計幀間位姿變換），關鍵幀用於定位和建圖。但是這樣只能使BA規模增長緩慢一些，無法真正做到控制BA規模在一定範圍內。

還有一種簡單思路是隻保留離當前時刻最近的N個關鍵幀，將更早的關鍵幀丟棄，稱為“滑動視窗法”。取這N個關鍵幀的策略不應該只看時間，如果只看時間的話，在相機靜止不動一段時間後，BA的結構就縮成一團了，因此既要考慮時間上的靠近，也要考慮空間上可以展開，在ORB-SLAM2中，還要考慮“共視”，共檢視是指與當前相機關鍵幀存在共同觀測的關鍵幀構成的圖，如僅最佳化與當前幀有20個以上的共視路標點的關鍵幀，其餘固定不變，不做最佳化。

下面討論：丟棄具體是指如何丟棄？被丟棄的幀是否會對保留幀的BA產生影響？

1。2 滑動視窗法

假設滑動視窗內的N個關鍵幀的位姿為：

。

在ch9中我們討論過，因為含有噪聲，我們將關鍵幀位姿看作是一種高斯分佈，均值就是最有可能的位姿值，方差就是它的不確定度：

括號內是我的理解：（採用基於非線性最佳化（BA）的後端時，均值就是BA的結果，∑就是H矩陣邊緣化的結果（S矩陣），因為S矩陣僅僅與

有關，顯然S矩陣越病態，

越不穩定，進而

的不確定度越高）

滑動視窗發生改變時，這些狀態變數應該如何變化？分為兩部分討論：

新增一個關鍵幀，以及它觀測到的路標點

刪除一箇舊的關鍵幀，可能刪除它觀測到的路標點。

如果是傳統的BA，這是兩個結構不同、互相獨立的BA問題，但是在滑動視窗中，這是一個問題，需要討論它具體的細節。

新增一個關鍵幀和路標點

是平凡的，不必討論。

刪除一箇舊的關鍵幀

如果我們只刪除一個關鍵幀（位姿）而不刪除它對路標點的觀測，這將會破壞H矩陣的稀疏結構，導致無法邊緣化加速求解。

括號內是我的理解：（注意到對於關鍵幀x1看到的路標點變數y1，y2，y3，y4，刪除了某個關鍵幀之後，明明沒有關鍵幀觀測到它們，但是BA中仍然保留了這些對路標點的觀測，這就產生了路標點的“先驗資訊”，破壞了原來的H的稀疏結構）

（我認為邊緣化就是將某個未知量透過矩陣恆等變換的方式，令其暫時不參與計算，將其餘的未知量求出後，被邊緣化的量可以自然得到，因此將它暫時理解為刪除也未嘗不可）

因此，邊緣化某個舊關鍵幀的同時，同時邊緣化它觀測到的路標點（假裝所有關鍵幀都沒看到或提取到這個關鍵點），可以保持H矩陣的稀疏結構。

在某些SLAM框架中，會採用更復雜的策略，在OKVIS中，需要判斷最新的關鍵幀中是否仍能看到這些舊的關鍵幀觀測路標點，如果能，就邊緣化舊關鍵幀對這個路標點觀測（假裝這個幀從未被當做關鍵幀）；如果不能，就直接邊緣化這個路標點（假裝從未觀察到或提取到這個路標點）。（我覺得OKVIS這樣做是認為：如果新關鍵幀沒有觀測到這個路標點，很有可能在之後的一段時間內也觀測不到，因此直接拋棄這個路標點，否則仍保留它。）

SWF(Sliding Window Filter)中邊緣化的直觀解釋

邊緣化在機率中的意義是條件機率，邊緣化某個關鍵幀的含義就是：保持這個關鍵幀當前估計值，求其他狀態變數以這個關鍵幀為條件的條件機率。因此當某個關鍵幀被邊緣化，它所觀測的路標點就會產生一個“這些路標應該在哪裡”的先驗資訊，從而影響其餘部分的估計值，如果再邊緣化這些路標點，那麼它們的觀測者將得到一個“觀測它們的關鍵幀應該在哪裡”的先驗資訊。（這段話與我上一段理解是同一個意思，但是可能比較難懂。）

從數學上來看，邊緣化某個關鍵幀就將整個視窗的狀態變數由聯合分佈變成條件機率分佈：

其中p（x1）捨去了，在工程中不再使用它。（等價於令p（x1）=1，因此我覺得也可以說把它刪去了）

因為總是要把路標點邊緣化（丟棄），所以滑動視窗比較適合VO系統（核心在於定位軌跡），而不適合大規模建圖的系統（核心在於建圖）。

二、位姿圖

2。1 位姿圖的意義

每次後端最佳化中，大部分的都是原有的舊路標點，只有少量新路標點，如此最佳化幾次之後，大部分的路標點的座標已經收斂，沒有必要對它們的座標再進行最佳化。更好的做法是最佳化幾次之後把它們的位置固定住，將它們作為位姿估計約束而不再作為路標點座標的約束。

完全不管路標（直接使用初始估計座標），只最佳化軌跡（相機位姿），這種圖最佳化就是位姿圖。

位姿圖是另一種控制後端最佳化規模的方式，可以用來與測量pose的感測器做融合。

2。2 位姿圖的最佳化

位姿圖的結構：

節點：相機位姿。

邊：兩個位姿節點之間的相對運動估計。（特徵點法，直接法，GPS，IMU積分）

誤差：

ln

（原書這裡還多加了一個

，後面又消失了，我覺得不應該加。）（其中Tij是估計出的相機之間的相對運動，Ti與Tj是真實的位姿，由於存在誤差，

不完全等於1，因而eij不為0）

雅克比矩陣推導：（理清思路：我們求的是誤差關於最佳化變數的導數）

左擾動模型：

交換法則（ch4習題）：

於是有

其中第二行到第三行是exp（X）exp（Y）在0向量處一階Taylor展開。

顯然擾動部分為小量，所以需要用右乘BCH近似：

比較係數，就得到了關於Ti和Tj的導數：

Jr求解困難，在誤差接近於0時，可以把它近似成單位陣I或：

實際上誤差不一定很接近0，因此將Jr近似為單位陣I會有一定的損失。

代價函式：（∑為資訊矩陣）

知道了節點、邊、雅克比矩陣、誤差、代價函式就用g2o求解了。

三、實踐：位姿圖最佳化

3。1 g2o原生位姿圖

g2o原生位姿圖是一個球體，我們可以認為每一層的圓環上的邊是相機軌跡的約束（里程計邊），而層與層之間的邊是迴環檢測的約束（迴環邊），相機軌跡的約束保證球體橫切面是圓，而回環檢測的約束保證球體豎切面是圓（而不是變成一個縱向的橢球體）。

g2o預設採用四元數和平移向量表達位姿，6*6對角陣作為資訊矩陣，節點型別為VertexSE3

程式碼比較簡單，不做詳細註釋，只需要注意兩個函式：

。。。

v

->

setFixed

（

true

）

// 將這個點的位置固定，不做最佳化

。。。

// ifstream：：good（） 用於處理檔案讀取過程中的意外情況，正常讀取返回1，否則返回0

if

（

！

fin

。

good

（））

break

；

3。2 李代數上的位姿圖最佳化

用李代數自定義頂點和邊

主程式與用g2o內建頂點和邊大致相同，主要看自定義的頂點和邊：

// 給定誤差求J_R^{-1}的近似

Matrix6d

JRInv

（

const

SE3d

&

e

）

{

Matrix6d

J

；

J

。

block

（

0

，

0

，

3

，

3

）

=

SO3d

：：

hat

（

e

。

so3

（）。

log

（））；

J

。

block

（

0

，

3

，

3

，

3

）

=

SO3d

：：

hat

（

e

。

translation

（））；

J

。

block

（

3

，

0

，

3

，

3

）

=

Matrix3d

：：

Zero

（

3

，

3

）；

J

。

block

（

3

，

3

，

3

，

3

）

=

SO3d

：：

hat

（

e

。

so3

（）。

log

（））；

J

=

J

*

0。5

+

Matrix6d

：：

Identity

（）；

// J = Matrix6d：：Identity（）； // try Identity if you want

return

J

；

}

// 李代數頂點

typedef

Matrix

<

double

，

6

，

1

>

Vector6d

；

class

VertexSE3LieAlgebra

：

public

g2o

：：

BaseVertex

<

6

，

SE3d

>

{

public

：

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

// 覆寫讀取函式，將檔案內容讀作四元數和（平移）向量

virtual

bool

read

（

istream

&

is

）

override

{

double

data

［

7

］；

for

（

int

i

=

0

；

i

<

7

；

i

++

）

is

>>

data

［

i

］；

setEstimate

（

SE3d

（

Quaterniond

（

data

［

6

］，

data

［

3

］，

data

［

4

］，

data

［

5

］），

Vector3d

（

data

［

0

］，

data

［

1

］，

data

［

2

］）

））；

}

// 覆寫寫入函式，將估計值寫作平移向量和四元數，四元數要用coeffs不能直接寫

virtual

bool

write

（

ostream

&

os

）

const

override

{

os

<<

id

（）

<<

“ ”

；

Quaterniond

q

=

_estimate

。

unit_quaternion

（）；

os

<<

_estimate

。

translation

（）。

transpose

（）

<<

“ ”

；

os

<<

q

。

coeffs

（）［

0

］

<<

“ ”

<<

q

。

coeffs

（）［

1

］

<<

“ ”

<<

q

。

coeffs

（）［

2

］

<<

“ ”

<<

q

。

coeffs

（）［

3

］

<<

endl

；

return

true

；

}

// 重置為空李代數

virtual

void

setToOriginImpl

（）

override

{

_estimate

=

SE3d

（）；

}

// 左乘更新

virtual

void

oplusImpl

（

const

double

*

update

）

override

{

Vector6d

upd

；

upd

<<

update

［

0

］，

update

［

1

］，

update

［

2

］，

update

［

3

］，

update

［

4

］，

update

［

5

］；

_estimate

=

SE3d

：：

exp

（

upd

）

*

_estimate

；

}

}；

// 兩個李代數節點之邊

class

EdgeSE3LieAlgebra

：

public

g2o

：：

BaseBinaryEdge

<

6

，

SE3d

，

VertexSE3LieAlgebra

，

VertexSE3LieAlgebra

>

{

public

：

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

// 覆寫讀取函式，不要忘了normalize，另外還要讀取資訊矩陣，資訊矩陣要保證它關於對角線對稱

virtual

bool

read

（

istream

&

is

）

override

{

double

data

［

7

］；

for

（

int

i

=

0

；

i

<

7

；

i

++

）

is

>>

data

［

i

］；

Quaterniond

q

（

data

［

6

］，

data

［

3

］，

data

［

4

］，

data

［

5

］）；

q

。

normalize

（）；

setMeasurement

（

SE3d

（

q

，

Vector3d

（

data

［

0

］，

data

［

1

］，

data

［

2

］）））；

for

（

int

i

=

0

；

i

<

information

（）。

rows

（）

&&

is

。

good

（）；

i

++

）

for

（

int

j

=

i

；

j

<

information

（）。

cols

（）

&&

is

。

good

（）；

j

++

）

{

is

>>

information

（）（

i

，

j

）；

if

（

i

！=

j

）

information

（）（

j

，

i

）

=

information

（）（

i

，

j

）；

}

return

true

；

}

// 寫出邊連線的兩個頂點id，以及邊最終的測量值，資訊矩陣寫為上三角陣

virtual

bool

write

（

ostream

&

os

）

const

override

{

VertexSE3LieAlgebra

*

v1

=

static_cast

<

VertexSE3LieAlgebra

*>

（

_vertices

［

0

］）；

VertexSE3LieAlgebra

*

v2

=

static_cast

<

VertexSE3LieAlgebra

*>

（

_vertices

［

1

］）；

os

<<

v1

->

id

（）

<<

“ ”

<<

v2

->

id

（）

<<

“ ”

；

SE3d

m

=

_measurement

；

Eigen

：：

Quaterniond

q

=

m

。

unit_quaternion

（）；

os

<<

m

。

translation

（）。

transpose

（）

<<

“ ”

；

os

<<

q

。

coeffs

（）［

0

］

<<

“ ”

<<

q

。

coeffs

（）［

1

］

<<

“ ”

<<

q

。

coeffs

（）［

2

］

<<

“ ”

<<

q

。

coeffs

（）［

3

］

<<

“ ”

；

// information matrix

for

（

int

i

=

0

；

i

<

information

（）。

rows

（）；

i

++

）

for

（

int

j

=

i

；

j

<

information

（）。

cols

（）；

j

++

）

{

os

<<

information

（）（

i

，

j

）

<<

“ ”

；

}

os

<<

endl

；

return

true

；

}

// 誤差計算與書中推導一致

virtual

void

computeError

（）

override

{

SE3d

v1

=

（

static_cast

<

VertexSE3LieAlgebra

*>

（

_vertices

［

0

］））

->

estimate

（）；

SE3d

v2

=

（

static_cast

<

VertexSE3LieAlgebra

*>

（

_vertices

［

1

］））

->

estimate

（）；

_error

=

（

_measurement

。

inverse

（）

*

v1

。

inverse

（）

*

v2

）。

log

（）；

}

// 雅可比計算

virtual

void

linearizeOplus

（）

override

{

SE3d

v1

=

（

static_cast

<

VertexSE3LieAlgebra

*>

（

_vertices

［

0

］））

->

estimate

（）；

SE3d

v2

=

（

static_cast

<

VertexSE3LieAlgebra

*>

（

_vertices

［

1

］））

->

estimate

（）；

Matrix6d

J

=

JRInv

（

SE3d

：：

exp

（

_error

））；

// Matrix6d J = Matrix6d：：Identity（）；

// 嘗試把J近似為I？

_jacobianOplusXi

=

-

J

*

v2

。

inverse

（）。

Adj

（）；

_jacobianOplusXj

=

J

*

v2

。

inverse

（）。

Adj

（）；

}

}；

近似為

時的結果與近似為單位陣 I 的結果在此例中差異不明顯。

3。3 小結

球的例子比較典型，既有里程計邊也有迴環邊，實際SLAM中也可能有。

位姿圖是結構最簡單的圖之一，但是在沒有相機運動的先驗假設的情況下，無法進一步利用位姿圖的求解結構。

前端（tracking）需要實時響應影片速度，如30幀/s，但是後端最佳化可以相對前端較慢的進行，進行沒必要對後端提出過高的速度要求。

個人覺得本章習題意義不大，第四題gtsam是因子圖相關的庫，在第二版沒有提到。