3 非 Schur Complement 部分信息計(jì)算

參考《DSO windowed optimization 公式》，非Schur Complement 部分指 \(H_{XX}\) 和 \(J^T_{X}r\)。

3.1 AccumulatedTopHessianSSE::addPoint()優(yōu)化的局部信息計(jì)算

EnergyFunctional::accumulateAF_MT() 與 EnergyFunctional::accumulateLF_MT() 遍歷每一個(gè)點(diǎn)，對(duì)每一個(gè)點(diǎn)調(diào)用 AccumulatedTopHessianSSE::addPoint()。在 AccumulatedTopHessianSSE::addPoint() 中遍歷點(diǎn)的每一個(gè) residual。計(jì)算所有優(yōu)化系統(tǒng)的信息，存儲(chǔ)在每個(gè)點(diǎn)的局部變量和 EnergyFunctional 的局部變量中。

3.1.1 resApprox

首先搞定resApprox。由 VecNRf 可知，這東西是 8x1 的矩陣（也就是每個(gè) residual 都是八個(gè)像素點(diǎn)的組合）。

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 的情況比較復(fù)雜，res_toZeroF 在EFResidual::fixLinearizationF()賦值，而 res_toZeroF 與下面計(jì)算的 rtz 是類似的。

linearized 在這里已經(jīng)給出了其賦值的方法，下面會(huì)說到，linearized residual 是不存在的。

所謂的 linearied residual 是指 EFResidual::isActive() 與 EFResidual::isLinearized 都為 true 的 Residual。初始階段 isLinearized 為 false，只要搞清楚 isLinearized 在什么時(shí)候設(shè)置為 true 就可以了解到 linearized residual 是何種意思。查找了 EFResidual::isLinearized 只在 EFResidual::fixLinearizationF 中設(shè)置為 true，而 EFResidual::fixLinearizationF() 僅僅只在 FullSystem::flagPointsForRemoval() 中調(diào)用。在此處，將那些符合 2 種情況（1. 因?yàn)?residual 太少造成了 Out Of Boundary（這里考慮到將要被 marginalize 掉的幀的影響），2. 主幀要被 marginalize 掉）的點(diǎn)的 residual 設(shè)置為 linearized。但是這些點(diǎn)緊接著又會(huì)在 EnergyFunctional::marginalizePointsF() 中被 marg 掉，被刪除掉。最終也沒有進(jìn)入 FullSystem::optimize() 的優(yōu)化過程中。我在 AccumulatedTopHessianSSE::addPoint() 的這個(gè)位置設(shè)置了 conditional breakpoint (mode==1)，或者assert(mode!=1)，實(shí)驗(yàn)證明 linearized residual 是不存在的。

active residual 時(shí)，resApprox對(duì)應(yīng)的就是簡單的 \(r_{21}\)。

linearized residual 時(shí)，還要看這個(gè)代碼是什么意思。

\(\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() 函數(shù)計(jì)算了 Hessian 矩陣。而這里的 Hessian 矩陣是存儲(chǔ)了兩個(gè)幀之間的相互信息，所有的信息存儲(chǔ)在 AccumulatedTopHessianSSE::acc 中，acc是一個(gè)數(shù)組，大小是 8*8 個(gè)，位置 (i, j) 上對(duì)應(yīng)的是 i 幀與 j 幀的相互信息。

AccumulatorApprox 也就是AccumulatedTopHessianSSE::acc 變量的“基礎(chǔ)”類型。這個(gè)類型對(duì)應(yīng)著 13x13 的矩陣。這個(gè)矩陣經(jīng)過閱讀代碼，可以知道存儲(chǔ)的是以下信息。

\[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對(duì)應(yīng)矩陣右下角 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對(duì)應(yīng)矩陣右上角 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對(duì)應(yīng)左上角 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}\]

這個(gè) AccumulatorApprox 中存儲(chǔ)的 13x13 矩陣并不是優(yōu)化過程中整體的大矩陣，只是對(duì)應(yīng)著窗口中兩幀之間的相互信息。注意到代碼中計(jì)算調(diào)用acc變量時(shí)是這么調(diào)用的acc[tid][htIDX]，int htIDX = r->hostIDX + r->targetIDX * nframes[tid];，不考慮tid線程編號(hào)，acc共有8*8=64個(gè)。

繼續(xù)講完 AccumulatedTopHessianSSE::addPoint 函數(shù)。

函數(shù)的目標(biāo)除了計(jì)算不同幀之間的相互信息（變量acc），還需要計(jì)算每一個(gè)點(diǎn)對(duì)于所有 residual 的信息和。即EFPoint中的成員變量Hdd_accAF, bd_accAF, Hcd_accAF, Hdd_accLF, bd_accLF, Hcd_accLF，如果這個(gè)點(diǎn)是 active 點(diǎn)，那么設(shè)置AF相關(guān)的變量，否則設(shè)置LF相關(guān)變量，如果是 marginalize 點(diǎn)，清除AF相關(guān)變量的信息。這三個(gè)成員變量將用于計(jì)算逆深度的優(yōu)化量。

局部變量Hdd_acc, bd_acc, Hcd_acc對(duì)應(yīng)著這些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對(duì)應(yīng) \({\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對(duì)應(yīng) \({\partial r_{21} \over \partial x_2}^T{\partial r_{21} \over \partial \rho_1}\)，2x1。

bd_acc對(duì)應(yīng)（1）active 時(shí)，\({\partial r_{21} \over \partial \rho_1}^Tr_{21}\)；（2）marginalize 時(shí)，\({\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對(duì)應(yīng) \({\partial r_{21} \over \partial \rho_1}^T{\partial r_{21} \over \partial \rho_1}\)，1x1。

Hcd_acc對(duì)應(yīng) \({\partial r_{21} \over \partial C}^T{\partial r_{21} \over \partial \rho_1}\)，4x1。

3.2 AccumulatedTopHessianSSE::stitchDoubleInternal()優(yōu)化信息統(tǒng)計(jì)

循環(huán)for(int k=min;k<max;k++)循環(huán)是遍歷所有可能的 (host_frame,target_frame) 組合。

內(nèi)層循環(huán)累積計(jì)算accH就不用看了，這個(gè)循環(huán)是用于累加多個(gè)線程的結(jié)果，accH就是acc[h+nframes*t]，參照 3.1。

下面的H（對(duì)應(yīng) \(H_{XX}\)）和b（對(duì)應(yīng) \(J^T_{X}r\)）的累加，使用了 EnergyFunctional::adHost 和 EnergyFunctional::adTarget。這是因?yàn)榍懊嬗?jì)算的 Jacobian 都是對(duì)相對(duì)狀態(tài)的偏導(dǎo)，這兩個(gè)變量存儲(chǔ)的是相對(duì)狀態(tài)對(duì)絕對(duì)狀態(tài)的偏導(dǎo)。

adHost[h+nframes*t]下標(biāo)是 (t,h)，對(duì)應(yīng)公式 \({\partial X_R^{(th)} \over \partial X_R^{(h)}}^T\)。

adTarget[h+nframes*t]下標(biāo)是 (t,h)，對(duì)應(yīng)公式 \({\partial X_R^{(th)} \over \partial X_R^{(t)}}^T\)。

\(X_R^{(i)}\) 是 i 幀的所有狀態(tài)，包括 se(3) 和 AffLight 參數(shù)，即 \(\begin{bmatrix} \xi_i \\ l_i \end{bmatrix}\)。

轉(zhuǎn)載于:https://www.cnblogs.com/JingeTU/p/8586163.html

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。