基于ArUco的視覺定位（四）

  這篇部落格專門介紹一下MarkerMapper的實作原理，不了解MarkerMapper的可以先看一下我的上一篇部落格：基于ArUco的視覺定位（三）。

Paper：Mapping and localization from planar markers （很重要）

  這不是一篇翻譯，而是部落客對上述論文的總結，了解之中難免有不到位的地方，不足之處還請參看原文。

〇、MarkerMapper幹了啥？

  這裡再陳述一遍MarkerMapper到底幹了些什麼事：

• 根據我們指定的空間中任意一個ID的marker，MakerMapper将整個空間中任意分布的所有marker以指定marker為參考坐标系建構marker的分布圖，生成一個二維碼分布檔案。二維碼的分布展現在每個marker的四個角點相對指定marker坐标系的三維坐标，即得到所有marker在參考世界坐标系下的位姿分布。其實最重要的也是我們最想得到的，就是所有marker的角點在世界坐标系下的三維坐标，因為有了這些角點的三維坐标，我們就可以用PnP算法估計相機的位姿。
• 在建構二維碼分布圖的過程中，MarkerMapper同時對相機的運作軌迹進行了估計，記錄了關鍵幀在參考世界坐标系下的位姿。這裡的關鍵幀是指那些至少觀察到兩個marker的幀。
• 除了優化了二維碼的位姿分布和相機的運作軌迹，MarkerMapper還優化了相機的内參數。
• MakerMapper為我們生成了一個包含二維碼位姿分布以及關鍵幀軌迹的可視化點雲圖。

一、基本原理

1、基本思路

  如上圖(a)所示，當第 f   6 f\,^6 f6幀圖像中同時檢測到marker1和marker2時，我們可以得到相機目前與marker1之間的位姿關系 γ   1 6 \gamma\,_1^6 γ16​、與marker2之間的位姿關系 γ   2 6 \gamma\,_2^6 γ26​，由此我們就可以在 f   6 f\,^6 f6幀下求得marker1與marker2之間的位姿變換矩陣 γ   2 , 1 6 = ( γ   2 6 ) − 1 γ   1 6 \gamma\,_{2,1}^6=(\gamma\,_2^6)^{-1}\gamma\,_1^6 γ2,16​=(γ26​)−1γ16​；當第 f   7 f\,^7 f7幀圖像中同時檢測到marker1、marker2和marker3時，我們可以得到相機目前與marker1之間的位姿關系 γ   1 7 \gamma\,_1^7 γ17​、與marker2之間的位姿關系 γ   2 7 \gamma\,_2^7 γ27​、以及與marker3之間的位姿關系 γ   3 7 \gamma\,_3^7 γ37​，進而我們可以在 f   7 f\,^7 f7幀下求得marker1與marker2之間的位姿變換矩陣 γ   2 , 1 7 \gamma\,_{2,1}^7 γ2,17​、marker1與marker3之間的位姿變換矩陣 γ   3 , 1 7 \gamma\,_{3,1}^7 γ3,17​、marker2與marker3之間的位姿變換矩陣 γ   3 , 2 7 \gamma\,_{3,2}^7 γ3,27​。同理，當第 f   t f\,^t ft幀圖像中同時檢測到marker{ i i i, j j j, k k k, . . . ... ...}時，我們可以求得在目前幀下各marker之間的位姿變換矩陣 γ   j , i t \gamma\,_{j,i}^t γj,it​、 γ   k , i t \gamma\,_{k,i}^t γk,it​、 γ   k , j t \gamma\,_{k,j}^t γk,jt​、 . . . ... ...。

  按照上面的思路，我們可以得到空間中所有二維碼任意兩兩之間的相對位姿關系。指定其中一個二維碼坐标系作為參考世界坐标系，就可以求得所有二維碼相對于參考坐标系的位姿，進而得到所有二維碼的角點在世界坐标系下的三維坐标。

  道理我們都懂，但實作起來遠非這麼簡單。

　　

  理論上，在相機經過若幹幀觀察到所有marker後，我們就可以推算出任意marker間的相對位姿關系 γ   j , i \gamma\,_{j,i} γj,i​，但由于光線不佳、相機移動過快、分辨率較低、對焦不清等等原因，二維碼角點的像素坐标是不準的，由此計算出來的 γ i t \gamma_i^t γit​也是不準的， γ   j , i t \gamma\,_{j,i}^t γj,it​就更不準。總而言之，由于觀測噪聲的存在，這種簡單地推算出來的關系是非常不準确的，存在極大的累積誤差。是以，如何在有噪聲的觀測資料下精确地計算出每個marker在世界坐标系下的角點坐标，才是我們真正要解決的問題。

