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)}}\)。
結果就出來了。
