记录下来吧。
代码:
H[tid].block<8,8>(hIdx, hIdx).noalias() += EF->adHost[aidx] * accH.block<8,8>(CPARS,CPARS) * EF->adHost[aidx].transpose();
adHost为在此之前的函数求解的相对位姿增量对绝对位姿增量的偏导,以及光度参数偏导组成的8*8矩阵。
accH.block<8,8>(CPARS,CPARS)是在上一步函数中计算的,是(d(fx)/d(g))转置*(d(fx)/d(g)) g包含了位姿和仿射系数 d表示求偏导
上面那个公式最终的形式类似 (d(fx)/d(g))转置*(d(fx)/d(g)) 只不过里面的g由原来的相对位姿光度增量变成了host帧的绝对位姿增量和光度增量。
公式展开推导一下就可以看到这一步实现了偏差对相对位姿的偏导变成了偏差对绝对位姿的偏导。