2、核心問題

  經過上面的分析，我們知道，問題的關鍵在于如何在相機不準确的觀測資料下估計出二維碼準确的位姿分布。了解SLAM的同學應該都知道，要解決這個問題最終不得不使出我們的殺手锏——非線性優化。好吧，MarkerMapper最終也要歸結到一個優化問題上，基本手段就是最小化重投影誤差：

e i t = ∑ j [ Ψ ( δ , γ t , γ i ⋅ c j ) − u i , j t ] 2 (非常重要) \color{#F00}{e_i^t=\sum_{j}[\Psi(\delta,\gamma^t,\gamma_i\cdot c_j)-{\bf u}_{i,j}^t]^2\tag{非常重要}} eit​=j∑​[Ψ(δ,γt,γi​⋅cj​)−ui,jt​]2(非常重要)

解釋一下這個重要的公式：

• c j c_j cj​為已知量，表示marker   i   \,i\, i在該marker坐标系下角點   j   \,j\, j的三維坐标。通常，在marker邊長已知為   s   \,s\, s的情況下，每個marker的4個角點在自身坐标系下的坐标分别為 c 0 = ( s / 2 , − s / 2 , 0 ) ,   c 1 = ( s / 2 , s / 2 , 0 ) ,   c 2 = ( − s / 2 , s / 2 , 0 ) ,   c 3 = ( − s / 2 , − s / 2 , 0 ) c_0=(s/2,-s/2,0),\,c_1=(s/2,s/2,0),\,c_2=(-s/2,s/2,0),\,c_3=(-s/2,-s/2,0) c0​=(s/2,−s/2,0),c1​=(s/2,s/2,0),c2​=(−s/2,s/2,0),c3​=(−s/2,−s/2,0)。
• γ i \gamma_i γi​為待估計量，表示從marker坐标系到世界坐标系的齊次變換矩陣，這裡的世界坐标系是指參考的marker坐标系。 γ i ⋅ c j \gamma_i\cdot c_j γi​⋅cj​是将marker   i   \,i\, i的第   j   \,j\, j個角點有marker坐标系轉換到世界坐标系下表示。
• γ t \gamma^t γt為待估計量，表示從世界坐标系到相機坐标系的齊次變換矩陣，也即相機的外參。
• δ \delta δ為待估計量，表示相機的内參矩陣。
• 函數 Ψ ( δ , γ t , γ i ⋅ c j ) \Psi(\delta,\gamma^t,\gamma_i\cdot c_j) Ψ(δ,γt,γi​⋅cj​)表示marker   i   \,i\, i的角點   j   \,j\, j從三維空間坐标到像素坐标的一種投影，它先将角點 c j c_j cj​轉換到世界坐标系，再轉換到相機坐标系，最後轉換到像素坐标系。是以， Ψ \Psi Ψ表示的角點像素坐标是根據待估計量計算出來的，稱為重投影。
• u i , j t u_{i,j}^t ui,jt​為觀測量，表示在第 f t f^t ft幀下觀察到的marker   i   \,i\, i第   j   \,j\, j個角點真實的像素坐标，由于各種原因産生的觀測噪聲，真實的并不代表就是準确的。

是以， e i t = ∑ j [ Ψ − u i , j t ] 2 e_i^t=\sum_{j}[\Psi-{\bf u}_{i,j}^t]^2 eit​=∑j​[Ψ−ui,jt​]2表示在第 f t f^t ft幀下marker   i   \,i\, i 4個角點的重投影誤差平方和，我們的最終目的是估計出最優的   γ i \,\gamma_i γi​、 γ t \gamma^t γt、 δ   \delta\, δ 使得所有的   e i t   \,e_i^t\, eit​和最小，即：

e ( γ 1 , ⋅ ⋅ ⋅ , γ M , γ 1 , ⋅ ⋅ ⋅ , γ N , δ ) = ∑ t ∑ i ∈ f t e i t (1) {\bf e}(\gamma_1,\cdot\cdot\cdot,\gamma_M,\gamma^1,\cdot\cdot\cdot,\gamma^N,\delta)=\sum_{t}\sum_{i\in{f^t}}e_i^t\tag{1} e(γ1​,⋅⋅⋅,γM​,γ1,⋅⋅⋅,γN,δ)=t∑​i∈ft∑​eit​(1)

将變換矩陣   γ   \,\gamma\, γ用它的李代數形式   ζ   \,\zeta\, ζ代替，寫成如下形式：

f ( γ i , γ t , δ ) = e ( ζ 1 , ⋅ ⋅ ⋅ , ζ M , ζ 1 , ⋅ ⋅ ⋅ , ζ N , δ ) = ∑ t ∑ i ∈ f t { ∑ j [ Ψ ( δ , Γ ( ζ t ) , Γ ( ζ i ) ⋅ c j ) − u i , j t ] 2 } (2) f(\gamma_i,\gamma^t,\delta)={\bf e}(\zeta_1,\cdot\cdot\cdot,\zeta_M,\zeta^1,\cdot\cdot\cdot,\zeta^N,\delta)=\sum_{t}\sum_{i\in{f^t}}\left\{\sum_{j}[\Psi(\delta,\Gamma(\zeta^t),\Gamma(\zeta_i)\cdot c_j)-{\bf u}_{i,j}^t]^2\right\}\tag{2} f(γi​,γt,δ)=e(ζ1​,⋅⋅⋅,ζM​,ζ1,⋅⋅⋅,ζN,δ)=t∑​i∈ft∑​{j∑​[Ψ(δ,Γ(ζt),Γ(ζi​)⋅cj​)−ui,jt​]2}(2)

其中， ζ = ( r , t )   ∣   r , t ∈ R 3 \zeta=({\bf r},{\bf t})\,|\,{\bf r},{\bf t}\in{\Bbb R}^3 ζ=(r,t)∣r,t∈R3， γ = Γ ( ζ ) = [ R t T 0 1 ] \gamma=\Gamma(\zeta)=\begin{bmatrix}{\bf R}&{\bf t}^\text{T}\\{\bf 0}&1\end{bmatrix} γ=Γ(ζ)=[R0​tT1​]，這裡的   r \,\bf r r、 t   \bf t\, t均為行向量。 f t f^t ft為所有觀察到兩個及兩個以上marker的幀組成的集合，我在這裡稱之為關鍵幀。

　　好了，接下來的問題就是求上述非線性函數 f ( γ i , γ t , δ ) f(\gamma_i,\gamma^t,\delta) f(γi​,γt,δ)的最小二乘解，它得到的最優解就是   γ i \,\gamma_i γi​、 γ t \gamma^t γt、 δ   \delta\, δ的最大似然估計值。那麼，如何求解這個最小二乘問題？有以下兩個要點：

• （1）給定待估計量一個合适的初始值
• （2）選擇合适的疊代算法

二、确定優化變量的初始值

  我們要優化的變量有三個：   γ i \,\gamma_i γi​、 γ t \gamma^t γt、 δ   \delta\, δ，其中   δ   \,\delta\, δ的初始值是通過各種相機标定方法線上下獲得的，屬于MakerMapper的輸入參數，是以對   δ   \,\delta\, δ而言直接優化即可，無需再确定它的初始值。下面詳細介紹如何确定   γ i \,\gamma_i γi​、 γ t   \gamma^t\, γt的初始值。

1、如何獲得每個marker在參考世界坐标系下的最優初始位姿   γ ~ i   \,\tilde{\gamma}_i\, γ~​i​

1.1、建立位姿箭圖 Q Q Q，找出marker間最優的位姿變換矩陣 γ ^ j , i \hat \gamma_{j,i} γ^​j,i​

  上圖（b）即是一個位姿箭圖 Q Q Q，它由節點和邊組成。節點表示不同ID的marker，節點之間相連的邊表示所對應marker與marker之間的相對位姿關系 γ j , i t \gamma_{j,i}^{t} γj,it​。不是所有節點之間都有直接相連的邊，節點之間也可能存在多條邊，任意兩個節點之間所有的邊組成的集合記為 χ j , i \chi_{j,i} χj,i​。

  我們首先要解決的，是從任意兩兩節點之間所有的邊 χ j , i \chi_{j,i} χj,i​中選出最優的 γ ^ j , i \hat \gamma_{j,i} γ^​j,i​，用來建立初始位姿圖。定義下式：

ε ( γ j , i t ′ , f t ) = ∑ k [ Φ ( δ , γ j t , γ j , i t ′ ⋅ c k ) − u i , k t ] 2 ，   γ j , i t ′ ∈ χ j , i (3) \varepsilon(\gamma_{j,i}^{t'},f^t)=\sum_k\left[\Phi(\delta,\gamma_j^t,\gamma_{j,i}^{t'}\cdot c_k)-{\bf u}_{i,k}^t\right]^2，\,\gamma_{j,i}^{t'}\in\chi_{j,i}\tag{3} ε(γj,it′​,ft)=k∑​[Φ(δ,γjt​,γj,it′​⋅ck​)−ui,kt​]2，γj,it′​∈χj,i​(3)

  這個式子的意思是，利用在第 f t ′ f^{t'} ft′幀算出的 γ j , i t ′ \gamma_{j,i}^{t'} γj,it′​将marker   i   \,i\, i四個角點的三維坐标轉換到marker   j   \,j\, j下表示，然後通過第 f t f^t ft幀中的 γ j t \gamma_j^t γjt​将marker   j   \,j\, j坐标系下表示的marker   i   \,i\, i的四個角點投影到 f t f^t ft幀的像素坐标中，最後求marker   i   \,i\, i在 f t f^t ft中四個角點的重投影誤差。其中， t , t ′ ∈ F i , j t,{t'}\in\mathcal{F}_{i,j} t,t′∈Fi,j​， F i , j \mathcal{F}_{i,j} Fi,j​表示所有包含marker   i   \,i\, i與marker   j   \,j\, j的幀。

  如果   ε ( γ j , i t ′ , f t ) < ε ( γ j , i t , f t ′ ) \,\varepsilon(\gamma_{j,i}^{t'},f^t)<\varepsilon(\gamma_{j,i}^{t},f^{t'}) ε(γj,it′​,ft)<ε(γj,it​,ft′)，即認為 γ j , i t ′ \gamma_{j,i}^{t'} γj,it′​優于 γ j , i t \gamma_{j,i}^{t} γj,it​，據此，我們就可以從節點   i   \,i\, i與節點   j   \,j\, j的所有邊中找到marker   i   \,i\, i與marker   j   \,j\, j之間最優的位姿矩陣 γ ^ j , i \hat \gamma_{j,i} γ^​j,i​：

γ ^ j , i = argmin γ j , i   ∈ χ j , i   e j , i ( γ j , i ) = argmin γ j , i   ∈ χ j , i ∑ t , t ′ ∈ F i , j ε ( γ j , i t ′ , f t ) (4) \hat\gamma_{j,i}=\underset{\gamma_{j,i}\,\in\chi_{j,i}}{\text{argmin}}\,{\bf e}_{j,i}(\gamma_{j,i})=\underset{\gamma_{j,i}\,\in\chi_{j,i}}{\text{argmin}}\sum_{t,t'\in\mathcal{F}_{i,j}}\varepsilon(\gamma_{j,i}^{t'},f^t)\tag{4} γ^​j,i​=γj,i​∈χj,i​argmin​ej,i​(γj,i​)=γj,i​∈χj,i​argmin​t,t′∈Fi,j​∑​ε(γj,it′​,ft)(4)

上式表示從節點 i , j i,j i,j之間所有的邊 χ j , i \chi_{j,i} χj,i​中找到一條 γ j , i \gamma_{j,i} γj,i​，使得在包含所有marker i , j i,j i,j的幀 F i , j \mathcal{F}_{i,j} Fi,j​中由 γ j , i \gamma_{j,i} γj,i​得到的重投影誤差平方和最小，這條邊就記作 γ ^ j , i \hat \gamma_{j,i} γ^​j,i​

1.2、建立初始位姿圖 G G G，計算位姿圖 G G G的最小生成樹***mst***

  有了 γ ^ j , i \hat \gamma_{j,i} γ^​j,i​，現在 Q Q Q中的所有節點之間就隻剩下了唯一的一條邊，當然有的節點之間也可能沒有邊，但這并不代表它們之間是沒有聯系的。接下來，我們要利用 γ ^ j , i \hat \gamma_{j,i} γ^​j,i​建立一個位姿圖 G G G，如上圖（c）所示。它的節點也代表不同ID的marker，但節點與節點之間有兩條邊，分别為 γ ^ j , i \hat \gamma_{j,i} γ^​j,i​和 γ ^ i , j \hat \gamma_{i,j} γ^​i,j​，兩者是互逆的關系。

  根據位姿圖 G G G，我們可以指定其中任意一個節點代表的marker   a   \,a\, a作為參考，利用節點之間連接配接的邊将所有marker的位姿都轉換到參考marker   a   \,a\, a坐标系下表示：

γ ^ i = γ ^ a , b ⋅ ⋅ ⋅ γ ^ k , h γ ^ h , i = γ ^ a , i (5) \hat \gamma_i=\hat\gamma_{a,b}\cdot\cdot\cdot\hat\gamma_{k,h}\hat\gamma_{h,i}=\hat\gamma_{a,i}\tag{5} γ^​i​=γ^​a,b​⋅⋅⋅γ^​k,h​γ^​h,i​=γ^​a,i​(5)

  在計算 γ ^ i \hat \gamma_i γ^​i​之前要先計算位姿圖 G G G的最小生成樹（mst），即找到可以連通所有節點的最優路徑，路徑是由節點與節點之間的邊連接配接而成。當然，任意一個節點   i   \,i\, i到參考節點   a   \,a\, a的路徑都可能不止一條，這裡尋找最優路徑的原則是使得組成該路徑的所有邊的重投影誤差和最小，即找到一條最佳路徑使得 e a , i ( γ ^ a , i ) {\bf e}_{a,i}(\hat\gamma_{a,i}) ea,i​(γ^​a,i​)最小：

( a , b , ⋅ ⋅ ⋅ , k , h , i ) = argmin   e a , i ( γ ^ a , i ) = argmin   { e a , b ( γ ^ a , b ) + ⋅ ⋅ ⋅ + e k , h ( γ ^ k , h ) + e h , i ( γ ^ h , i ) } (6) (a,b,\cdot\cdot\cdot ,k,h,i)={\text{argmin}}\,{\bf e}_{a,i}(\hat\gamma_{a,i})={\text{argmin}}\,\{{\bf e}_{a,b}(\hat\gamma_{a,b})+\cdot\cdot\cdot+{\bf e}_{k,h}(\hat\gamma_{k,h})+{\bf e}_{h,i}(\hat\gamma_{h,i})\}\tag{6} (a,b,⋅⋅⋅,k,h,i)=argminea,i​(γ^​a,i​)=argmin{ea,b​(γ^​a,b​)+⋅⋅⋅+ek,h​(γ^​k,h​)+eh,i​(γ^​h,i​)}(6)

  上式在論文中并沒有出現，是本人自己的了解，其中 e j , i ( γ ^ j , i ) {\bf e}_{j,i}(\hat\gamma_{j,i}) ej,i​(γ^​j,i​)就是在上一步式（4）中計算的 γ ^ j , i \hat\gamma_{j,i} γ^​j,i​所對應的最小重投影誤差平方和。最後，位姿圖 G G G會變成類似下圖所示的樣子（加粗的黑線就是 mst ），連通節點   i   \,i\, i與節點   j   \,j\, j的路徑都隻剩下了唯一一條，但節點與節點之間依然有兩條互逆的邊（表示往返）：

  其實這一步的最終目的并不是計算 γ ^ i \hat \gamma_i γ^​i​，而是要得到滿足上述條件的最小生成樹 mst 。另外，如果我們在MarkerMapper的入口參數中沒有指定參考marker，那麼程式就會根據上述原則利用弗洛伊德算法從所有的marker中選擇一個合适的參考節點，使得生成的 mst 最優。

1.3、優化位姿圖 G G G，并得到所有marker在參考坐标系下的初始位姿 γ ~ i \tilde\gamma_i γ~​i​

  這一步是論文中最複雜的部分。首先，為了防止outliers（位姿圖 G G G中某些不太靠譜的邊）敗壞優化的過程，我們先計算 mst 中所有邊的權重 ϖ ( e ) = e j , i ( γ ^ j , i ) \varpi(e)={\bf e}_{j,i}(\hat\gamma_{j,i}) ϖ(e)=ej,i​(γ^​j,i​)的均值和标準差，對于不在 mst 中的邊，凡是權重大于均值2.58倍的都去掉，剩下的所有邊和節點組成的位姿圖記作 G ′ G' G′。然後對 G ′ G' G′做進一步優化。

　　

  考慮 G ′ G' G′中的一組循環節點 c = ( 1 , 2 , 3 , ⋅ ⋅ ⋅ , n − 1 , n , 1 ) c=(1,2,3,\cdot\cdot\cdot,n-1,n,1) c=(1,2,3,⋅⋅⋅,n−1,n,1)，節點之間最優的邊 γ ~ j , i \tilde\gamma_{j,i} γ~​j,i​應該滿足下式：

I 4 × 4 = γ ~ 1 , 2 γ ~ 2 , 3 ⋅ ⋅ ⋅ γ ~ n − 1 , n γ ~ n , 1 {\bf I}_{4\times4}=\tilde\gamma_{1,2}\tilde\gamma_{2,3}\cdot\cdot\cdot\tilde\gamma_{n-1,n}\tilde\gamma_{n,1} I4×4​=γ~​1,2​γ~​2,3​⋅⋅⋅γ~​n−1,n​γ~​n,1​

  即經過一個循環，節點   i   \,i\, i仍能投影到原處。假設節點之間的初始位姿 γ ^ j , i \hat\gamma_{j,i} γ^​j,i​都是相對正确的，那我們應該最小化 γ ~ j , i \tilde\gamma_{j,i} γ~​j,i​與 γ ^ j , i \hat\gamma_{j,i} γ^​j,i​之間的權重距離，即：

min ∑ k ∈ c w k , k + 1   d ( γ ~ k , k + 1 , γ ^ k , k + 1 ) \text{min}\sum_{k\in c}w_{k,k+1}\,\text{d}(\tilde\gamma_{k,k+1},\hat\gamma_{k,k+1}) mink∈c∑​wk,k+1​d(γ~​k,k+1​,γ^​k,k+1​)

其中 w k , k + 1 = 1 e k , k + 1 ∑ k ∈ c 1 e k , k + 1 w_{k,k+1}=\cfrac{1}{{\bf e}_{k,k+1}\sum\limits_{k\in c}\cfrac{1}{{\bf e}_{k,k+1}}} wk,k+1​=ek,k+1​k∈c∑​ek,k+1​1​1​， w k , k + 1 w_{k,k+1} wk,k+1​的值域為(0,1)，并且 ∑ k ∈ c w k , k + 1 = 1 \sum\limits_{k\in c}w_{k,k+1}=1 k∈c∑​wk,k+1​=1，下面對 w k , k + 1 w_{k,k+1} wk,k+1​的這兩個性質進行推導：

令    e ′ ( c ) = ∑ k ∈ c 1 e k , k + 1 ∈ ( 0 , + ∞ ) ,    e ′ ′ ( c ) = ∏ k ∈ c e k , k + 1 w k , k + 1 = 1 e k , k + 1 e ′ ( c ) = 1 1 + e ′ ( c − k ) ∈ ( 0 , 1 ) ,    k ∈ c ; e ′ ( c − k ) ∈ ( 0 , + ∞ ) 表 示 e ′ ( c ) 中 沒 有 1 e k , k + 1 這 一 項 ∑ k ∈ c w k , k + 1 = e ′ ′ ( c − 1 ) + ⋅ ⋅ ⋅ + e ′ ′ ( c − k ) + ⋅ ⋅ ⋅ + e ′ ′ ( c − n ) { ∏ k ∈ c e k , k + 1 } e ′ ( c ) = ( 1 e 1 , 2 + ⋅ ⋅ ⋅ + 1 e k , k + 1 + ⋅ ⋅ ⋅ 1 e n , 1 ) e ′ ′ ( c ) { ∏ k ∈ c e k , k + 1 } e ′ ( c ) = e ′ ′ ( c ) { ∑ k ∈ c 1 e k , k + 1 } { ∏ k ∈ c e k , k + 1 } e ′ ( c ) = 1 令\; {\bf e}'(c)=\sum_{k\in c}\cfrac{1}{{\bf e}_{k,k+1}}\in(0,+\infty),\;{\bf e}''(c)=\prod_{k\in c}{\bf e}_{k,k+1}\\ w_{k,k+1}=\cfrac{1}{{\bf e}_{k,k+1}{\bf e}'(c)}=\cfrac{1}{1+{\bf e}'(c-k)}\in(0,1),\;k\in c;{\bf e}'(c-k)\in(0,+\infty)表示{\bf e}'(c)中沒有\cfrac{1}{{\bf e}_{k,k+1}}這一項\\ \sum_{k\in c}w_{k,k+1}=\cfrac{{\bf e}''(c-1)+\cdot\cdot\cdot+{\bf e}''(c-k)+\cdot\cdot\cdot+{\bf e}''(c-n)}{\left\{\prod\limits_{k\in c}{\bf e}_{k,k+1}\right\}{\bf e}'(c)}=\cfrac{(\cfrac{1}{{\bf e}_{1,2}}+\cdot\cdot\cdot+\cfrac{1}{{\bf e}_{k,k+1}}+\cdot\cdot\cdot\cfrac{1}{{\bf e}_{n,1}}){\bf e}''(c)}{\left\{\prod\limits_{k\in c}{\bf e}_{k,k+1}\right\}{\bf e}'(c)}=\cfrac{e''(c)\left\{\sum\limits_{k\in c}\cfrac{1}{{\bf e}_{k,k+1}}\right\}}{\left\{\prod\limits_{k\in c}{\bf e}_{k,k+1}\right\}{\bf e}'(c)}=1 令e′(c)=k∈c∑​ek,k+1​1​∈(0,+∞),e′′(c)=k∈c∏​ek,k+1​wk,k+1​=ek,k+1​e′(c)1​=1+e′(c−k)1​∈(0,1),k∈c;e′(c−k)∈(0,+∞)表示e′(c)中沒有ek,k+1​1​這一項k∈c∑​wk,k+1​={k∈c∏​ek,k+1​}e′(c)e′′(c−1)+⋅⋅⋅+e′′(c−k)+⋅⋅⋅+e′′(c−n)​={k∈c∏​ek,k+1​}e′(c)(e1,2​1​+⋅⋅⋅+ek,k+1​1​+⋅⋅⋅en,1​1​)e′′(c)​={k∈c∏​ek,k+1​}e′(c)e′′(c){k∈c∑​ek,k+1​1​}​=1

  對于 γ ~ = [ R ~ t ~ 0 1 ] \tilde\gamma=\begin{bmatrix}\tilde{\bf R}&\tilde{\bf t}\\{\bf 0}&1\end{bmatrix} γ~​=[R~0​t~1​]和 γ ^ = [ R ^ t ^ 0 1 ] \hat\gamma=\begin{bmatrix}\hat{\bf R}&\hat{\bf t}\\{\bf 0}&1\end{bmatrix} γ^​=[R^0​t^1​]，有：

R ~ 1 , 2 ⋅ ⋅ ⋅ R ~ k , k + 1 ⋅ ⋅ ⋅ R ~ n , 1 = R ^ 1 , 2 E 1 , 2 α 1 , 2 ⋅ ⋅ ⋅ R ^ k , k + 1 E k , k + 1 α k , k + 1 ⋅ ⋅ ⋅ R ^ n , 1 E n , 1 α n , 1 = I 3 × 3 (7) {\bf {\tilde R}}_{1,2}\cdot\cdot\cdot{\bf {\tilde R}}_{k,k+1}\cdot\cdot\cdot{\bf {\tilde R}}_{n,1}= {\bf {\hat R}}_{1,2}{\bf E}_{1,2}^{\alpha_{1,2}}\cdot\cdot\cdot{\bf {\hat R}}_{k,k+1}{\bf E}_{k,k+1}^{\alpha_{k,k+1}}\cdot\cdot\cdot{\bf {\hat R}}_{n,1}{\bf E}_{n,1}^{\alpha_{n,1}}={\bf I}_{3\times3}\tag{7} R~1,2​⋅⋅⋅R~k,k+1​⋅⋅⋅R~n,1​=R^1,2​E1,2α1,2​​⋅⋅⋅R^k,k+1​Ek,k+1αk,k+1​​⋅⋅⋅R^n,1​En,1αn,1​​=I3×3​(7)

其中， E k , k + 1 α k , k + 1 = exp { α k , k + 1   ln E k , k + 1 } {\bf E}_{k,k+1}^{\alpha_{k,k+1}}=\text{exp}\{\alpha_{k,k+1}\,\text{ln}{\bf E}_{k,k+1}\} Ek,k+1αk,k+1​​=exp{αk,k+1​lnEk,k+1​}， α k , k + 1 = 1 / w k , k + 1 ∑ j ∈ c 1 / w j , j + 1 = e k , k + 1 ∑ j ∈ c e j , j + 1 ∈ [ 0 , 1 ] \alpha_{k,k+1}=\cfrac{1/w_{k,k+1}}{\sum\limits_{j\in c}1/w_{j,j+1}}=\cfrac{{\bf e}_{k,k+1}}{\sum\limits_{j\in c}{\bf e}_{j,j+1}}\in[0,1] αk,k+1​=j∈c∑​1/wj,j+1​1/wk,k+1​​=j∈c∑​ej,j+1​ek,k+1​​∈[0,1]， ∑ k ∈ c α k , k + 1 = 1 \sum\limits_{k\in c}\alpha_{k,k+1}=1 k∈c∑​αk,k+1​=1， e j , i ( γ j , i ) = ∑ t , t ′ ∈ F i , j γ j , i   ∈ χ j , i ε ( γ j , i t ′ , f t ) {\bf e}_{j,i}(\gamma_{j,i})=\sum\limits_{t,t'\in\mathcal{F}_{i,j}\\\gamma_{j,i}\,\in\chi_{j,i}}\varepsilon(\gamma_{j,i}^{t'},f^t) ej,i​(γj,i​)=t,t′∈Fi,j​γj,i​∈χj,i​∑​ε(γj,it′​,ft)。注意這裡   E k , k + 1 α k , k + 1    \,{\bf E}_{k,k+1}^{\alpha_{k,k+1}}\; Ek,k+1αk,k+1​​中的 α k , k + 1 {\alpha_{k,k+1}} αk,k+1​并不是一個普通的右上标，而是代表矩陣的指數，可以了解為旋轉矩陣的幂運算，即有：

