3 非 Schur Complement 部分信息计算
参考《DSO windowed optimization 公式》，非Schur Complement 部分指 (H_{XX}) 和 (J^T_{X}r)。
3.1 AccumulatedTopHessianSSE::addPoint()优化的局部信息计算
EnergyFunctional::accumulateAF_MT() 与 EnergyFunctional::accumulateLF_MT() 遍历每一个点，对每一个点调用 AccumulatedTopHessianSSE::addPoint()。在 AccumulatedTopHessianSSE::addPoint() 中遍历点的每一个 residual。计算所有优化系统的信息，存储在每个点的局部变量和 EnergyFunctional 的局部变量中。
3.1.1 resApprox
首先搞定resApprox。由 VecNRf 可知，这东西是 8x1 的矩阵（也就是每个 residual 都是八个像素点的组合）。
VecNRf resApprox;
if(mode==0) // active
resApprox = rJ->resF;
if(mode==2) // marginalize
resApprox = r->res_toZeroF;
if(mode==1) // linearized
{
// compute Jp*delta
__m128 Jp_delta_x = _mm_set1_ps(rJ->Jpdxi[0].dot(dp.head<6>())+rJ->Jpdc[0].dot(dc)+rJ->Jpdd[0]*dd);
__m128 Jp_delta_y = _mm_set1_ps(rJ->Jpdxi[1].dot(dp.head<6>())+rJ->Jpdc[1].dot(dc)+rJ->Jpdd[1]*dd);
__m128 delta_a = _mm_set1_ps((float)(dp[6]));
__m128 delta_b = _mm_set1_ps((float)(dp[7]));
for(int i=0;i<patternNum;i+=4)
{
// PATTERN: rtz = resF - [JI*Jp Ja]*delta.
__m128 rtz = _mm_load_ps(((float*)&r->res_toZeroF)+i);
rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JIdx))+i),Jp_delta_x));
rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JIdx+1))+i),Jp_delta_y));
rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF))+i),delta_a));
rtz = _mm_add_ps(rtz,_mm_mul_ps(_mm_load_ps(((float*)(rJ->JabF+1))+i),delta_b));
_mm_store_ps(((float*)&resApprox)+i, rtz);
}
}
Residual 有三种情况：
- active 情况最简单，直接是 residual。
- marginalize 的情况比较复杂，res_toZeroF 在EFResidual::fixLinearizationF()赋值，而 res_toZeroF 与下面计算的 rtz 是类似的。
- linearized 在这里已经给出了其赋值的方法，下面会说到，linearized residual 是不存在的。
所谓的 linearied residual 是指 EFResidual::isActive() 与 EFResidual::isLinearized 都为 true 的 Residual。初始阶段 isLinearized 为 false，只要搞清楚 isLinearized 在什么时候设置为 true 就可以了解到 linearized residual 是何种意思。查找了 EFResidual::isLinearized 只在 EFResidual::fixLinearizationF 中设置为 true，而 EFResidual::fixLinearizationF() 仅仅只在 FullSystem::flagPointsForRemoval() 中调用。在此处，将那些符合 2 种情况（1. 因为 residual 太少造成了 Out Of Boundary（这里考虑到将要被 marginalize 掉的帧的影响），2. 主帧要被 marginalize 掉）的点的 residual 设置为 linearized。但是这些点紧接着又会在 EnergyFunctional::marginalizePointsF() 中被 marg 掉，被删除掉。最终也没有进入 FullSystem::optimize() 的优化过程中。我在 AccumulatedTopHessianSSE::addPoint() 的这个位置设置了 conditional breakpoint (mode==1)，或者assert(mode!=1)，实验证明 linearized residual 是不存在的。
-
active residual 时，
resApprox对应的就是简单的 (r_{21})。 -
linearized residual 时，还要看这个代码是什么意思。
(egin{bmatrix} ext{Jp_delta_x} \ ext{Jp_delta_y} end{bmatrix} = {partial x_2 over partial xi_1}{delta xi_1} + {partial x_2 over partial xi_2}{delta xi_2} + {partial x_2 over partial C}{delta C} + {partial x_2 over partial ho_1}{delta ho_1})
(egin{bmatrix} ext{delta_a} \ ext{delta_b}end{bmatrix} = {partial l_{21} over partial l_1}{delta l_1} + {partial l_{21} over partial l_2}{delta l_2})
( ext{rtz} = {partial r_{21} over partial xi_1}{delta xi_1} + {partial r_{21} over partial xi_2}{delta xi_2} + {partial r_{21} over partial C}{delta C} + {partial r_{21} over partial ho_1}{delta ho_1} + {partial r_{21} over partial l_1}{delta l_1} + {partial r_{21} over partial l_2}{delta l_2})
res_toZeroF与rtz相同。resApprox = res_toZeroF + rtz。
3.1.2 acc
在 AccumulatedTopHessianSSE::addPoint() 函数计算了 Hessian 矩阵。而这里的 Hessian 矩阵是存储了两个帧之间的相互信息，所有的信息存储在 AccumulatedTopHessianSSE::acc 中，acc是一个数组，大小是 8*8 个，位置 (i, j) 上对应的是 i 帧与 j 帧的相互信息。
AccumulatorApprox 也就是AccumulatedTopHessianSSE::acc 变量的“基础”类型。这个类型对应着 13x13 的矩阵。这个矩阵经过阅读代码，可以知道存储的是以下信息。
代码中的BotRight对应矩阵右下角 3x3 的分块：
TopRight对应矩阵右上角 10x3 的分块：
Data对应左上角 10x10 的分块：
这个 AccumulatorApprox 中存储的 13x13 矩阵并不是优化过程中整体的大矩阵，只是对应着窗口中两帧之间的相互信息。注意到代码中计算调用acc变量时是这么调用的acc[tid][htIDX]，int htIDX = r->hostIDX + r->targetIDX * nframes[tid];，不考虑tid线程编号，acc共有8*8=64个。
继续讲完 AccumulatedTopHessianSSE::addPoint 函数。
函数的目标除了计算不同帧之间的相互信息（变量acc），还需要计算每一个点对于所有 residual 的信息和。即EFPoint中的成员变量Hdd_accAF, bd_accAF, Hcd_accAF, Hdd_accLF, bd_accLF, Hcd_accLF，如果这个点是 active 点，那么设置AF相关的变量，否则设置LF相关变量，如果是 marginalize 点，清除AF相关变量的信息。这三个成员变量将用于计算逆深度的优化量。
局部变量Hdd_acc, bd_acc, Hcd_acc对应着这些EFPoint的成员变量，最后赋值到成员变量。
3.1.3 bd_acc, Hdd_acc, Hcd_acc
JI_r[0] += resApprox[i] *rJ->JIdx[0][i];
JI_r[1] += resApprox[i] *rJ->JIdx[1][i];
...
Vec2f Ji2_Jpdd = rJ->JIdx2 * rJ->Jpdd;
bd_acc += JI_r[0]*rJ->Jpdd[0] + JI_r[1]*rJ->Jpdd[1];
Hdd_acc += Ji2_Jpdd.dot(rJ->Jpdd);
Hcd_acc += rJ->Jpdc[0]*Ji2_Jpdd[0] + rJ->Jpdc[1]*Ji2_Jpdd[1];
JI_r对应 ({partial r_{21} over partial x_2}^T({partial r_{21} over partial xi_1}{delta xi_1} + {partial r_{21} over partial xi_2}{delta xi_2} + {partial r_{21} over partial C}{delta C} + {partial r_{21} over partial
ho_1}{delta
ho_1} + {partial r_{21} over partial l_1}{delta l_1} + {partial r_{21} over partial l_2}{delta l_2}))，2x1。
Ji2_Jpdd对应 ({partial r_{21} over partial x_2}^T{partial r_{21} over partial
ho_1})，2x1。
bd_acc对应（1）active 时，({partial r_{21} over partial
ho_1}^Tr_{21})；（2）marginalize 时，({partial r_{21} over partial
ho_1}^T({partial r_{21} over partial xi_1}{delta xi_1} + {partial r_{21} over partial xi_2}{delta xi_2} + {partial r_{21} over partial C}{delta C} + {partial r_{21} over partial
ho_1}{delta
ho_1} + {partial r_{21} over partial l_1}{delta l_1} + {partial r_{21} over partial l_2}{delta l_2}))。1x1。
Hdd_acc对应 ({partial r_{21} over partial
ho_1}^T{partial r_{21} over partial
ho_1})，1x1。
Hcd_acc对应 ({partial r_{21} over partial C}^T{partial r_{21} over partial
ho_1})，4x1。
3.2 AccumulatedTopHessianSSE::stitchDoubleInternal()优化信息统计
循环for(int k=min;k<max;k++)循环是遍历所有可能的 (host_frame,target_frame) 组合。
内层循环累积计算accH就不用看了，这个循环是用于累加多个线程的结果，accH就是acc[h+nframes*t]，参照 3.1。
下面的H（对应 (H_{XX})）和b（对应 (J^T_{X}r)）的累加，使用了 EnergyFunctional::adHost 和 EnergyFunctional::adTarget。这是因为前面计算的 Jacobian 都是对相对状态的偏导，这两个变量存储的是相对状态对绝对状态的偏导。
adHost[h+nframes*t]下标是 (t,h)，对应公式 ({partial X_R^{(th)} over partial X_R^{(h)}}^T)。
adTarget[h+nframes*t]下标是 (t,h)，对应公式 ({partial X_R^{(th)} over partial X_R^{(t)}}^T)。
(X_R^{(i)}) 是 i 帧的所有状态，包括 se(3) 和 AffLight 参数，即 (egin{bmatrix} xi_i \ l_i end{bmatrix})。