窺探PTAM之姿態估計

4 姿態估計

在斑點搜索之後，我們有一堆新找到的點集在當前視圖的投影，由於測量誤差的關係，點集的座標位置並不是精準的，我們需要從這些不太準確的數據中估算新的攝像機姿態和位置。把這羣點集的座標值作爲觀測值，把運動模型計算的點位置作爲估計值。求某個運動參數向量，使估計值與觀測值的殘差平方和最小，即爲我們要計算的姿態更新增量。
姿態估計分爲粗搜索和精搜索兩個階段。粗搜索搜索範圍更廣，使用點數較少。精搜索搜索範圍較窄，使用上千個點樣本。兩個階段解決的一樣的問題，但精搜索樣本大對計算資源消耗極大，因此引入一些優化方法。

4.1 姿態更新

斑塊搜索中得到的斑塊第0層位置u,v作爲獨立的測量值，把j組測量值線性組合成一個向量：Pj={u1,v1,u2,v2,...uj,vj} 。而運動模型估計的u,v爲：

[û j,v̂ j]T=CamProj(exp(μ)Ecwpj)

用 ^ 區分觀測值和估計值。ej 代表重投影誤差向量。CamProj函數可以將攝像機座標系中的值轉到圖像座標系，則測量殘差爲：

r2j=uj−û j;r2j+1=vj−v̂ j

那麼解∑iri=min 就是一個最小二乘問題。把參數向量μ=[α,β,γ,Tx,Ty,Tz] 作爲待估計參數。試着把uj^(pj),vj^(pj) 展開會發現這是一個非線性方程(組)。即估計運動參數的實質就是求解非線性最優解問題。下面介紹用高斯牛頓法求解非線性最小二乘問題。

4.2 最小二乘

(ref:使用Math.NET求解線性和非線性最小二乘問題 | 趙毅力的視覺研究博客 )

最小二乘問題出現在許多應用中。它可以被看作是一個最優化問題（optimization problem）。例如，我們蒐集了一些數據，然後想用這些數據對一個模型進行擬合，並使得這些數據和求解到的模型在某種意義上達到最佳擬合。 一般可以將最小二乘問題緊湊地表示爲：

f(x)=12∑j=1mr2j(x)

其中x是模型參數組成的向量，r被稱爲殘差向量（residual vector），可以表示預期值和測量值之間的差異，是由單獨的殘差所組成的列向量：

rj(x)=y(x̃ j|x)–ỹ j

y(α|x)=x1X1(α)+x2X2(α)+…+xnXn(α)

函數y是我們的模型函數（或期望模型），它根據給定的樣本點x̃_j以及模型參數x返回預期值。這個值再和ỹ j 進行比較，這裏ỹ j 爲觀測值。波浪線標記(~)用來區分從模型參數(x)和模型函數(y)獲獲取的數據點。

4.2.1設計矩陣

最小二乘問題可以用矩陣表示，引入設計矩陣A：

Aji=Xi(x̃ j)=∂rj∂xi=∂∂xi[y(x̃ j|x)–ỹ j]=Xi(x̃ j)

這裏x是要擬合模型的參數向量，X是基函數（basis functions）。基函數可以是參數向量x的任何函數。把最小二乘化爲矩陣形式：

∥r∥2=rTr=(Ax–ỹ )T(Ax–ỹ )=xTATAx–2xTATỹ +ỹ Tỹ 

要對∥r∥2 進行最優化求解。可以通過計算上述表達式對於參數x的偏導數，並令它等於零：

2ATAx–2ATỹ =0

求解得到：

x=(ATA)−1ATỹ 

4.2.2 非線性最小二乘

對於線性最小二乘問題，我們將問題求解歸結到對方程組Ax=b進行求解，而對於非線性最小二乘問題，我們將會有諸如A(x)x=b形式的方程組，即設計矩陣也依賴於模型參數。 在非線性的情況下能夠使用的最好方法是爲參數向量提供一個初始值，然後讓這個初始值逐步趨近於正確的解向量。這種方法會在每一步對參數向量進行更新，並重復這個過程直到收斂，因此是一種迭代算法。 在線性最小二乘問題中稱爲設計矩陣的矩陣A在非線性最小二乘問題中被稱爲雅可比矩陣J(x)。它的定義和設計矩陣的定義相同：