E k , k + 1 α 1 , 2 ⋅ ⋅ ⋅ E k , k + 1 α k , k + 1 ⋅ ⋅ ⋅ E k , k + 1 α n , 1 = E k , k + 1 α 1 , 2    + ⋅ ⋅ ⋅ +    α k , k + 1    + ⋅ ⋅ ⋅ +    α n , 1 = E k , k + 1 {\bf E}_{k,k+1}^{\alpha_{1,2}}\cdot\cdot\cdot{\bf E}_{k,k+1}^{\alpha_{k,k+1}}\cdot\cdot\cdot{\bf E}_{k,k+1}^{\alpha_{n,1}}={\bf E}_{k,k+1}^{\alpha_{1,2}\;+\cdot\cdot\cdot+\;\alpha_{k,k+1}\;+\cdot\cdot\cdot+\;\alpha_{n,1}}={\bf E}_{k,k+1} Ek,k+1α1,2​​⋅⋅⋅Ek,k+1αk,k+1​​⋅⋅⋅Ek,k+1αn,1​​=Ek,k+1α1,2​+⋅⋅⋅+αk,k+1​+⋅⋅⋅+αn,1​​=Ek,k+1​

其中每一個   E k , k + 1 α k , k + 1    \,{\bf E}_{k,k+1}^{\alpha_{k,k+1}}\; Ek,k+1αk,k+1​​都是與 E k , k + 1 {\bf E}_{k,k+1} Ek,k+1​同軸的旋轉矩陣，即旋轉 E k , k + 1 {\bf E}_{k,k+1} Ek,k+1​由這些微小的旋轉   E k , k + 1 α k , k + 1    \,{\bf E}_{k,k+1}^{\alpha_{k,k+1}}\; Ek,k+1αk,k+1​​組成。

  那麼（7）式表示的到底是什麼意思？就是說，比如在上圖所示的一個循環 c = ( 1 , 2 , 4 , 3 ) c=(1,2,4,3) c=(1,2,4,3)中，我們可以給每個邊添加一個微小的誤差項，使得這個循環滿足（7）式。同樣對于循環 c = ( 1 , 2 , 5 , 3 ) c=(1,2,5,3) c=(1,2,5,3)，我們也可以給每個邊添加一個微小的誤差項，使得這個循環滿足（7）式。即

