DSO windowed optimization 代码 (4)

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

结果就出来了。