DSO 优化代码中的 Schur Complement

接上一篇博客《直接法光度误差导数推导》，DSO 代码中 CoarseInitializer::trackFrame 目的是优化两帧（ref frame 和 new frame）之间的相对状态和 ref frame 中所有点的逆深度。

在代码中出现了变量Hsc和变量bsc，其中的"sc"是指 Schur Complement。依据这个事实就能够确定整个优化过程的所有细节。

一下假设 ref frame 上需要优化逆深度的点共有 N 个。

首先构建 Gauss Newton 方程，需要优化的参数共有 N + 8 个。

\[J^TJ \delta x = - J^T r_{21}
\]

其中\(\delta x\)是一个(N+8)x1的向量，\(J\) 是 Nx(N+8) 的矩阵，第 i 行表示\(r_{21}^{(i)}\) 对\(\delta x\)的导数（求导得到的结果就是“横着”的），\(r_{21}\)是 Nx1 的向量。

\[\begin{align}\delta x &= \begin{bmatrix} \delta \rho^{(1)} & \delta \rho^{(2)} & \dots & \delta \rho^{(N)} & \delta \xi_{21}^{(1)} & \delta \xi_{21}^{(2)} & \dots & \delta \xi_{21}^{(6)} & \delta a_{21} & \delta b_{21} \end{bmatrix}^T \notag \\
& = \begin{bmatrix} \delta \rho^T & \delta x_{21}^T \end{bmatrix}^T\notag \end{align}\]

\(x_{21}\) 表示量帧之间的相对状态转换，包括 8 个参数。

\[\begin{align} J &= \begin{bmatrix} \partial r_{21}^{(1)} \over \partial \rho^{(1)} & \partial r_{21}^{(1)} \over \partial \rho^{(2)} & \dots & \partial r_{21}^{(1)} \over \partial \rho^{(N)} & \partial r_{21}^{(1)} \over \partial \xi_{21}^{(1)} & \partial r_{21}^{(1)} \over \partial \xi_{21}^{(2)} & \dots & \partial r_{21}^{(1)} \over \partial \xi_{21}^{(6)} & \partial r_{21}^{(1)} \over \partial a_{21} & \partial r_{21}^{(1)} \over \partial b_{21} \\
\partial r_{21}^{(2)} \over \partial \rho^{(1)} & \partial r_{21}^{(2)} \over \partial \rho^{(2)} & \dots & \partial r_{21}^{(2)} \over \partial \rho^{(N)} & \partial r_{21}^{(2)} \over \partial \xi_{21}^{(1)} & \partial r_{21}^{(2)} \over \partial \xi_{21}^{(2)} & \dots & \partial r_{21}^{(2)} \over \partial \xi_{21}^{(6)} & \partial r_{21}^{(2)} \over \partial a_{21} & \partial r_{21}^{(2)} \over \partial b_{21} \\
\vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots \\
\partial r_{21}^{(N)} \over \partial \rho^{(1)} & \partial r_{21}^{(N)} \over \partial \rho^{(2)} & \dots & \partial r_{21}^{(N)} \over \partial \rho^{(N)} & \partial r_{21}^{(N)} \over \partial \xi_{21}^{(1)} & \partial r_{21}^{(N)} \over \partial \xi_{21}^{(2)} & \dots & \partial r_{21}^{(N)} \over \partial \xi_{21}^{(6)} & \partial r_{21}^{(N)} \over \partial a_{21} & \partial r_{21}^{(N)} \over \partial b_{21} \end{bmatrix} \notag \\
& = \begin{bmatrix} J_\rho & J_{x_{21}} \end{bmatrix}\notag \end{align}\]

Gauss Newton 方程可以进一步写成

\[\begin{bmatrix} (J_\rho^TJ_\rho)_{N \times N} & (J_\rho^TJ_{x_{21}})_{N \times 8} \\ (J_{x_{21}}^TJ_\rho)_{8 \times N} & (J_{x_{21}}^TJ_{x_{21}})_{8 \times 8} \end{bmatrix} \begin{bmatrix} \delta \rho \\ \delta x_{21} \end{bmatrix} = - \begin{bmatrix} J_\rho^T r_{21} \\ J_{x_{21}}^T r_{21}\end{bmatrix}
\]

\[\begin{bmatrix} H_{\rho\rho} & H_{\rho x_{21}} \\ H_{\rho x_{21}}^T & H_{x_{21}x_{21}} \end{bmatrix} \begin{bmatrix} \delta \rho \\ \delta x_{21} \end{bmatrix} = - \begin{bmatrix} J_\rho^T r_{21} \\ J_{x_{21}}^T r_{21}\end{bmatrix}
\]

Schur Complement 消除 \(\delta \rho\)：

\[\begin{bmatrix} H_{\rho\rho} & H_{\rho x_{21}} \\ 0 & H_{x_{21}x_{21}} - H_{\rho x_{21}}^TH_{\rho\rho}^{-1}H_{\rho x_{21}} \end{bmatrix} \begin{bmatrix} \delta \rho \\ \delta x_{21} \end{bmatrix} = - \begin{bmatrix} J_\rho^T r_{21} \\ J_{x_{21}}^T r_{21} - H_{\rho x_{21}}^TH_{\rho\rho}^{-1} J_\rho^T r_{21} \end{bmatrix}
\]

