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