{ R ^ 1 , 2 E 1 , 2 α 1 , 2   R ^ 2 , 4 E 2 , 4 α 2 , 4   R ^ 4 , 3 E 4 , 3 α 4 , 3   R ^ 3 , 1 E 3 , 1 α 3 , 1 = I 3 × 3 R ^ 1 , 2 E 1 , 2 α 1 , 2   R ^ 2 , 5 E 2 , 5 α 2 , 5   R ^ 5 , 3 E 5 , 3 α 5 , 3   R ^ 3 , 1 E 3 , 1 α 3 , 1 = I 3 × 3 \begin{cases}{\bf {\hat R}}_{1,2}{\bf E}_{1,2}^{\alpha_{1,2}}\,{\bf {\hat R}}_{2,4}{\bf E}_{2,4}^{\alpha_{2,4}}\,{\bf {\hat R}}_{4,3}{\bf E}_{4,3}^{\alpha_{4,3}}\,{\bf {\hat R}}_{3,1}{\bf E}_{3,1}^{\alpha_{3,1}}={\bf I}_{3\times3}\\ {\bf {\hat R}}_{1,2}{\bf E}_{1,2}^{\alpha_{1,2}}\,{\bf {\hat R}}_{2,5}{\bf E}_{2,5}^{\alpha_{2,5}}\,{\bf {\hat R}}_{5,3}{\bf E}_{5,3}^{\alpha_{5,3}}\,{\bf {\hat R}}_{3,1}{\bf E}_{3,1}^{\alpha_{3,1}}={\bf I}_{3\times3}\end{cases} {R^1,2​E1,2α1,2​​R^2,4​E2,4α2,4​​R^4,3​E4,3α4,3​​R^3,1​E3,1α3,1​​=I3×3​R^1,2​E1,2α1,2​​R^2,5​E2,5α2,5​​R^5,3​E5,3α5,3​​R^3,1​E3,1α3,1​​=I3×3​​

對于旋轉矩陣 R k , k + 1 {\bf { R}}_{k,k+1} Rk,k+1​，它的誤差項是一個與之同軸的旋轉矩陣 E k , k + 1 α k , k + 1 {\bf E}_{k,k+1}^{\alpha_{k,k+1}} Ek,k+1αk,k+1​​，這個旋轉矩陣的旋轉角度由 α k , k + 1 \alpha_{k,k+1} αk,k+1​确定， α k , k + 1 \alpha_{k,k+1} αk,k+1​與節點   k   \,k\, k到節點   k + 1 \,k+1 k+1之間的重投影誤差有關。（這一部分我不是特别了解，待修正）

2、如何獲得關鍵幀在參考世界坐标系下的最優初始位姿   γ ~ t   \,\tilde{\gamma}^t\, γ~​t

未完成。。。