5 “step”计算
参考《DSO windowed optimization 公式》，计算各个优化变量的增加量。
公式再写一下：
[egin{align} egin{bmatrix} H_{
ho
ho} & H_{
ho X} \ H_{X
ho} & H_{XX} end{bmatrix} egin{bmatrix} delta
ho \ delta X end{bmatrix} &= - egin{bmatrix} J_{
ho}^T r \ J_X^T r end{bmatrix}
otag \
egin{bmatrix} H_{
ho
ho} & H_{
ho X} \ 0 & H_{XX} - H_{X
ho} H_{
ho
ho}^{-1} H_{
ho X} end{bmatrix} egin{bmatrix} delta
ho \ delta X end{bmatrix} &= - egin{bmatrix} J_{
ho}^T r \ J_X^T r - H_{X
ho} H_{
ho
ho}^{-1} J_{
ho}^T r end{bmatrix}
otag end{align}]
我们的目标是用上面的第二个方程
[(H_{XX} - H_{X
ho} H_{
ho
ho}^{-1} H_{
ho X})delta X = -(J_X^T r - H_{X
ho} H_{
ho
ho}^{-1} J_{
ho}^T r)
]
计算出 (delta X)，再代回第一个方程
[H_{
ho
ho}delta
ho+H_{
ho X}delta X = -J_{
ho}^T r
]
计算 (delta
ho)。
5.1 (delta X) 计算
这里 ldlt 计算
[(H_{XX} - H_{X
ho} H_{
ho
ho}^{-1} H_{
ho X})(-delta X) = J_X^T r - H_{X
ho} H_{
ho
ho}^{-1} J_{
ho}^T r
]
的结果(-delta X)。少了一个负号，所以后面在函数 EnergyFunctional::resubstituteF_MT 的计算内参增量和计算帧增量，加了一个负号。这里不要犯迷糊，一开始什么不懂的时候，认为这里 Engel 写错了。
这个计算还是很清晰的。
5.2 (delta
ho) 计算
整理一下，我们要计算的方程是这个：
[delta
ho = -H_{
ho
ho}^{-1}(J_{
ho}^T r+H_{
ho X}delta X)
]
(-H_{
ho
ho}^{-1}) 是一个对角阵，如果看上面方程的一行，得到的结果是
[egin{align} {delta
ho^{(j)}} = -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)} + \ sum_{i=1}^{N} {partial r^{(i)} over partial
ho^{(j)}}^T left( {partial r^{(i)} over partial C} {delta C} + {partial r^{(i)} over partial X_t} {delta X_t} + {partial r^{(i)} over partial X_h} {delta X_h}
ight)
ight)
otag end{align}
]
（如果 (r^{(i)}) 与 (
ho^{(j)}) 没有关系，导数 ({partial r^{(i)} over partial
ho^{(j)}}) 为 0。）
这个计算比较麻烦，计算过程在函数 EnergyFunctional::resubstituteFPt 中，首先在 EnergyFunctional::resubstituteF_MT的这里 准备xAd数组，这个数组的[h,t]是
[-delta X_h^T frac{partial X_{th}}{partial X_h}^T - delta X_t^T frac{partial X_{th}}{partial X_t}^T
]
嗯，事先把 adjoint 导数转换准备好。
接着在这里几行计算 (delta
ho^{(j)})：
float b = p->bdSumF;
b -= xc.dot(p->Hcd_accAF + p->Hcd_accLF);
for(EFResidual* r : p->residualsAll)
{
if(!r->isActive()) continue;
b -= xAd[r->hostIDX*nFrames + r->targetIDX] * r->JpJdF;
}
p->data->step = - b*p->HdiF;
p->bdSumF 对应 (sum_{i=1}^N {partial r^{(i)} over partial
ho^{(j)}}^T r^{(i)})。
p->Hcd_accAF + p->Hcd_accLF 对应 (sum_{i=1}^N {partial r^{(i)} over partial C}^T{partial r^{(i)} over partial
ho^{(j)}})。
r->JpJdF 对应 ({partial r^{(i)} over partial X_{th}}^T{partial r^{(i)} over partial
ho^{(j)}})。
结果就出来了。