DSO学习笔记五 trackFrame函数
目录
- 一、概述
- 二、resetPoints()
- 三、calcResAndGS()
-
- 3.1 原理
-
- a、光度仿射变换
- b、光度误差
- c、待优化状态
- d、光度误差对各状态导数
- e、SSE指令集加速
- f、优化
- 3.2 代码
- 3.3 总结
- 四、其他
- 五、trackFrame函数流程
一、概述
makeImage函数对图像计算金字塔，计算所有像素梯度。
对第一帧图像执行setFirst函数，第一帧处理的结果是得到第一帧金字塔每层的特征点以及每个特征对应的最近邻。在setfirst最后，设置落在边缘三格像素以内的点为坏点，frameID设为0，snapped被设为false。
对第二帧图像执行trackFrame函数，图像的处理顺序从顶层到底层。
二、resetPoints()
对顶层图像，为顶层所有坏点重新计算逆深度。如果存在最近邻为好点，则将最近邻逆深度平均值作为新的逆深度，坏点改为好点。
三、calcResAndGS()
3.1 原理
详细推导查看直接法光度误差导数推导和DSO详解。
a、光度仿射变换
认为同一空间点X在两帧图像投影的辐照度（灰度）之间有线性关系
I 2 = a 21 I 1 + b 21 I_2= a_{21}I_1+b_{21} I2=a21I1+b21 a 21 = e a 2 Δ t 2 e a 1 Δ t 1 a_{21}=\frac{e^{a_2} \Delta t_2}{e^{a_1} \Delta t_1} a21=ea1Δt1ea2Δt2 b 21 = b 2 − a 21 b 1 b_{21}=b_2-a_{21}b_1 b21=b2−a21b1
其中每帧图像对应的参数有两个[a,b]。
b、光度误差
根据空间同一点的光度仿射变换就可以得到光度误差
r = L ( I 2 − I 2 ′ ) = w h ( I 2 [ x 2 ] − ( a 21 I 1 [ x 1 ] + b 21 ) ) r=L(I_2-I_2')=w_h(I_2[x_2]-(a_{21}I_1[x_1]+b_{21})) r=L(I2−I2′)=wh(I2[x2]−(a21I1[x1]+b21))
x1和x2由同一点X投影得到。
为优化光度误差，需要求光度误差对状态的导数。
c、待优化状态
六自由度姿态 ξ 21 \xi_{21} ξ21、光度仿射变换参数[a,b]，逆深度 ρ \rho ρ 。
d、光度误差对各状态导数
推导略。
∂ r 21 ∂ a 21 = − w h I 1 [ x 1 ] \frac{\partial r_{21}}{\partial a_{21}}=-w_{h}I_1[x_1] ∂a21∂r21=−whI1[x1] ∂ r 21 ∂ b 21 = − w h \frac{\partial r_{21}}{\partial b_{21}}=-w_{h} ∂b21∂r21=−wh ∂ r 21 ∂ ξ 21 = w h [ g x f x ρ 2 g y f y ρ 2 − g x f x ρ 2 u 2 ′ − g y f y ρ 2 v 2 ′ − g x f x u 2 ′ v 2 ′ − g y f y ( 1 + v 2 ′ 2 ) g x f x ( 1 + u 2 ′ 2 ) + g y f y u 2 ′ v 2 ′ − g x f x v 2 ′ + g y f y u 2 ′ ] T \frac{\partial r_{21}}{\partial \xi_{21}}=w_{h} \begin{bmatrix} g_xf_x\rho _2\\ g_yf_y\rho _2\\ -g_xf_x\rho_2u_2'-g_yf_y\rho_2v_2'\\ -g_xf_xu_2'v_2'-g_yf_y(1+v_2'^2)\\ g_xf_x(1+u_2'^2)+g_yf_yu_2'v_2'\\ -g_xf_xv_2'+g_yf_yu_2' \end{bmatrix} ^T ∂ξ21∂r21=wh⎣⎢⎢⎢⎢⎢⎢⎡gxfxρ2gyfyρ2−gxfxρ2u2′−gyfyρ2v2′−gxfxu2′v2′−gyfy(1+v2′2)gxfx(1+u2′2)+gyfyu2′v2′−gxfxv2′+gyfyu2′⎦⎥⎥⎥⎥⎥⎥⎤T ∂ r 21 ∂ ρ 1 = w h ( g x f x ρ 1 − 1 ρ 2 ( t 21 x − u 2 ′ t 21 z ) + g y f y ρ 1 − 1 ρ 2 ( t 21 y − v 2 ′ t 21 z ) ) \frac{\partial r_{21}}{\partial \rho_{1}}=w_{h}(g_xf_x\rho_1^{-1}\rho_2(t_{21}^x-u_2't_{21}^z)+g_yf_y\rho_1^{-1}\rho_2(t_{21}^y-v_2't_{21}^z)) ∂ρ1∂r21=wh(gxfxρ1−1ρ2(t21x−u2′t21z)+gyfyρ1−1ρ2(t21y−v2′t21z))
e、SSE指令集加速
参考c/c++ 代码中使用sse指令集加速。一般而言，使用SSE指令写代码，步骤为：
（1）使用load/set函数将数据从内存加载到SSE暂存器；
（2）使用相关SSE指令完成计算等；
（3）使用store系列函数将结果从暂存器保存到内存，供后面使用。
对EIGEN使用SSE指令集加速时，注意进行内存对齐。参考从Eigen向量化谈内存对齐和EIGEN官网。
f、优化
参考DSO 优化代码中的 Schur Complement。
首先构建高斯牛顿方程
J T J δ x = − J T r 21 J^TJ\delta x=-J^Tr_{21} JTJδx=−JTr21
这里分享一下我曾经的疑惑：如果方程两边同时左乘 J − T J^{-T} J−T，或者算舒尔补时用J展开可以吗？
这个方程其实并不是真正意义的相等，而是经过泰勒一阶近似，直接约掉J，计算误差会变大，收敛变慢。
其中
δ x = [ δ ρ δ ξ 21 ( 1 ) δ ξ 21 ( 2 ) δ ξ 21 ( 3 ) δ ξ 21 ( 4 ) δ ξ 21 ( 5 ) δ ξ 21 ( 6 ) δ a 21 δ b 21 ] T \delta x= \begin{bmatrix} \delta \rho & \delta \xi_{21}^{(1)} & \delta \xi_{21}^{(2)} & \delta \xi_{21}^{(3)} & \delta \xi_{21}^{(4)} & \delta \xi_{21}^{(5)} & \delta \xi_{21}^{(6)} & \delta a_{21} & \delta b_{21} \end{bmatrix}^T δx=[δρδξ21(1)δξ21(2)δξ21(3)δξ21(4)δξ21(5)δξ21(6)δa21δb21]T = [ δ ρ T δ x 21 T ] =\begin{bmatrix} \delta \rho ^T & \delta x_{21}^T \end{bmatrix} =[δρTδx21T]
一个特征点对应一个残差和一个逆深度，则
J = [ ∂ r 21 ∂ ρ ∂ r 21 ∂ ξ 21 ( 1 ) ∂ r 21 ∂ ξ 21 ( 2 ) ∂ r 21 ∂ ξ 21 ( 3 ) ∂ r 21 ∂ ξ 21 ( 4 ) ∂ r 21 ∂ ξ 21 ( 5 ) ∂ r 21 ∂ ξ 21 ( 6 ) ∂ r 21 ∂ a 21 ∂ r 21 ∂ b 21 ] J=\begin{bmatrix} \frac{\partial r_{21}}{\partial \rho } & \frac{\partial r_{21}}{\partial \xi_{21}^{(1)} } & \frac{\partial r_{21}}{\partial \xi_{21}^{(2)} } & \frac{\partial r_{21}}{\partial \xi_{21}^{(3)} } & \frac{\partial r_{21}}{\partial \xi_{21}^{(4)} } & \frac{\partial r_{21}}{\partial \xi_{21}^{(5)} } & \frac{\partial r_{21}}{\partial \xi_{21}^{(6)} } & \frac{\partial r_{21}}{\partial a_{21} } & \frac{\partial r_{21}}{\partial b_{21} } \end{bmatrix} J=[∂ρ∂r21∂ξ21(1)∂r21∂ξ21(2)∂r21∂ξ21(3)∂r21∂ξ21(4)∂r21∂ξ21(5)∂r21∂ξ21(6)∂r21∂a21∂r21∂b21∂r21] = [ J ρ J x 21 ] =\begin{bmatrix} J_\rho & J_{x_{21}} \end{bmatrix} =[JρJx21]
用舒尔补消除 δ ρ \delta \rho δρ，可以修改GN方程为
[ H ρ ρ H ρ x 21 0 H x 21 x 21 − H ρ x 21 T H ρ ρ − 1 H ρ x 21 ] [ δ ρ δ x 21 ] = − [ J ρ T r 21 J x 21 T r 21 − H ρ x 21 T H ρ ρ J ρ T r 21 ] \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^Tr_{21} \\ J_{x_{21}}^Tr_{21}-H_{\rho x_{21}}^TH_{\rho\rho}J_\rho^Tr_{21} \end{bmatrix} [Hρρ0Hρx21Hx21x21−Hρx21THρρ−1Hρx21][δρδx21]=−[JρTr21Jx21Tr21−Hρx21THρρJρTr21]
3.2 代码
Eigen::Vector2f r2new_aff = Eigen::Vector2f(exp(refToNew_aff.a), refToNew_aff.b);
计算从当前帧顶层图像开始。定义了光度变换参数r2new_aff
第一帧光度变换参数输入为 [ a 1 , b 1 ] = [ l o g ( Δ t 2 Δ t 1 ) , 0 ] [a_1,b_1]=[log(\frac{\Delta t_2}{\Delta t_1}),0] [a1,b1]=[log(Δt1Δt2),0]，因此 a 21 = e a 2 a_{21}=e^{a_2} a21=ea2
int dx = patternP[idx][0];//staticPattern[8][idx][0]
int dy = patternP[idx][1];
程序在setting.cpp中一共设置了10种pattern，每种pattern40个可选点，这里选择第8种前8个点。
Vec3f pt = RKi * Vec3f(point->u+dx, point->v+dy, 1) + t*point->idepth_new;
空间归一化点从第一帧到当前帧的坐标变换，转换成公式
ρ 1 ρ 2 − 1 K − 1 x 2 = p t = R 21 K − 1 x 1 + t 21 ρ 1 \rho_1\rho_2^{-1}K^{-1}x_2=pt=R_{21}K^{-1}x_1+t_{21}\rho_1 ρ1ρ2−1K−1x2=pt=R21K−1x1+t21ρ1
pl[nl].u = x+0.1;
Vec3f hitColor = getInterpolatedElement33(colorNew, Ku, Kv, wl);
进行像素插值，程序在保存点坐标时加上0.1是为了保证插值函数正常运行。
float hw = fabs(residual) < setting_huberTH ? 1 : setting_huberTH / fabs(residual);
energy += hw *residual*residual*(2-hw);
计算Huber权重，采用鲁棒核函数能避免异常值导致的目标函数增长太快的问题。Huber核写成
H ( e ) = { r 2 , ∣ r ∣ < δ r ∣ r ∣ δ ( 2 r − r ∣ r ∣ δ ) , ∣ r ∣ ≥ δ H(e)= \left\{ \begin{matrix} r^2,|r|< \delta \\ \frac{r}{|r|}\delta (2r-\frac{r}{|r|}\delta),|r| \geq \delta \end{matrix}\right. H(e)={ r2,∣r∣<δ∣r∣rδ(2r−∣r∣rδ),∣r∣≥δ h w = { 1 , ∣ r ∣ < δ δ ∣ r ∣ , ∣ r ∣ ≥ δ h_w= \left\{ \begin{matrix} 1,|r|< \delta \\ \frac{\delta }{|r|},|r| \geq \delta \end{matrix}\right. hw={ 1,∣r∣<δ∣r∣δ,∣r∣≥δ
float dxInterp = hw*hitColor[1]*fxl;
float dyInterp = hw*hitColor[2]*fyl;
dp0[idx] = new_idepth*dxInterp;
dp1[idx] = new_idepth*dyInterp;
dp2[idx] = -new_idepth*(u*dxInterp + v*dyInterp);
dp3[idx] = -u*v*dxInterp - (1+v*v)*dyInterp;
dp4[idx] = (1+u*u)*dxInterp + u*v*dyInterp;
dp5[idx] = -v*dxInterp + u*dyInterp;
dp0-5六项分别对应 ∂ r 21 ∂ ξ 21 \frac{\partial r_{21}}{\partial \xi_{21}} ∂ξ21∂r21六列。
dp6[idx] = - hw*r2new_aff[0] * rlR;
dp7[idx] = - hw*1;
前面已经知道 ∂ r 21 ∂ a 21 = − w h I 1 [ x 1 ] \frac{\partial r_{21}}{\partial a_{21}}=-w_{h}I_1[x_1] ∂a21∂r21=−whI1[x1]， ∂ a 21 ∂ a 2 = e a 2 \frac{\partial a_{21}}{\partial a_{2}}=e^{a_2} ∂a2∂a21=ea2，根据链式法则可知
∂ r 21 ∂ a 2 = − w h I 1 [ x 1 ] e a 2 \frac{\partial r_{21}}{\partial a_{2}}=-w_{h}I_1[x_1]e^{a_2} ∂a2∂r21=−whI1[x1]ea2
因此dp6，dp7分别表示 ∂ r 21 ∂ a 2 \frac{\partial r_{21}}{\partial a_{2}} ∂a2∂r21, ∂ r 21 ∂ b 2 \frac{\partial r_{21}}{\partial b_{2}} ∂b2∂r21。
float dxdd = (t[0]-t[2]*u)/pt[2];
float dydd = (t[1]-t[2]*v)/pt[2];
dd[idx] = dxInterp * dxdd + dyInterp * dydd;
由上文已经知道 p t = ρ 1 ρ 2 − 1 K − 1 x 2 pt=\rho_1\rho_2^{-1}K^{-1}x_2 pt=ρ1ρ2−1K−1x2，则 p t [ 2 ] = ρ 1 ρ 2 − 1 pt[2]=\rho_1\rho_2^{-1} pt[2]=ρ1ρ2−1，因此dd表示 ∂ r 21 ∂ ρ 1 \frac{\partial r_{21}}{\partial \rho_{1}} ∂ρ1∂r21。
float maxstep = 1.0f / Vec2f(dxdd*fxl, dydd*fyl).norm();
maxstep可以理解为移动单位像素，深度的变化。
JbBuffer_new[i][0] += dp0[idx]*dd[idx];
JbBuffer_new[i][1] += dp1[idx]*dd[idx];
JbBuffer_new[i][2] += dp2[idx]*dd[idx];
JbBuffer_new[i][3] += dp3[idx]*dd[idx];
JbBuffer_new[i][4] += dp4[idx]*dd[idx];
JbBuffer_new[i][5] += dp5[idx]*dd[idx];
JbBuffer_new[i][6] += dp6[idx]*dd[idx];
JbBuffer_new[i][7] += dp7[idx]*dd[idx];
JbBuffer_new[i][8] += r[idx]*dd[idx];
JbBuffer_new[i][9] += dd[idx]*dd[idx];
对每个点计算
[ J x 21 J ρ r 21 J ρ J ρ J ρ ] \begin{bmatrix} J_{x_{21}}J_\rho \\ r_{21}J_\rho \\ J_\rho J_\rho \end{bmatrix} ⎣⎡Jx21Jρr21JρJρJρ⎦⎤
E.updateSingle(energy);
其实就是累加，每次处理一个新的点，都累加对应的能量。点数过多时就调用shiftUp()进位，防止数据溢出。
acc9.updateSSE(
_mm_load_ps(((float*)(&dp0))+i),
_mm_load_ps(((float*)(&dp1))+i),
_mm_load_ps(((float*)(&dp2))+i),
_mm_load_ps(((float*)(&dp3))+i),
_mm_load_ps(((float*)(&dp4))+i),
_mm_load_ps(((float*)(&dp5))+i),
_mm_load_ps(((float*)(&dp6))+i),
_mm_load_ps(((float*)(&dp7))+i),
_mm_load_ps(((float*)(&r))+i));
每输入一个点，计算一次 [ J x 21 T J x 21 J x 21 T r 21 J x 21 r 21 r 21 r 21 ] \begin{bmatrix} J_{x_{21}}^TJ_{x_{21}} & J_{x_{21}}^Tr_{21}\\ J_{x_{21}}r_{21} & r_{21}r_{21} \end{bmatrix} [Jx21TJx21Jx21r21Jx21Tr21r21r21]，并累加，直到遍历所有点。程序一次提取4个float数据，因此可以分两次将特征点周围pattern内的8个点全部计算一遍。
Accumulator11 EAlpha;
EAlpha.initialize();
for(int i=0;i<npts;i++)
{
Pnt* point = ptsl+i;
//坏点
if(!point->isGood_new)
{
E.updateSingle((float)(point->energy[1]));
}
else
{
point->energy_new[1] = (point->idepth_new-1)*(point->idepth_new-1);//初始逆深度都是1，这里直接为0
E.updateSingle((float)(point->energy_new[1]));
}
}
EAlpha.finish();
float alphaEnergy = alphaW*(EAlpha.A + refToNew.translation().squaredNorm() * npts);
这里用到不同的对象E和EAlpha，应该是失误？
后面计算 ( ρ − 1 ) 2 (\rho -1)^2 (ρ−1)2，我无法确定用途，但是alphaEnergy和点的总平移量有关，和点的总逆深度有关。平移量越大，alphaEnergy越大；深度越大，alphaEnergy越大。
float alphaOpt;
if(alphaEnergy > alphaK*npts)
{
alphaOpt = 0;
alphaEnergy = alphaK*npts;
}
else
{
alphaOpt = alphaW;
}
这里设置一个阈值alphaK，如果所有点的平均[t+ ( ρ − 1 ) 2 (\rho -1)^2 (ρ−1)2]大于alphaK，也就是平移量或者辐照度变化过大，那么alphaEnergy取alphaK*npts，alphaOpt取0；如果小于，则不变。
point->lastHessian_new = JbBuffer_new[i][9];
JbBuffer_new[i][8] += alphaOpt*(point->idepth_new - 1);
JbBuffer_new[i][9] += alphaOpt;
if(alphaOpt==0)
{
JbBuffer_new[i][8] += couplingWeight*(point->idepth_new - point->iR);//权重设置为1
JbBuffer_new[i][9] += couplingWeight;
}
平移量过大时，认为平移量对逆深度的导数为1。
JbBuffer_new[i][9] = 1/(1+JbBuffer_new[i][9]);
计算 H ρ ρ − 1 H_{\rho \rho}^{-1} Hρρ−1。
acc9SC.updateSingleWeighted(
(float)JbBuffer_new[i][0],(float)JbBuffer_new[i][1],(float)JbBuffer_new[i][2],(float)JbBuffer_new[i][3],
(float)JbBuffer_new[i][4],(float)JbBuffer_new[i][5],(float)JbBuffer_new[i][6],(float)JbBuffer_new[i][7],
(float)JbBuffer_new[i][8],(float)JbBuffer_new[i][9]);
计算对称矩阵
[ H ρ x 21 T H ρ ρ − 1 H ρ x 21 H ρ x 21 T H ρ ρ − 1 J ρ T r 21 . . . r 21 J ρ J ρ T r 21 ] \begin{bmatrix} H_{\rho x_{21}}^TH_{\rho\rho}^{-1}H_{\rho x_{21}} & H_{\rho x_{21}}^TH_{\rho\rho}^{-1}J_\rho ^Tr_{21} \\ ... & r_{21}J_\rho J_\rho^Tr_{21} \end{bmatrix} [Hρx21THρρ−1Hρx21...Hρx21THρρ−1JρTr21r21JρJρTr21]
与上文updateSSE不同的是，不是提取4个数据，而是先在上面循环中累加，这里直接输入。
到这里已经可以计算出 δ x 21 \delta x_{21} δx21，再将计算的结果代入GN方程可以计算 δ ρ \delta \rho δρ。
3.3 总结
calcResAndGS函数作用是，输入当前层数和待更新状态，计算GN方程，更新状态。
四、其他
Hl = wM * Hl * wM * (0.01f/(w[lvl]*h[lvl]));
bl = wM * bl * (0.01f/(w[lvl]*h[lvl]));
程序在迭代求解过程中，wm前三项对应旋转，实际Hl前3项对应SE前三项平移，不是很明白。
乘以wm的目的应该是让待优化的状态之间尺度相差不会太大。
Vec8f inc;
if(fixAffine)//是否固定光度参数，默认true
{
inc.head<6>() = - (wM.toDenseMatrix().topLeftCorner<6,6>() * (Hl.topLeftCorner<6,6>().ldlt().solve(bl.head<6>())));
inc.tail<2>().setZero();
}
else
inc = - (wM * (Hl.ldlt().solve(bl))); //=-H^-1 * b.
这里可以选择是否优化光度变换参数a,b。
doStep(lvl, lambda, inc);
计算完 δ x \delta x δx，代入方程，计算逆深度步长 δ ρ \delta \rho δρ。
Vec3f resNew = calcResAndGS(lvl, H_new, b_new, Hsc_new, bsc_new, refToNew_new, refToNew_aff_new, false);
Vec3f regEnergy = calcEC(lvl);
再调用一次calcResAndGS(头疼)，计算状态更新后的能量、所有点总位移和深度偏差。
调用calcEC，计算逆深度相对均值IR的偏差平方和。
bool accept = eTotalOld > eTotalNew;
比较结果存入accept，如果能量降低，则更新状态，减小lambda；如果能量增加，则增大lambda。
按照看代码的顺序记录，看起来好乱…
总之先记录下来再说。
五、trackFrame函数流程
1、首先是初始化状态，包括位姿，逆深度，光度变换参数；
2、从顶层开始遍历当前帧金字塔
（1）调用resetPoints为顶层更新坏点逆深度；调用calcResAndGS计算GN方程；调用applyStep更新状态；
（2）迭代进行LM优化，直到满足迭代停止条件；
（3）对金字塔下一层，首先调用propagateDown处理当前层的坏点，计算点最近邻的平均逆深度保存到iR，再重复第2步；
3、整个金字塔计算完毕后，更新优化到最后的状态，然后调用propagateUp，用下层点更新上层点的逆深度，最后求均值；
4、如果位移足够大（snapped将被设为true），那么再优化5帧后，函数返回true；否则返回false，返回主程序。