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 都是八個像素點的組合）。

https://github.com/JakobEngel/dso/blob/5fb2c065b1638e10bccf049a6575ede4334ba673/src/OptimizationBackend/AccumulatedTopHessian.cpp#L72

VecNRf resApprox; if(mode==0) // activeresApprox = rJ->resF; if(mode==2) // marginalizeresApprox = 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 的矩陣。這個矩陣經過閱讀代碼，可以知道存儲的是以下信息。

\[H = \begin{bmatrix} J^T \\ r^T \end{bmatrix}\begin{bmatrix} J & r \end{bmatrix} \]

\[J = \begin{bmatrix} {\partial r_{21} \over \partial C}_{8\times4} & {\partial r_{21} \over \partial \xi_{21}}_{8\times6} & {\partial r_{21} \over \partial l_{21}}_{8\times2} \end{bmatrix}_{8\times12}\]

\[r = \begin{bmatrix} r_{21} \end{bmatrix}_{8\times1}\]

\[\begin{align} H &= \begin{bmatrix} J^T \\ r^T \end{bmatrix}\begin{bmatrix} J & r \end{bmatrix}\notag \\ &= \begin{bmatrix} {\partial r_{21} \over \partial C}^T_{4\times8} \\ {\partial r_{21} \over \partial \xi_{21}}^T_{6\times8} \\ {\partial r_{21} \over \partial l_{21}}^T_{2\times8} \\ {r_{21}}^T_{1\times8} \end{bmatrix} \begin{bmatrix} {\partial r_{21} \over \partial C}_{8\times4} & {\partial r_{21} \over \partial \xi_{21}}_{8\times6} & {\partial r_{21} \over \partial l_{21}}_{8\times2} & {r_{21}}_{8\times1}\end{bmatrix} \notag \\ &= \begin{bmatrix} {{\partial r_{21} \over \partial C}^T{\partial r_{21} \over \partial C}}_{4\times4} & {{\partial r_{21} \over \partial C}^T{\partial r_{21} \over \partial \xi_{21}}}_{4\times6} & {{\partial r_{21} \over \partial C}^T{\partial r_{21} \over \partial l_{21}}}_{4\times2} & {{\partial r_{21} \over \partial C}^T{r_{21}}}_{4\times1} \\ {{\partial r_{21} \over \partial \xi_{21}}^T{\partial r_{21} \over \partial C}}_{6\times4} & {{\partial r_{21} \over \partial \xi_{21}}^T{\partial r_{21} \over \partial \xi_{21}}}_{6\times6} & {{\partial r_{21} \over \partial \xi_{21}}^T{\partial r_{21} \over \partial l_{21}}}_{6\times2} & {{\partial r_{21} \over \partial \xi_{21}}^T{r_{21}}}_{6\times1} \\ {{\partial r_{21} \over \partial l_{21}}^T{\partial r_{21} \over \partial C}}_{2\times4} & {{\partial r_{21} \over \partial l_{21}}^T{\partial r_{21} \over \partial \xi_{21}}}_{2\times6} & {{\partial r_{21} \over \partial l_{21}}^T{\partial r_{21} \over \partial l_{21}}}_{2\times2} & {{\partial r_{21} \over \partial l_{21}}^T{r_{21}}}_{2\times1} \\ {{r_{21}}^T{\partial r_{21} \over \partial C}}_{1\times4} & {{r_{21}}^T{\partial r_{21} \over \partial \xi_{21}}}_{1\times6} & {{r_{21}}^T{\partial r_{21} \over \partial l_{21}}}_{1\times2} & {{r_{21}}^T{r_{21}}}_{1\times1} \end{bmatrix} \notag \end{align}\]

代碼中的BotRight對應矩陣右下角 3x3 的分塊：

\[\begin{bmatrix} {{\partial r_{21} \over \partial l_{21}}^T{\partial r_{21} \over \partial l_{21}}}_{2\times2} & {{\partial r_{21} \over \partial l_{21}}^T{r_{21}}}_{2\times1} \\ {{r_{21}}^T{\partial r_{21} \over \partial l_{21}}}_{1\times2} & {{r_{21}}^T{r_{21}}}_{1\times1} \end{bmatrix}\]

TopRight對應矩陣右上角 10x3 的分塊：

\[\begin{bmatrix} {{\partial r_{21} \over \partial C}^T{\partial r_{21} \over \partial l_{21}}}_{4\times2} & {{\partial r_{21} \over \partial C}^T{r_{21}}}_{4\times1} \\ {{\partial r_{21} \over \partial \xi_{21}}^T{\partial r_{21} \over \partial l_{21}}}_{6\times2} & {{\partial r_{21} \over \partial \xi_{21}}^T{r_{21}}}_{6\times1} \end{bmatrix}\]

Data對應左上角 10x10 的分塊：

\[\begin{bmatrix} {{\partial r_{21} \over \partial C}^T{\partial r_{21} \over \partial C}}_{4\times4} & {{\partial r_{21} \over \partial C}^T{\partial r_{21} \over \partial \xi_{21}}}_{4\times6} \\ {{\partial r_{21} \over \partial \xi_{21}}^T{\partial r_{21} \over \partial C}}_{6\times4} & {{\partial r_{21} \over \partial \xi_{21}}^T{\partial r_{21} \over \partial \xi_{21}}}_{6\times6} \end{bmatrix}\]

這個 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

https://github.com/JakobEngel/dso/blob/5fb2c065b1638e10bccf049a6575ede4334ba673/src/OptimizationBackend/AccumulatedTopHessian.cpp#L128

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}\)。

轉載于:https://www.cnblogs.com/JingeTU/p/8586163.html

總結

以上是生活随笔為你收集整理的DSO windowed optimization 代码 (2)的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。