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 的向量。

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

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

[egin{align} J &= egin{bmatrix} partial r_{21}^{(1)} over partial ho^{(1)} & partial r_{21}^{(1)} over partial ho^{(2)} & dots & partial r_{21}^{(1)} over partial ho^{(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 ho^{(1)} & partial r_{21}^{(2)} over partial ho^{(2)} & dots & partial r_{21}^{(2)} over partial ho^{(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 ho^{(1)} & partial r_{21}^{(N)} over partial ho^{(2)} & dots & partial r_{21}^{(N)} over partial ho^{(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} otag \ & = egin{bmatrix} J_ ho & J_{x_{21}} end{bmatrix} otag end{align}]

Gauss Newton 方程可以进一步写成

[egin{bmatrix} (J_ ho^TJ_ ho)_{N imes N} & (J_ ho^TJ_{x_{21}})_{N imes 8} \ (J_{x_{21}}^TJ_ ho)_{8 imes N} & (J_{x_{21}}^TJ_{x_{21}})_{8 imes 8} end{bmatrix} egin{bmatrix} delta ho \ delta x_{21} end{bmatrix} = - egin{bmatrix} J_ ho^T r_{21} \ J_{x_{21}}^T r_{21}end{bmatrix} ]

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

Schur Complement 消除 (delta ho)：

[egin{bmatrix} H_{ ho ho} & H_{ ho x_{21}} \ 0 & H_{x_{21}x_{21}} - H_{ ho x_{21}}^TH_{ ho ho}^{-1}H_{ ho x_{21}} end{bmatrix} egin{bmatrix} delta ho \ delta x_{21} end{bmatrix} = - egin{bmatrix} J_ ho^T r_{21} \ J_{x_{21}}^T r_{21} - H_{ ho x_{21}}^TH_{ ho ho}^{-1} J_ ho^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_{ ho x_{21}}^TH_{ ho ho}^{-1}H_{ ho x_{21}}, H_{ ho x_{21}}^TH_{ ho ho}^{-1} J_ ho^T r_{21})相关。

acc9.H是9x9的矩阵：

[egin{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需要将前面的两个舒尔补矩阵进行变换：

[egin{align}H_{ ho x_{21}}^TH_{ ho ho}^{-1}H_{ ho x_{21}} &= (J_ ho^TJ_{x_{21}})^T (J_ ho^TJ_ ho)^{-1}J_ ho^TJ_{x_{21}} otag \ &= {1 over J_ ho J_ ho^T} J_{x_{21}}^TJ_ ho (J_ ho^TJ_ ho)^{-1} J_ ho^T J_ ho J_ ho^T J_{x_{21}} otag \ &= {1 over J_ ho J_ ho^T} J_{x_{21}}^TJ_ ho J_ ho^T J_{x_{21}} otag end{align}]

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

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

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

对应着({1 over J_ ho J_ ho^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 ho)：

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

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