DSO windowed optimization 代码 (2)

这里写一点代码与公式的对应关系，解决 Schur Complement 更新逆深度的细节问题。

\[\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}\]

在得到 \(\delta X\) 之后可以计算 \(\delta \rho\)，这个计算过程是每一个逆深度分别计算，因为矩阵实在是很大，直接计算无法求逆。

\[\begin{align} H_{\rho\rho}\delta \rho + H_{\rho X} \delta X &= -J_\rho^Tr \notag \H_{\rho\rho}\delta \rho &= - J_\rho^Tr - H_{\rho X} \delta X \notag \\left({\partial r \over \partial \rho}\right)^T {\partial r \over \partial \rho} \delta \rho &= -\left( \left({\partial r \over \partial \rho}\right)^T r + \left({\partial r \over \partial \rho}\right)^T {\partial r \over \partial X}\delta X \right) \notag \end{align}\]

式子中各个矩阵、向量的维度如下：

\(r\) : \(N \times 1\)
\(\rho, \delta \rho\) : \(M \times 1\)
\(\delta X\) : \(68 \times 1\)
\({\partial r \over \partial \rho}\) : \(N \times M\)
\({\partial r \over \partial X}\) : \(N \times 68\)

仅考虑 \(\delta\rho\) 中的一个逆深度更新量。

下面对应代码中的 EnergyFunctional::resubstituteFPt。

由于每一个 residual 只对一个逆深度求导不为 0。 \({\partial r \over \partial \rho}\) 一共有 \(N\) 行，每一行都有且仅有一个不为 0 的数字。这样的结果造成 \(\left({\partial r \over \partial \rho}\right)^T {\partial r \over \partial \rho}\) 是一个对角阵，于是可以分别计算每一个逆深度的更新量。

\(\left({\partial r \over \partial \rho}\right)^T {\partial r \over \partial \rho}\) 对角线上的元素是 \(\sum \left({\partial r_{21} \over \partial \rho_1}\right)^T {\partial r_{21} \over \partial \rho_1}\)，这是与该逆深度对应的所有 residual 对它导数的和，对应EFPoint::Hdd_accAF+EFPoint::Hdd_accLF。对角线上元素的倒数是EFPoint::HdiF。

对应代码：

p->data->step = -b * p->HdiF;

\(\left({\partial r \over \partial \rho}\right)^T r\)对应EFPoint::bdSum_F，依旧是每个逆深度对应的 residual 相关参数求和。

对应代码：

float b = p->bdSumF;

\[\begin{align} \left({\partial r \over \partial \rho}\right)^T {\partial r \over \partial X}\delta X &= \left({\partial r \over \partial \rho}\right)^T \begin{bmatrix} {\partial r \over \partial C} &{\partial r \over \partial X_F}\end{bmatrix} \begin{bmatrix} \delta C \\ \delta X_F \end{bmatrix} \notag \&= \left({\partial r \over \partial \rho}\right)^T \left( {\partial r \over \partial C} \delta C + {\partial r \over \partial X_F} \delta X_F \right)\notag \&= \left({\partial r \over \partial \rho}\right)^T {\partial r \over \partial C} \delta C + \left({\partial r \over \partial \rho}\right)^T {\partial r \over \partial X_F} \delta X_F \notag \end{align}\]

\(X_F\) 表示所有帧的参数，共 64 个，其中包含 se(3) 也包括 affLight。

\(\left({\partial r \over \partial \rho}\right)^T {\partial r \over \partial C} \delta C\) 对应代码：

b += xc.dot(p->Hcd_accAF + p->Hcd_accLF);

（我觉得Engel博士写了错误的代码，上面的代码是按照我推出的公式写的。）

\(\left({\partial r \over \partial \rho}\right)^T {\partial r \over \partial X_F} \delta X_F\) 对应代码：

b += xAd[r->hostIDX * nFrames + r->targetIDX] * r->JpJdF;

其中r->JpJdF表示\(\left( {r_{21} \over \partial X_{21}} \right)^T {\partial r_{21} \over \partial \rho_1}\)，转置一下不是很大关系，因为这个就是一个scalar。

xAd[r->hostIDX * nFrames + r->targetIDX]在 EnergyFunctional::resubstituteF_MT 中构造，对应的公式是

\[\delta X_1^T \left( {\partial X_{21}\over \partial X_1}\right)^T + \delta X_2^T \left( {\partial X_{21}\over \partial X_2}\right)^T\]

所以，最终\(\left({\partial r \over \partial \rho}\right)^T {\partial r \over \partial X_F} \delta X_F\)对于每一个逆深度而言是（做了一次转置）
\[\sum \left( \delta X_1^T \left( {\partial X_{21}\over \partial X_1}\right)^T + \delta X_2^T \left( {\partial X_{21}\over \partial X_2}\right)^T \right)^T \left( {r_{21} \over \partial X_{21}} \right)^T {\partial r_{21} \over \partial \rho_1}\]

回溯到 EnergyFunctional::resubstituteF_MT 函数中，确定一下adHostF和adTargetF的所代表的意义。

adHostF[h->idx + nFrames * t->idx]对应\(\left( {\partial X_{21}\over \partial X_1}\right)^T\)，按照这个下标可以看做是第t行第h列，也就是adHost[2, 1]（用我喜欢的 21 表示 target host）。

adTargetF[h->idx + nFrames * t->idx]对应\(\left( {\partial X_{21}\over \partial X_2}\right)^T\)。