DSO windowed optimization 代码 (3)

4 Schur Complement 部分信息计算

参考《DSO windowed optimization 公式》，Schur Complement 部分指 Hsc（(H_{X ho} H_{ ho ho}^{-1} H_{ ho X})）和 bsc（(H_{X ho} H_{ ho ho}^{-1} J_{ ho}^T r)）。

4.1 AccumulatedSCHessianSSE::addPoint()优化的局部信息计算

最终得到的 Hsc 是 68x68 的矩阵，bsc 是 68x1 的矩阵。

4.1.1 局部变量

p->HdiF对应 (left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T{partial r^{(i)} over partial ho^{(j)}} ight)^{-1})，1x1。在前面的 AccumulatedTopHessianSSE::addPoint() 已经进行了累加，而这个是一个 Scalar 量，现在只需要求一个倒数就行了。

Hcd对应 (left( sum_{i=1}^N {partial r^{(i)} over partial C}^T {partial r^{(i)} over partial ho^{(j)}} ight))，4x1。

p->bdSumF对应当前点下，所有 ({partial r_{21} over partial ho_1}^T r_{21}) 的求和，即 (left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T r^{(i)} ight))，1x1。

r1->JpJdF对应当前residual下，所有 ({partial r_{21} over partial X_{21}}^T {partial r_{21} over partial ho_1} = egin{bmatrix} {partial r_{21} over partial xi_{21}}^T{partial r_{21} over partial ho_1} \ {partial r_{21} over partial l_{21}}^T{partial r_{21} over partial ho_1}end{bmatrix}) 的和。(left( {partial r^{(i)} over partial X_{tj}}^T {partial r^{(i)} over partial ho^{(j)}} ight))，8x1。(t) 表示 target，也就是 (r^{(i)}) 联系的另外一个 frame。

4.1.2 成员变量更新

accHcc[tid].update(Hcd,Hcd,p->HdiF)是在accHcc中加上了针对当前点的Hcc，对应 (left( sum_{i=1}^N {partial r^{(i)} over partial C}^T {partial r^{(i)} over partial ho^{(j)}} ight) left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T{partial r^{(i)} over partial ho^{(j)}} ight)^{-1} left( sum_{i=1}^N {partial r^{(i)} over partial C}^T {partial r^{(i)} over partial ho^{(j)}} ight)^T)。

accbc[tid].update(Hcd, p->bdSumF * p->HdiF)是在accbc中加上了针对当前点的bc，对应 (left( sum_{i=1}^N {partial r^{(i)} over partial C}^T {partial r^{(i)} over partial ho^{(j)}} ight) left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T{partial r^{(i)} over partial ho^{(j)}} ight)^{-1} left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T r^{(i)} ight))。

注意accE, accEB, accD都是数组。

accE[tid][r1ht].update(r1->JpJdF, Hcd, p->HdiF)是在accE[r1ht]中加上了针对当前residual（target, host）的 (left( {partial r^{(k)} over partial X_{th}}^T {partial r^{(k)} over partial ho^{(j)}} ight) left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T{partial r^{(i)} over partial ho^{(j)}} ight)^{-1} left( sum_{i=1}^N {partial r^{(i)} over partial C}^T {partial r^{(i)} over partial ho^{(j)}} ight)^T)。注，当前residual的 index 是 k，联系 t, h 两个 frame。对当前点的所有 residual 求和完成之后，accE[t, h]对应 (left( sum_{i=1}^N {partial r^{(i)} over partial X_{th}}^T {partial r^{(i)} over partial ho^{(j)}} ight) left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T{partial r^{(i)} over partial ho^{(j)}} ight)^{-1} left( sum_{i=1}^N {partial r^{(i)} over partial C}^T {partial r^{(i)} over partial ho^{(j)}} ight)^T)。

accEB[tid][r1ht].update(r1->JpJdF,p->HdiF*p->bdSumF)是在accEB中加上了针对当前residual的 (left( {partial r^{(k)} over partial X_{th}}^T {partial r^{(k)} over partial ho^{(j)}} ight) left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T{partial r^{(i)} over partial ho^{(j)}} ight)^{-1} left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T r^{(i)} ight)^T)。注，当前residual的 index 是 k，联系 t, h 两个 frame。对当前点的所有 residual 求和完成之后，accEB[t, h]对应 (left( sum_{i=1}^N {partial r^{(i)} over partial X_{th}}^T {partial r^{(i)} over partial ho^{(j)}} ight) left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T{partial r^{(i)} over partial ho^{(j)}} ight)^{-1} left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T r^{(i)} ight)^T)。

