DSO windowed optimization 代码 (3)
4 Schur Complement 部分信息計算
參考《DSO windowed optimization 公式》，Schur Complement 部分指 Hsc（\(H_{X\rho} H_{\rho\rho}^{-1} H_{\rho X}\)）和 bsc（\(H_{X\rho} H_{\rho\rho}^{-1} J_{\rho}^T r\)）。
4.1 AccumulatedSCHessianSSE::addPoint()優(yōu)化的局部信息計算
最終得到的 Hsc 是 68x68 的矩陣，bsc 是 68x1 的矩陣。
4.1.1 局部變量
p->HdiF對應(yīng) \(\left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T{\partial r^{(i)} \over \partial \rho^{(j)}} \right)^{-1}\)，1x1。在前面的 AccumulatedTopHessianSSE::addPoint() 已經(jīng)進(jìn)行了累加，而這個是一個 Scalar 量，現(xiàn)在只需要求一個倒數(shù)就行了。
Hcd對應(yīng) \(\left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right)\)，4x1。
p->bdSumF對應(yīng)當(dāng)前點(diǎn)下，所有 \({\partial r_{21} \over \partial \rho_1}^T r_{21}\) 的求和，即 \(\left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T r^{(i)} \right)\)，1x1。
r1->JpJdF對應(yīng)當(dāng)前residual下，所有 \({\partial r_{21} \over \partial X_{21}}^T {\partial r_{21} \over \partial \rho_1} = \begin{bmatrix} {\partial r_{21} \over \partial \xi_{21}}^T{\partial r_{21} \over \partial \rho_1} \\ {\partial r_{21} \over \partial l_{21}}^T{\partial r_{21} \over \partial \rho_1}\end{bmatrix}\) 的和。\(\left( {\partial r^{(i)} \over \partial X_{tj}}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right)\)，8x1。\(t\) 表示 target，也就是 \(r^{(i)}\) 聯(lián)系的另外一個 frame。
4.1.2 成員變量更新
accHcc[tid].update(Hcd,Hcd,p->HdiF)是在accHcc中加上了針對當(dāng)前點(diǎn)的Hcc，對應(yīng) \(\left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T{\partial r^{(i)} \over \partial \rho^{(j)}} \right)^{-1} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right)^T\)。
accbc[tid].update(Hcd, p->bdSumF * p->HdiF)是在accbc中加上了針對當(dāng)前點(diǎn)的bc，對應(yīng) \(\left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T{\partial r^{(i)} \over \partial \rho^{(j)}} \right)^{-1} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T r^{(i)} \right)\)。
注意accE, accEB, accD都是數(shù)組。
accE[tid][r1ht].update(r1->JpJdF, Hcd, p->HdiF)是在accE[r1ht]中加上了針對當(dāng)前residual（target, host）的 \(\left( {\partial r^{(k)} \over \partial X_{th}}^T {\partial r^{(k)} \over \partial \rho^{(j)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T{\partial r^{(i)} \over \partial \rho^{(j)}} \right)^{-1} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right)^T\)。注，當(dāng)前residual的 index 是 k，聯(lián)系 t, h 兩個 frame。對當(dāng)前點(diǎn)的所有 residual 求和完成之后，accE[t, h]對應(yīng) \(\left( \sum_{i=1}^N {\partial r^{(i)} \over \partial X_{th}}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T{\partial r^{(i)} \over \partial \rho^{(j)}} \right)^{-1} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right)^T\)。
accEB[tid][r1ht].update(r1->JpJdF,p->HdiF*p->bdSumF)是在accEB中加上了針對當(dāng)前residual的 \(\left( {\partial r^{(k)} \over \partial X_{th}}^T {\partial r^{(k)} \over \partial \rho^{(j)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T{\partial r^{(i)} \over \partial \rho^{(j)}} \right)^{-1} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T r^{(i)} \right)^T\)。注，當(dāng)前residual的 index 是 k，聯(lián)系 t, h 兩個 frame。對當(dāng)前點(diǎn)的所有 residual 求和完成之后，accEB[t, h]對應(yīng) \(\left( \sum_{i=1}^N {\partial r^{(i)} \over \partial X_{th}}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T{\partial r^{(i)} \over \partial \rho^{(j)}} \right)^{-1} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T r^{(i)} \right)^T\)。
accD[tid][r1ht+r2->targetIDX*nFrames2].update(r1->JpJdF, r2->JpJdF, p->HdiF)對應(yīng)當(dāng)前residual``r1與相同點(diǎn)下所有residual``r2（r1, r2可相同），即 h2 == h1 兩個 residual 同 host。單個更新是在accD[t2,t1,h1]加上的東西是 \(\left( {\partial r_1 \over \partial X_{t_1h_1}}^T {\partial r_1 \over \partial \rho^{(j)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T{\partial r^{(i)} \over \partial \rho^{(j)}} \right)^{-1} \left( {\partial r_2 \over \partial X_{t_2h_1}}^T {\partial r_2 \over \partial \rho^{(j)}} \right)^T\)。在對當(dāng)前residual``r1累加完成之后，accD[t2,t1,h1]加上的東西是 \(\left( {\partial r_1 \over \partial X_{t_1h_1}}^T {\partial r_1 \over \partial \rho^{(j)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T{\partial r^{(i)} \over \partial \rho^{(j)}} \right)^{-1} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial X_{t_2h_1}}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right)^T\)。 在對當(dāng)前點(diǎn)累加完成之后，accD[t2,t1,h1]加上的東西是 \(\left( \sum_{i=1}^N {\partial r^{(i)} \over \partial X_{t_1h_1}}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T{\partial r^{(i)} \over \partial \rho^{(j)}} \right)^{-1} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial X_{t_2h_1}}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right)^T\)。
4.1.3 更新完成后成員變量的意義
這個更新完成是指遍歷了所有點(diǎn)之后，請結(jié)合 AccumulatedTopHessianSSE::stitchDouble 看。
所以accHcc對應(yīng) \(\sum_{j=1}^M \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T{\partial r^{(i)} \over \partial \rho^{(j)}} \right)^{-1} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right)^T\)，4x4。
所以accbc對應(yīng) \(\sum_{j=1}^M \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T{\partial r^{(i)} \over \partial \rho^{(j)}} \right)^{-1} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T r^{(i)} \right)\)，4x1。
所以accE[t,h]（t 行 h 列）對應(yīng) \(\sum_{j=1}^M \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial X_{th}}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T{\partial r^{(i)} \over \partial \rho^{(j)}} \right)^{-1} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right)^T\)，8x4。
所以accEB[t,h]對應(yīng) \(\sum_{j=1}^M \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial X_{th}}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T{\partial r^{(i)} \over \partial \rho^{(j)}} \right)^{-1} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T r^{(i)} \right)^T\)，8x1。
所以accD[t2,t1,h1]對應(yīng) \(\sum_{j=1}^M \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial X_{t_1h_1}}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T{\partial r^{(i)} \over \partial \rho^{(j)}} \right)^{-1} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial X_{t_2h_1}}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right)^T\)。
4.2 AccumulatedSCHessianSSE::stitchDoubleInternal()優(yōu)化信息統(tǒng)計
下面該乘 Adj(adHost, adTarget) 就乘，為了方便，我下面就不說了。
accHcc加到Hsc.block<CPARS, CPARS>(0,0)。
accbc加到bsc.head<CPARS>()。
accE[t,h]加到Hsc.block<8, CPARS>(0,t*8), Hsc.block<8, CPARS>(0,h*8)，以及轉(zhuǎn)置后加到對角對稱位置Hsc.block<CPARS, 8>(t*8,0), Hsc.block<CPARS, 8>(h*8,0)。
accEB[t,h]加到bsc.segment<8>(t*8), bsc.segment<8>(h*8)。
accD[t2,t1,h1]加到Hsc.block<8,8>(h1*8, h1*8), Hsc.block<8,8>(t1*8, t2*8), Hsc.block<8,8>(t1*8, h1*8), Hsc.block<8,8>(h1*8, t2*8)。
Hsc.block<8,8>(t, h)對應(yīng)公式 \(\sum_{j=1}^M \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial X_{t}}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(j)}}^T{\partial r^{(i)} \over \partial \rho^{(j)}} \right)^{-1} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial X_{h}}^T {\partial r^{(i)} \over \partial \rho^{(j)}} \right)^T\)。
轉(zhuǎn)載于:https://www.cnblogs.com/JingeTU/p/8586172.html
創(chuàng)作挑戰(zhàn)賽新人創(chuàng)作獎勵來咯，堅持創(chuàng)作打卡瓜分現(xiàn)金大獎總結(jié)
以上是生活随笔為你收集整理的DSO windowed optimization 代码 (3)的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。
