这里写一点代码与公式的对应关系,解决 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的所代表的意义。