Jj,i(x)=[∂∂xirj]

但與設計矩陣不同的是，它依賴於模型參數x。由定義可知，我們可以直接從我們的模型函數構造雅可比矩陣：

J(x)=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢∂∂x1y(x̃ 1|x)∂∂x1y(x̃ 2|x)⋮∂∂x1y(x̃ m|x)∂∂x2y(x̃ 1|x)∂∂x2y(x̃ 2|x)⋮∂∂x2y(x̃ n|x)⋯⋯⋱⋯∂∂xny(x̃ 1|x)∂∂xny(x̃ 2|x)⋮∂∂xny(x̃ m|x)⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥

每一種迭代算法在開始計算時都需要提供參數向量x的一個初始估計值x(0)，並用下面的形式計算在當前迭代步驟中新的參數估計值：

x(k+1)=x(k)+p(k)

pk 表示第k次迭代的修正因子，x（k）表示第k次迭代的值。此外，目標函數的函數值應該是在每一步變得更小：

f(x+p)<f(x)

通過對不等式左邊進行一階泰勒展開我們有：

f(x)+g(x)Tpg(x)Tp<<f(x)0

∇2f(x)≈J(x)TJ(x)=H(x)

上式中：

• g(x)表示目標函數的梯度向量，即目標函數，或當前對目標函數的近似，相對於模型參數的一階導數的向量。它描述了目標函數在某點x的斜率（slopes）。
• 用H(x)表示目標函數的海森矩陣，即目標函數（或當前對目標函數的近似）相對於每一種參數組合的二階偏導數矩陣。它描述了目標函數在某點x的曲率（curvature）。目標函數的二階偏導可以用hessian矩陣近似表達。

這樣就給出了以目標函數的梯度表示的減小條件。

4.3 高斯牛頓法

應用高斯牛頓法可以解決非線性最小二乘問題。由於的fi(x)非線性性，此時按梯度等於 0,得到的是一個非線性方程組，求解困難。常用的基本思想是用一系列線性最小二乘問題求解該非線性最小二乘問題：設x(k)是解的第k次近似， 在x(k)處將函數fi(x)線性化，把問題化爲線性最小二乘問題，求出第k+1 次近似解x(k+1);再從x(k+1) 出發，重複此過程，直到達到迭代終止準則。
首先原方程可以化解爲二次式最優解的問題：

f(x+p)≈f(x)+pT∇f(x)+12pT∇2f(x)p=min

嘗試用牛頓法得到牛頓法迭代步驟公式，實際就是上式對向量p求導令等於0,然後變形，得到牛頓迭代定義：( ref：矩陣求導.pdf):

p(k)=–∇2f−1(k)∇f(k)=–H−1(k)g(k)

此時按牛頓法的思路用f的hessian矩陣計算上式結果太複雜。如上節所述，用雅可比矩陣近似替代目標函數的hessian矩陣。所以上式也可以寫爲：

HkpGNkJTkJkpGNk==–gk–JTkrk

對上式求解P即爲高斯牛頓法迭代定義。完整的迭代過程如下：
1. 取初始點 x (1) ,置精度要求 ε ,令 k=1;
2. 計算雅可比矩陣Jk ；
3. 若||Jkr||≤ε ,則停止計算，x(k)爲所求k的極小點;否則解以上線性方程組。得到 Gauss-Newton 方向 pGNk ；||表示取範數。
4. 令x(k+1)=x(k)+pGNk,k=k+1 轉2;

由迭代步驟可知解非線性最小二乘最優化問題僅需要雅可比矩陣和殘差向量。實際編程中使用TooN庫的wls類解姿態更新的最小二乘問題。

4.4 圖像雅可比

現在我們的問題就變成前一節所說的純數學問題：求出哪些攝像機運動參數能讓目標函數達到最小值，參數向量μ 相當於上一節的x,觀測值û j 相當於y(xj^,x) ,而xj^ 表示一組觀測輸入，比如點所在的世界座標系xw,yw,zw 。
解最小二乘需要雅可比矩陣，首先Camproj函數分別對參數求偏導。用X代表該參數。
對X 求偏導：

