5 “step”计算
参考《DSO windowed optimization 公式》，计算各个优化变量的增加量。
公式再写一下：
\[\begin{align} \begin{bmatrix} H_{\rho\rho} & H_{\rho X} \\ H_{X\rho} & H_{XX} \end{bmatrix} \begin{bmatrix} \delta \rho \\ \delta X \end{bmatrix} &= - \begin{bmatrix} J_{\rho}^T r \\ J_X^T r \end{bmatrix} \notag \\
\begin{bmatrix} H_{\rho\rho} & H_{\rho X} \\ 0 & H_{XX} - H_{X\rho} H_{\rho\rho}^{-1} H_{\rho X} \end{bmatrix} \begin{bmatrix} \delta \rho \\ \delta X \end{bmatrix} &= - \begin{bmatrix} J_{\rho}^T r \\ J_X^T r - H_{X\rho} H_{\rho\rho}^{-1} J_{\rho}^T r \end{bmatrix} \notag \end{align}\]
我们的目标是用上面的第二个方程
\[(H_{XX} - H_{X\rho} H_{\rho\rho}^{-1} H_{\rho X})\delta X = -(J_X^T r - H_{X\rho} H_{\rho\rho}^{-1} J_{\rho}^T r)
\]
计算出 \(\delta X\)，再代回第一个方程
\[H_{\rho\rho}\delta\rho+H_{\rho X}\delta X = -J_{\rho}^T r
\]
计算 \(\delta \rho\)。
5.1 \(\delta X\) 计算
这里 ldlt 计算
\[(H_{XX} - H_{X\rho} H_{\rho\rho}^{-1} H_{\rho X})(-\delta X) = J_X^T r - H_{X\rho} H_{\rho\rho}^{-1} J_{\rho}^T r
\]
的结果\(-\delta X\)。少了一个负号，所以后面在函数 EnergyFunctional::resubstituteF_MT 的计算内参增量和计算帧增量，加了一个负号。这里不要犯迷糊，一开始什么不懂的时候，认为这里 Engel 写错了。
这个计算还是很清晰的。
5.2 \(\delta \rho\) 计算
整理一下，我们要计算的方程是这个：
\[\delta\rho = -H_{\rho\rho}^{-1}(J_{\rho}^T r+H_{\rho X}\delta X)
\]
\(-H_{\rho\rho}^{-1}\) 是一个对角阵，如果看上面方程的一行，得到的结果是
\[\begin{align} {\delta \rho^{(j)}} = -\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)} + \\ \sum_{i=1}^{N} {\partial r^{(i)} \over \partial \rho^{(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} \right) \right) \notag \end{align}
\]
（如果 \(r^{(i)}\) 与 \(\rho^{(j)}\) 没有关系，导数 \({\partial r^{(i)} \over \partial \rho^{(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 \rho^{(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 \rho^{(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 \rho^{(j)}}\)。
r->JpJdF 对应 \({\partial r^{(i)} \over \partial X_{th}}^T{\partial r^{(i)} \over \partial \rho^{(j)}}\)。
结果就出来了。
