3 非 Schur Complement 部分信息計算
參考《DSO windowed optimization 公式》，非Schur Complement 部分指 \(H_{XX}\) 和 \(J^T_{X}r\)。
3.1 AccumulatedTopHessianSSE::addPoint()優化的局部信息計算
EnergyFunctional::accumulateAF_MT() 與 EnergyFunctional::accumulateLF_MT() 遍歷每一個點，對每一個點調用 AccumulatedTopHessianSSE::addPoint()。在 AccumulatedTopHessianSSE::addPoint() 中遍歷點的每一個 residual。計算所有優化系統的信息，存儲在每個點的局部變量和 EnergyFunctional 的局部變量中。
3.1.1 resApprox
首先搞定resApprox。由 VecNRf 可知，這東西是 8x1 的矩陣（也就是每個 residual 都是八個像素點的組合）。
VecNRf resApprox;
if(mode==0) // active
resApprox = rJ->resF;
if(mode==2) // marginalize
resApprox = r->res_toZeroF;
if(mode==1) // linearized
{
// compute Jp*delta
__m128 Jp_delta_x = _mm_set1_ps(rJ->Jpdxi[0].dot(dp.head<6>())+rJ->Jpdc[0].dot(dc)+rJ->Jpdd[0]*dd);
__m128 Jp_delta_y = _mm_set1_ps(rJ->Jpdxi[1].dot(dp.head<6>())+rJ->Jpdc[1].dot(dc)+rJ->Jpdd[1]*dd);
__m128 delta_a = _mm_set1_ps((float)(dp[6]));
__m128 delta_b = _mm_set1_ps((float)(dp[7]));
for(int i=0;i<patternNum;i+=4)
{
// PATTERN: rtz = resF - [JI*Jp Ja]*delta.
__m128 rtz = _mm_load_ps(((float*)&r->res_toZeroF)+i);
rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JIdx))+i),Jp_delta_x));
rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JIdx+1))+i),Jp_delta_y));
rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF))+i),delta_a));
rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF+1))+i),delta_b));
_mm_store_ps(((float*)&resApprox)+i, rtz);
}
}
Residual 有三種情況：
- active 情況最簡單，直接是 residual。
- marginalize 的情況比較復雜，res_toZeroF 在EFResidual::fixLinearizationF()賦值，而 res_toZeroF 與下面計算的 rtz 是類似的。
- linearized 在這里已經給出了其賦值的方法，下面會說到，linearized residual 是不存在的。
所謂的 linearied residual 是指 EFResidual::isActive() 與 EFResidual::isLinearized 都為 true 的 Residual。初始階段 isLinearized 為 false，只要搞清楚 isLinearized 在什么時候設置為 true 就可以了解到 linearized residual 是何種意思。查找了 EFResidual::isLinearized 只在 EFResidual::fixLinearizationF 中設置為 true，而 EFResidual::fixLinearizationF() 僅僅只在 FullSystem::flagPointsForRemoval() 中調用。在此處，將那些符合 2 種情況（1. 因為 residual 太少造成了 Out Of Boundary（這里考慮到將要被 marginalize 掉的幀的影響），2. 主幀要被 marginalize 掉）的點的 residual 設置為 linearized。但是這些點緊接着又會在 EnergyFunctional::marginalizePointsF() 中被 marg 掉，被刪除掉。最終也沒有進入 FullSystem::optimize() 的優化過程中。我在 AccumulatedTopHessianSSE::addPoint() 的這個位置設置了 conditional breakpoint (mode==1)，或者assert(mode!=1)，實驗證明 linearized residual 是不存在的。
-
active residual 時，
resApprox對應的就是簡單的 \(r_{21}\)。 -
linearized residual 時，還要看這個代碼是什么意思。
\(\begin{bmatrix} \text{Jp_delta_x} \\ \text{Jp_delta_y} \end{bmatrix} = {\partial x_2 \over \partial \xi_1}{\delta \xi_1} + {\partial x_2 \over \partial \xi_2}{\delta \xi_2} + {\partial x_2 \over \partial C}{\delta C} + {\partial x_2 \over \partial \rho_1}{\delta \rho_1}\)
\(\begin{bmatrix} \text{delta_a} \\ \text{delta_b}\end{bmatrix} = {\partial l_{21} \over \partial l_1}{\delta l_1} + {\partial l_{21} \over \partial l_2}{\delta l_2}\)
\(\text{rtz} = {\partial r_{21} \over \partial \xi_1}{\delta \xi_1} + {\partial r_{21} \over \partial \xi_2}{\delta \xi_2} + {\partial r_{21} \over \partial C}{\delta C} + {\partial r_{21} \over \partial \rho_1}{\delta \rho_1} + {\partial r_{21} \over \partial l_1}{\delta l_1} + {\partial r_{21} \over \partial l_2}{\delta l_2}\)
res_toZeroF與rtz相同。resApprox = res_toZeroF + rtz。
3.1.2 acc
在 AccumulatedTopHessianSSE::addPoint() 函數計算了 Hessian 矩陣。而這里的 Hessian 矩陣是存儲了兩個幀之間的相互信息，所有的信息存儲在 AccumulatedTopHessianSSE::acc 中，acc是一個數組，大小是 8*8 個，位置 (i, j) 上對應的是 i 幀與 j 幀的相互信息。
AccumulatorApprox 也就是AccumulatedTopHessianSSE::acc 變量的“基礎”類型。這個類型對應着 13x13 的矩陣。這個矩陣經過閱讀代碼，可以知道存儲的是以下信息。
代碼中的BotRight對應矩陣右下角 3x3 的分塊：
TopRight對應矩陣右上角 10x3 的分塊：
Data對應左上角 10x10 的分塊：
這個 AccumulatorApprox 中存儲的 13x13 矩陣並不是優化過程中整體的大矩陣，只是對應着窗口中兩幀之間的相互信息。注意到代碼中計算調用acc變量時是這么調用的acc[tid][htIDX]，int htIDX = r->hostIDX + r->targetIDX * nframes[tid];，不考慮tid線程編號，acc共有8*8=64個。
繼續講完 AccumulatedTopHessianSSE::addPoint 函數。
函數的目標除了計算不同幀之間的相互信息（變量acc），還需要計算每一個點對於所有 residual 的信息和。即EFPoint中的成員變量Hdd_accAF, bd_accAF, Hcd_accAF, Hdd_accLF, bd_accLF, Hcd_accLF，如果這個點是 active 點，那么設置AF相關的變量，否則設置LF相關變量，如果是 marginalize 點，清除AF相關變量的信息。這三個成員變量將用於計算逆深度的優化量。
局部變量Hdd_acc, bd_acc, Hcd_acc對應着這些EFPoint的成員變量，最后賦值到成員變量。
3.1.3 bd_acc, Hdd_acc, Hcd_acc
JI_r[0] += resApprox[i] *rJ->JIdx[0][i];
JI_r[1] += resApprox[i] *rJ->JIdx[1][i];
...
Vec2f Ji2_Jpdd = rJ->JIdx2 * rJ->Jpdd;
bd_acc += JI_r[0]*rJ->Jpdd[0] + JI_r[1]*rJ->Jpdd[1];
Hdd_acc += Ji2_Jpdd.dot(rJ->Jpdd);
Hcd_acc += rJ->Jpdc[0]*Ji2_Jpdd[0] + rJ->Jpdc[1]*Ji2_Jpdd[1];
JI_r對應 \({\partial r_{21} \over \partial x_2}^T({\partial r_{21} \over \partial \xi_1}{\delta \xi_1} + {\partial r_{21} \over \partial \xi_2}{\delta \xi_2} + {\partial r_{21} \over \partial C}{\delta C} + {\partial r_{21} \over \partial \rho_1}{\delta \rho_1} + {\partial r_{21} \over \partial l_1}{\delta l_1} + {\partial r_{21} \over \partial l_2}{\delta l_2})\)，2x1。
Ji2_Jpdd對應 \({\partial r_{21} \over \partial x_2}^T{\partial r_{21} \over \partial \rho_1}\)，2x1。
bd_acc對應（1）active 時，\({\partial r_{21} \over \partial \rho_1}^Tr_{21}\)；（2）marginalize 時，\({\partial r_{21} \over \partial \rho_1}^T({\partial r_{21} \over \partial \xi_1}{\delta \xi_1} + {\partial r_{21} \over \partial \xi_2}{\delta \xi_2} + {\partial r_{21} \over \partial C}{\delta C} + {\partial r_{21} \over \partial \rho_1}{\delta \rho_1} + {\partial r_{21} \over \partial l_1}{\delta l_1} + {\partial r_{21} \over \partial l_2}{\delta l_2})\)。1x1。
Hdd_acc對應 \({\partial r_{21} \over \partial \rho_1}^T{\partial r_{21} \over \partial \rho_1}\)，1x1。
Hcd_acc對應 \({\partial r_{21} \over \partial C}^T{\partial r_{21} \over \partial \rho_1}\)，4x1。
3.2 AccumulatedTopHessianSSE::stitchDoubleInternal()優化信息統計
循環for(int k=min;k<max;k++)循環是遍歷所有可能的 (host_frame,target_frame) 組合。
內層循環累積計算accH就不用看了，這個循環是用於累加多個線程的結果，accH就是acc[h+nframes*t]，參照 3.1。
下面的H（對應 \(H_{XX}\)）和b（對應 \(J^T_{X}r\)）的累加，使用了 EnergyFunctional::adHost 和 EnergyFunctional::adTarget。這是因為前面計算的 Jacobian 都是對相對狀態的偏導，這兩個變量存儲的是相對狀態對絕對狀態的偏導。
adHost[h+nframes*t]下標是 (t,h)，對應公式 \({\partial X_R^{(th)} \over \partial X_R^{(h)}}^T\)。
adTarget[h+nframes*t]下標是 (t,h)，對應公式 \({\partial X_R^{(th)} \over \partial X_R^{(t)}}^T\)。
\(X_R^{(i)}\) 是 i 幀的所有狀態，包括 se(3) 和 AffLight 參數，即 \(\begin{bmatrix} \xi_i \\ l_i \end{bmatrix}\)。