接上一篇博客《直接法光度誤差導數推導》，DSO 代碼中 CoarseInitializer::trackFrame 目的是優化兩幀（ref frame 和 new frame）之間的相對狀態和 ref frame 中所有點的逆深度。
在代碼中出現了變量Hsc和變量bsc，其中的"sc"是指 Schur Complement。依據這個事實就能夠確定整個優化過程的所有細節。
一下假設 ref frame 上需要優化逆深度的點共有 N 個。
首先構建 Gauss Newton 方程，需要優化的參數共有 N + 8 個。
\[J^TJ \delta x = - J^T r_{21} \]
其中\(\delta x\)是一個(N+8)x1的向量，\(J\) 是 Nx(N+8) 的矩陣，第 i 行表示\(r_{21}^{(i)}\) 對\(\delta x\)的導數（求導得到的結果就是“橫着”的），\(r_{21}\)是 Nx1 的向量。
\[\begin{align}\delta x &= \begin{bmatrix} \delta \rho^{(1)} & \delta \rho^{(2)} & \dots & \delta \rho^{(N)} & \delta \xi_{21}^{(1)} & \delta \xi_{21}^{(2)} & \dots & \delta \xi_{21}^{(6)} & \delta a_{21} & \delta b_{21} \end{bmatrix}^T \notag \\ & = \begin{bmatrix} \delta \rho^T & \delta x_{21}^T \end{bmatrix}^T\notag \end{align}\]
\(x_{21}\) 表示量幀之間的相對狀態轉換，包括 8 個參數。
\[\begin{align} J &= \begin{bmatrix} \partial r_{21}^{(1)} \over \partial \rho^{(1)} & \partial r_{21}^{(1)} \over \partial \rho^{(2)} & \dots & \partial r_{21}^{(1)} \over \partial \rho^{(N)} & \partial r_{21}^{(1)} \over \partial \xi_{21}^{(1)} & \partial r_{21}^{(1)} \over \partial \xi_{21}^{(2)} & \dots & \partial r_{21}^{(1)} \over \partial \xi_{21}^{(6)} & \partial r_{21}^{(1)} \over \partial a_{21} & \partial r_{21}^{(1)} \over \partial b_{21} \\ \partial r_{21}^{(2)} \over \partial \rho^{(1)} & \partial r_{21}^{(2)} \over \partial \rho^{(2)} & \dots & \partial r_{21}^{(2)} \over \partial \rho^{(N)} & \partial r_{21}^{(2)} \over \partial \xi_{21}^{(1)} & \partial r_{21}^{(2)} \over \partial \xi_{21}^{(2)} & \dots & \partial r_{21}^{(2)} \over \partial \xi_{21}^{(6)} & \partial r_{21}^{(2)} \over \partial a_{21} & \partial r_{21}^{(2)} \over \partial b_{21} \\ \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots \\ \partial r_{21}^{(N)} \over \partial \rho^{(1)} & \partial r_{21}^{(N)} \over \partial \rho^{(2)} & \dots & \partial r_{21}^{(N)} \over \partial \rho^{(N)} & \partial r_{21}^{(N)} \over \partial \xi_{21}^{(1)} & \partial r_{21}^{(N)} \over \partial \xi_{21}^{(2)} & \dots & \partial r_{21}^{(N)} \over \partial \xi_{21}^{(6)} & \partial r_{21}^{(N)} \over \partial a_{21} & \partial r_{21}^{(N)} \over \partial b_{21} \end{bmatrix} \notag \\ & = \begin{bmatrix} J_\rho & J_{x_{21}} \end{bmatrix}\notag \end{align}\]
Gauss Newton 方程可以進一步寫成
\[\begin{bmatrix} (J_\rho^TJ_\rho)_{N \times N} & (J_\rho^TJ_{x_{21}})_{N \times 8} \\ (J_{x_{21}}^TJ_\rho)_{8 \times N} & (J_{x_{21}}^TJ_{x_{21}})_{8 \times 8} \end{bmatrix} \begin{bmatrix} \delta \rho \\ \delta x_{21} \end{bmatrix} = - \begin{bmatrix} J_\rho^T r_{21} \\ J_{x_{21}}^T r_{21}\end{bmatrix} \]
\[\begin{bmatrix} H_{\rho\rho} & H_{\rho x_{21}} \\ H_{\rho x_{21}}^T & H_{x_{21}x_{21}} \end{bmatrix} \begin{bmatrix} \delta \rho \\ \delta x_{21} \end{bmatrix} = - \begin{bmatrix} J_\rho^T r_{21} \\ J_{x_{21}}^T r_{21}\end{bmatrix} \]
Schur Complement 消除 \(\delta \rho\)：
\[\begin{bmatrix} H_{\rho\rho} & H_{\rho x_{21}} \\ 0 & H_{x_{21}x_{21}} - H_{\rho x_{21}}^TH_{\rho\rho}^{-1}H_{\rho x_{21}} \end{bmatrix} \begin{bmatrix} \delta \rho \\ \delta x_{21} \end{bmatrix} = - \begin{bmatrix} J_\rho^T r_{21} \\ J_{x_{21}}^T r_{21} - H_{\rho x_{21}}^TH_{\rho\rho}^{-1} J_\rho^T r_{21} \end{bmatrix} \]
CoarseInitializer::trackFrame 是調用了 CoarseIntializer::calcResAndGS 計算 Gauss Newton 相關的數值。
在函數 CoarseIntializer::calcResAndGS 中變量acc9與公式中的\(H_{x_{21}x_{21}}, J_{x_{21}}^T r_{21}\)相關，acc9SC與公式中的\(H_{\rho x_{21}}^TH_{\rho\rho}^{-1}H_{\rho x_{21}}, H_{\rho x_{21}}^TH_{\rho\rho}^{-1} J_\rho^T r_{21}\)相關。
acc9.H是9x9的矩陣：
\[\begin{bmatrix} J_{x_{21}}^TJ_{x_{21}} & J_{x_{21}}^Tr_{21} \\ J_{x_{21}} r_{21} & r_{21}r_{21} \end{bmatrix} \]
acc9的累加在兩個循環內部，循環是遍歷 patternNum。
第一個循環 for(int i=0;i+3<patternNum;i+=4) 內部是 acc9.updateSSE。
acc9.updateSSE 使用了 Intel 的 Streaming SIMD Extensions (SSE)，一次操作取 4 個 float，完成 4 個不同數據、相同命令的 float 運算。這種一次 4 個數據的操作，說明了循環的 i 增量是 4。算法現在使用的 patternNum 是 8（8%4==0），所以當前循環可以完成所有 pattern 點的操作。如果 patternNum%4 != 0，剩下的余數就可以用第二個循環 for(int i=((patternNum>>2)<<2); i < patternNum; i++)，((patternNum>>2)<<2)把最后兩個 bit 設置為 0，得到了小於 patternNum 的最大的 4 的倍數，這個循環每次進行 1 個float運算，把最后未循環到的 pattern 點補足。
為了理解acc9SC需要將前面的兩個舒爾補矩陣進行變換：
\[\begin{align}H_{\rho x_{21}}^TH_{\rho\rho}^{-1}H_{\rho x_{21}} &= (J_\rho^TJ_{x_{21}})^T (J_\rho^TJ_\rho)^{-1}J_\rho^TJ_{x_{21}}\notag \\ &= {1 \over J_\rho J_\rho^T} J_{x_{21}}^TJ_\rho (J_\rho^TJ_\rho)^{-1} J_\rho^T J_\rho J_\rho^T J_{x_{21}} \notag \\ &= {1 \over J_\rho J_\rho^T} J_{x_{21}}^TJ_\rho J_\rho^T J_{x_{21}} \notag \end{align}\]
\[\begin{align} H_{\rho x_{21}}^TH_{\rho\rho}^{-1} J_\rho^T r_{21} &= (J_\rho^TJ_{x_{21}})^T (J_\rho^TJ_\rho)^{-1}J_\rho^Tr_{21} \notag \\ &= {1 \over J_\rho J_\rho^T} J_{x_{21}}^TJ_\rho J_\rho ^Tr_{21} \notag \end{align}\]
JbBuffer_new在 idx pattern 循環內，分別對每點的8個 pattern 的\(J_{x_{21}}^TJ_\rho, J_\rho^Tr_{21}, J_\rho J_\rho^T\)進行累加。最后在 idx 點循環完成之后，使用JbBuffer_new中的數據對acc9SC進行處理，累加得到最終的結果。在處理的過程中代碼
JbBuffer_new[i][9] = 1 / (1 + JbBuffer_new[i][9]);
對應着\({1 \over J_\rho J_\rho^T}\)，分母里多了一個1，猜測是為了防止JbBuffer_new[i][9]太小造成系統不穩定。
回到函數 CoarseInitializer::trackFrame 中，在得到相關的矩陣之后，變量H, Hsc相減解算\(x_{21}\)，對Hsc加了一個權值1/(1+lambda)，並且在接受更新時降低lambda提高了Hsc的權重。這個權重相當於是\(d\)對於\(x_{21}\)的權重。
對\(x_{21}\)的更新很容易看到，對\(d\)的更新在 CoarseInitializer::doStep 中。
float b = JbBuffer[i][8] + JbBuffer[i].head<8>().dot(inc);
float step = -b * JbBuffer[i][9] / (1 + lambda);
在得到\(\delta x_{21}\)之后可以代回求\(\delta \rho\)：
\[ H_{\rho\rho}\delta \rho + H_{\rho x_{21}}\delta x_{21} = -J_\rho^Tr_{21} \\ \delta \rho = -H_{\rho\rho}^{-1}(J_\rho^Tr_{21} + H_{\rho x_{21}}\delta x_{21})\]
對於每一個點，代碼中inc對應\(\delta x_{21}\)，JbBuffer[i].head<8>()對應\(H_{\rho x_{21}}\)的第 8*i 行到第 8*i+7 行，JbBuffer[i][8]對應\(J_\rho^Tr_{21}\)的第 8*i 行到第8*i+7 行，JbBuffer[i][9]對應\(H_{\rho\rho}^{-1}\)的\((i, i)\)位置上的 Scalar。