CoarseInitializer::trackFrame 是调用了 CoarseIntializer::calcResAndGS 计算 Gauss Newton 相关的数值。

在函数 CoarseIntializer::calcResAndGS 中变量acc9与公式中的\(H_{x_{21}x_{21}}, J_{x_{21}}^T r_{21}\)相关，acc9SC与公式中的\(H_{\rho x_{21}}^TH_{\rho\rho}^{-1}H_{\rho x_{21}}, H_{\rho x_{21}}^TH_{\rho\rho}^{-1} J_\rho^T r_{21}\)相关。

acc9.H是9x9的矩阵：

\[\begin{bmatrix} J_{x_{21}}^TJ_{x_{21}} & J_{x_{21}}^Tr_{21} \\ J_{x_{21}} r_{21} & r_{21}r_{21} \end{bmatrix}
\]

acc9的累加在两个循环内部，循环是遍历 patternNum。

第一个循环 for(int i=0;i+3<patternNum;i+=4) 内部是 acc9.updateSSE。

acc9.updateSSE 使用了 Intel 的 Streaming SIMD Extensions (SSE)，一次操作取 4 个 float，完成 4 个不同数据、相同命令的 float 运算。这种一次 4 个数据的操作，说明了循环的 i 增量是 4。算法现在使用的 patternNum 是 8（8%4==0），所以当前循环可以完成所有 pattern 点的操作。如果 patternNum%4 != 0，剩下的余数就可以用第二个循环 for(int i=((patternNum>>2)<<2); i < patternNum; i++)，((patternNum>>2)<<2)把最后两个 bit 设置为 0，得到了小于 patternNum 的最大的 4 的倍数，这个循环每次进行 1 个float运算，把最后未循环到的 pattern 点补足。

为了理解acc9SC需要将前面的两个舒尔补矩阵进行变换：

\[\begin{align}H_{\rho x_{21}}^TH_{\rho\rho}^{-1}H_{\rho x_{21}} &= (J_\rho^TJ_{x_{21}})^T (J_\rho^TJ_\rho)^{-1}J_\rho^TJ_{x_{21}}\notag \\
&= {1 \over J_\rho J_\rho^T} J_{x_{21}}^TJ_\rho (J_\rho^TJ_\rho)^{-1} J_\rho^T J_\rho J_\rho^T J_{x_{21}} \notag \\
&= {1 \over J_\rho J_\rho^T} J_{x_{21}}^TJ_\rho J_\rho^T J_{x_{21}} \notag \end{align}\]

\[\begin{align} H_{\rho x_{21}}^TH_{\rho\rho}^{-1} J_\rho^T r_{21} &= (J_\rho^TJ_{x_{21}})^T (J_\rho^TJ_\rho)^{-1}J_\rho^Tr_{21} \notag \\
&= {1 \over J_\rho J_\rho^T} J_{x_{21}}^TJ_\rho J_\rho ^Tr_{21} \notag \end{align}\]

JbBuffer_new在 idx pattern 循环内，分别对每点的8个 pattern 的\(J_{x_{21}}^TJ_\rho, J_\rho^Tr_{21}, J_\rho J_\rho^T\)进行累加。最后在 idx 点循环完成之后，使用JbBuffer_new中的数据对acc9SC进行处理，累加得到最终的结果。在处理的过程中代码

JbBuffer_new[i][9] = 1 / (1 + JbBuffer_new[i][9]);

对应着\({1 \over J_\rho J_\rho^T}\)，分母里多了一个1，猜测是为了防止JbBuffer_new[i][9]太小造成系统不稳定。

回到函数 CoarseInitializer::trackFrame 中，在得到相关的矩阵之后，变量H, Hsc相减解算\(x_{21}\)，对Hsc加了一个权值1/(1+lambda)，并且在接受更新时降低lambda提高了Hsc的权重。这个权重相当于是\(d\)对于\(x_{21}\)的权重。

对\(x_{21}\)的更新很容易看到，对\(d\)的更新在 CoarseInitializer::doStep 中。

float b = JbBuffer[i][8] + JbBuffer[i].head<8>().dot(inc);
float step = -b * JbBuffer[i][9] / (1 + lambda);

在得到\(\delta x_{21}\)之后可以代回求\(\delta \rho\)：

\[ H_{\rho\rho}\delta \rho + H_{\rho x_{21}}\delta x_{21} = -J_\rho^Tr_{21} \\
\delta \rho = -H_{\rho\rho}^{-1}(J_\rho^Tr_{21} + H_{\rho x_{21}}\delta x_{21})\]

对于每一个点，代码中inc对应\(\delta x_{21}\)，JbBuffer[i].head<8>()对应\(H_{\rho x_{21}}\)的第 8*i 行到第 8*i+7 行，JbBuffer[i][8]对应\(J_\rho^Tr_{21}\)的第 8*i 行到第8*i+7 行，JbBuffer[i][9]对应\(H_{\rho\rho}^{-1}\)的\((i, i)\)位置上的 Scalar。