accD[tid][r1ht+r2->targetIDX*nFrames2].update(r1->JpJdF, r2->JpJdF, p->HdiF)对应当前residual``r1与相同点下所有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 ho^{(j)}} ight) left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T{partial r^{(i)} over partial ho^{(j)}} ight)^{-1} left( {partial r_2 over partial X_{t_2h_1}}^T {partial r_2 over partial ho^{(j)}} ight)^T)。在对当前residual``r1累加完成之后，accD[t2,t1,h1]加上的东西是 (left( {partial r_1 over partial X_{t_1h_1}}^T {partial r_1 over partial ho^{(j)}} ight) left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T{partial r^{(i)} over partial ho^{(j)}} ight)^{-1} left( sum_{i=1}^N {partial r^{(i)} over partial X_{t_2h_1}}^T {partial r^{(i)} over partial ho^{(j)}} ight)^T)。 在对当前点累加完成之后，accD[t2,t1,h1]加上的东西是 (left( sum_{i=1}^N {partial r^{(i)} over partial X_{t_1h_1}}^T {partial r^{(i)} over partial ho^{(j)}} ight) left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T{partial r^{(i)} over partial ho^{(j)}} ight)^{-1} left( sum_{i=1}^N {partial r^{(i)} over partial X_{t_2h_1}}^T {partial r^{(i)} over partial ho^{(j)}} ight)^T)。

4.1.3 更新完成后成员变量的意义

这个更新完成是指遍历了所有点之后，请结合 AccumulatedTopHessianSSE::stitchDouble 看。

所以accHcc对应 (sum_{j=1}^M left( sum_{i=1}^N {partial r^{(i)} over partial C}^T {partial r^{(i)} over partial ho^{(j)}} ight) left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T{partial r^{(i)} over partial ho^{(j)}} ight)^{-1} left( sum_{i=1}^N {partial r^{(i)} over partial C}^T {partial r^{(i)} over partial ho^{(j)}} ight)^T)，4x4。

所以accbc对应 (sum_{j=1}^M left( sum_{i=1}^N {partial r^{(i)} over partial C}^T {partial r^{(i)} over partial ho^{(j)}} ight) left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T{partial r^{(i)} over partial ho^{(j)}} ight)^{-1} left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T r^{(i)} ight))，4x1。

所以accE[t,h]（t 行 h 列）对应 (sum_{j=1}^M left( sum_{i=1}^N {partial r^{(i)} over partial X_{th}}^T {partial r^{(i)} over partial ho^{(j)}} ight) left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T{partial r^{(i)} over partial ho^{(j)}} ight)^{-1} left( sum_{i=1}^N {partial r^{(i)} over partial C}^T {partial r^{(i)} over partial ho^{(j)}} ight)^T)，8x4。

所以accEB[t,h]对应 (sum_{j=1}^M left( sum_{i=1}^N {partial r^{(i)} over partial X_{th}}^T {partial r^{(i)} over partial ho^{(j)}} ight) left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T{partial r^{(i)} over partial ho^{(j)}} ight)^{-1} left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T r^{(i)} ight)^T)，8x1。

所以accD[t2,t1,h1]对应 (sum_{j=1}^M left( sum_{i=1}^N {partial r^{(i)} over partial X_{t_1h_1}}^T {partial r^{(i)} over partial ho^{(j)}} ight) left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T{partial r^{(i)} over partial ho^{(j)}} ight)^{-1} left( sum_{i=1}^N {partial r^{(i)} over partial X_{t_2h_1}}^T {partial r^{(i)} over partial ho^{(j)}} ight)^T)。

4.2 AccumulatedSCHessianSSE::stitchDoubleInternal()优化信息统计

下面该乘 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)，以及转置后加到对角对称位置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)对应公式 (sum_{j=1}^M left( sum_{i=1}^N {partial r^{(i)} over partial X_{t}}^T {partial r^{(i)} over partial ho^{(j)}} ight) left( sum_{i=1}^N {partial r^{(i)} over partial ho^{(j)}}^T{partial r^{(i)} over partial ho^{(j)}} ight)^{-1} left( sum_{i=1}^N {partial r^{(i)} over partial X_{h}}^T {partial r^{(i)} over partial ho^{(j)}} ight)^T)。