∂u∂X=ΔuΔX;X=μ[n]

由針孔攝像機模型可知：

p′c=MEcwpw=Mpc

M爲4x3運動矩陣。需要注意的，M可以分解。例如第l+1次迭代時，M可以分解爲

⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢r11r21r310r12r22r320r13r23r330∑jTxj∑jTyj∑jTzj1⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥=[Rl,Tl]...[R2,T2][R1,T1]

pl+1=Mpc=[Rl,Tl]pl

即第l+1次迭代時，p′c 可以通過上一次迭代結果作爲初值。

記p′c=[x′c,y′c,z′c] 向量p′c 求微分有：

Δmotion=[Δx,Δy,Δz]T=∂p′c∂XΔX

將Δmotion 的Δx,Δz 分量代入[4]式，得û  對變量X求偏導的通用公式：

∂û ∂X=λzc∂x′c∂X+xc∂z′c∂Xz2

λ 爲x方向的焦距λ=fx 。記motion=[∂y′c∂X,∂x′c∂X,∂z′c∂X] ,物理意義上，motion可以理解爲攝像機做某個微分運動時造成該點在攝像機座標系下的運動量。攝像機微分運動分爲微分平移和微分旋轉。

當X爲Tx時，P′c 對Tx求偏導得

∂x′c∂Tx=∂(m11x+m12y+m13z+Tx)∂Tx=1

∂y′c∂Tx=0

∂z′c∂Tx=0

motionTx=[1,0,0] ,同理得motionTy=[0,1,0],motionTz=[0,0,1]
當X爲繞x軸的旋轉量α 時，矩陣Mpc 對α 求偏導：

motionα=(∂R∂α+∂T∂α)pc

第二項爲0,第一項爲繞x軸旋轉的微分旋轉矩陣。因爲微分旋轉量很小，所以有：

sin(δα)=0,cos(δα)=1

微分旋轉矩陣rot(x,α) 爲：

∂R∂α=⎡⎣⎢⎢⎢⎢00000sin(δα)cos(δα)00−cos(δα)sin(δα)00001⎤⎦⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢000000100−1000001⎤⎦⎥⎥⎥⎥

代入求偏導式得：

motionα=[0,−z,y]

同理可得，對繞y軸旋轉量β 求偏導：

motionβ=[z,0,−x]

對繞z軸旋轉γ 求偏導：

motionγ=[−y,x,0]

將motion代入求偏導通用公式即得到û j 對參數向量[Tx,Ty,Tz,α,β,γ] 求偏導的結果，v̂ j 對參數向量求偏導的的過程也類似。遍歷所有點對參數向量求導便得到一個雅可比矩陣：

⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢∂û 1∂Tx∂v̂ 1∂Tx...∂û j∂Tx∂v̂ j∂Tx∂û 1∂Ty∂v̂ 1∂Ty...∂û j∂Ty∂v̂ j∂Ty∂û 1∂Tz∂v̂ 1∂Tz...∂û j∂Tz∂v̂ j∂Tz∂û 1∂α∂v̂ 1∂α...∂û 1∂α∂v̂ j∂α∂û 1∂β∂v̂ 1∂β...∂û j∂β∂v̂ j∂β∂û 1∂γ∂v̂ 1∂γ...∂û j∂γ∂v̂ j∂γ⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥

將此雅可比矩陣和殘差rj 代入高斯牛頓迭代定義式，求得修正因子PN 。編程實現上，採用TooN庫的wls類可以簡化以上操作。使用修正因子修正μj+1=μj+PN 即完成一次迭代。之後再次運行上面的過程，投影獲得估計值，計算雅可比矩陣，代入高斯牛頓迭代定義式。直到修正因子收斂，完成一次姿態更新。

4.5 精搜索時使用線性最小二乘

PTAM跟蹤系統中，第一次搜索僅選取至多40個跟蹤點，在大範圍內搜索以更新姿態。第二次爲精搜索，在精搜索中，我們要計算數千個點。如果每次迭代都重新投影這些點，重新計算它們組成的非線性方程組的雅可比矩陣。那麼消耗的計算資源是巨大的。PTAM採用的策略是先非線性計算一次，再線性計算兩次，作爲一個週期。通常三個週期後修復因子已經到了10−7 精度的極小值。最後再做一次非線性計算，完成一次精搜索的姿態更新。
在前面的非線性解最優解中，提到了目標方程是非線性的，以其中求u變量爲例：

uj=Camproj(μ,pc)=u0+fuxczc

[xc,yc,zc]T=MEcwpwj,M=exp(μ)

顯然Camproj關於μ 的方程是非線性方程。用F表示Camproj(p1…j) 方程組，x 表示μ 參數向量，J爲該方程組的雅可比矩陣。F是個多元方程，F(x)在p 處的多元一次泰勒展開爲:

F(x)≈F̃ (x)=F(p)+JF(p)⋅(x–p)

這是個線性方程，變形一下得到：

F̃ (x)−F(p)=JF(p)⋅PN

上式展示了用近似的線性方程逼近結果。等式左邊可以是Δuj^ 或Δvj^ 。其逼近的方向是上一次非線性逼近時計算的各個梯度方向，增量也是上一次非線性迭代計算的PN 。這樣的逼近的結果是近似的，但是有可能不準甚至遠離結果，這取決於得到的修正引子Δμ ,當它取極小值時，得到的結果和重計算投影公式結果相同。取值越大，越偏離F(p+Pn) 的結果。所以不能做多次近似逼近。以一個二元方程爲例演示：

僞代碼如下：

4.6 穩健迴歸(M估計)方法簡介

由於測量中系統可能存在粗差。個別異常大的殘差的出現會導致平方和迅速增大。爲了達到平方和爲最小的目的，估值必然要遷就那些離羣值[13]。如果用增長較慢的殘差函數代替平方和函數，也許就能獲得一定抗粗差性的估計值。M估計正是基於這樣的想法，在60年代由Huber提出來。
M-estimators是一類廣泛估計函數[8]，定義爲所給數據上的最小和函數。最小平方估計和極大似然估計都是M估計法。M估計法由魯棒的數據作爲運行保證。一般地，一個M-estimation定義爲一個估計函數爲0的情況。這個估計函數經常是一些統計函數。比如令一個由參數定義的極大似然函數爲0，因此一個極大似然估計值往往是一個能量函數取得極值得點。在很多應用中，這樣的M估計可以用於參數估計。

在最小二乘中，觀測方程爲：
rTPr=min
其中權重矩陣P=diag(p1,p2...) 。也就是每一項多乘以一個權重因子。pi 爲權重引子，由權函數計算獲得。權函數是關於殘差的函數。在PTAM中，我們使用tukey發明的biweight權函數做迭權迭代：

wi={0(1−∣ui/2∣)2 if ∣ui/c∣>1 if ∣ui/c∣≤1

式中c一般4.685。uI=eI/s=0.6745⋅ei/med(∣ei−med(ei)∣) . med()爲中位數，s爲殘差尺度。在跟蹤系統中，wls庫已經封裝好了計算最小二乘時加入權重的方法。綜合上一節，只需要迭代每個點時這樣添加數據：

wls.add_mJ(v2[0], TD.dSqrtInvNoise * m26Jac[0], dWeight);

參數分別是上一節計算得到的殘差，雅可比矩陣的第i個方程的梯度向量，權重。添加完後調用wls.compute()得到結果。其實質就是求解高斯牛頓迭代定義式中的修正因子向量。

4.7 重定位簡介

PTAM使用比較簡單的重定位方法。在跟蹤系統中，當判定跟蹤丟失時啓動重定位。重定位使用低分辨率的模糊的關鍵幀做處理。首先在構建線程每次加入關鍵幀時保留圖像金字塔最上層的圖像，並做高斯模糊處理，生成圖像和地圖數據一起保存。當判定跟蹤丟失時，給當前幀畫面也生成一個小尺寸模糊圖像。直接對地圖數據中所有關鍵幀比較處理，選擇最接近的關鍵幀做姿態估計。