数据流角度看DSO（三）

上一篇博客介绍了对第一帧的处理，接下来看第二帧。
第二帧
1.与第一帧一样，进行图像帧相关信息的初始定义，并设置初值。

	FrameHessian* fh = new FrameHessian();
	FrameShell* shell = new FrameShell();
	shell->camToWorld = SE3(); 		// no lock required, as fh is not used anywhere yet.
	shell->aff_g2l = AffLight(0,0);
    shell->marginalizedAt = shell->id = allFrameHistory.size();
    shell->timestamp = image->timestamp;
    shell->incoming_id = id;
	fh->shell = shell;
	allFrameHistory.push_back(shell);

2.makeImages()计算梯度，梯度平方和。

	fh->ab_exposure = image->exposure_time;
    fh->makeImages(image->image, &Hcalib);

3.对第一帧进行跟踪

else if(coarseInitializer->trackFrame(fh, outputWrapper))

4.trackFrame()函数解析：
4.1 将当前帧赋给newFrame为第一次跟踪，并为第一次跟踪做准备，（此时snapped为false，上一篇博客有提到）将两帧相对位姿的平移部分置0，并且给第一帧选取的像素点相关信息设置初值。

	if(!snapped)
	{
		thisToNext.translation().setZero();
		for(int lvl=0;lvl<pyrLevelsUsed;lvl++)
		{
			int npts = numPoints[lvl];
			Pnt* ptsl = points[lvl];
			for(int i=0;i<npts;i++)
			{
				ptsl[i].iR = 1;
				ptsl[i].idepth_new = 1;
				ptsl[i].lastHessian = 0;
			}
		}
	}

4.2设置两帧之间的相对位姿变换以及由曝光时间设置两帧的光度放射变换。

SE3 refToNew_current = thisToNext;
AffLight refToNew_aff_current = thisToNext_aff;
if(firstFrame->ab_exposure>0 && newFrame->ab_exposure>0)
refToNew_aff_current = AffLight(logf(newFrame->ab_exposure / firstFrame->ab_exposure),0);

4.3金字塔跟踪模型（重点）：对金字塔每层进行跟踪，由最高层开始，构建残差进行优化。
最高层：
首先利用resetPoints(lvl);设置最高层选取的点的energy和idepth_new；pts[i].energy.setZero();pts[i].idepth_new = pts[i].idepth;，并且将坏点的逆深度为当前点的neighbours的逆深度平均值。
然后利用resOld = calcResAndGS()计算当前的残差energy和优化的H矩阵和b以及Hsc，bsc。
applyStep(lvl);应用计算的相关信息。

		pts[i].energy = pts[i].energy_new;
		pts[i].isGood = pts[i].isGood_new;
		pts[i].idepth = pts[i].idepth_new;
		pts[i].lastHessian = pts[i].lastHessian_new;

通过上述计算的矩阵信息，进行迭代增量的求解。inc迭代增量表示相对位姿增量，相对仿射变换增量（8维）。求解增量会加入lambda，有点类似与LM的求解方法。

inc = - (wM * (Hl.ldlt().solve(bl)));   迭代增量的求解

SE3 refToNew_new = SE3::exp(inc.head<6>().cast<double>()) * refToNew_current;
AffLight refToNew_aff_new = refToNew_aff_current;
refToNew_aff_new.a += inc[6];
refToNew_aff_new.b += inc[7];

利用doStep(lvl, lambda, inc);计算选取的像素点逆深度增量。
再计算并更新了所有增量之后，重新计算残差以及优化用到的矩阵信息。其中calcEC(lvl)计算像素点的逆深度energy。

Mat88f H_new, Hsc_new; Vec8f b_new, bsc_new;
Vec3f resNew = calcResAndGS(lvl, H_new, b_new, Hsc_new, bsc_new, refToNew_new, refToNew_aff_new, false);
Vec3f regEnergy = calcEC(lvl);

然后比较两次的energy，如果减小了则接受优化，并且将新计算的H矩阵和b矩阵赋值，从而实现迭代优化。反之则调整lambda的值继续，并且失败次数加1。

float eTotalNew = (resNew[0]+resNew[1]+regEnergy[1]);
float eTotalOld = (resOld[0]+resOld[1]+regEnergy[0]);
bool accept = eTotalOld > eTotalNew;
if(accept)
{
	if(resNew[1] == alphaK*numPoints[lvl])
	snapped = true;
	H = H_new; b = b_new; Hsc = Hsc_new; bsc = bsc_new; resOld = resNew;
	refToNew_aff_current = refToNew_aff_new;
	refToNew_current = refToNew_new;
	applyStep(lvl);
	optReg(lvl);
	lambda *= 0.5; fails=0;
}

退出迭代优化的条件是：增量过小，迭代次数超过该层设置的最大迭代次数，误差增大的次数超过2（即发散）。
其他层：
首先propagateDown(lvl+1);，对当前层的坏点继承其parent的逆深度信息。其他与最高层一样，也是先计算resOld，然后求解迭代增量，计算resNew,，如果energy减小则接受更新，继续迭代。如果增大则调整lambad，重新计算。

在对所有层跟踪完成之后，得到最终优化结果：

	thisToNext = refToNew_current;
	thisToNext_aff = refToNew_aff_current;
	for(int i=0;i<pyrLevelsUsed-1;i++)
		propagateUp(i);
		frameID++;
	if(!snapped) snappedAt=0;
	if(snapped && snappedAt==0)
		snappedAt = frameID;

此时：frameID=1；snappedAt=1；snapped的值根据是否接受了优化而决定。
判断能否初始化的条件为：

return snapped && frameID > snappedAt+5;

至此，trackFrame()函数全部分析完成，其中calcResAndGS()是其中优化部分的核心。
5.calcResAndGS()函数解析：
trackFrame()函数优化的变量是两帧之间的相对状态（包括位姿和光度，8维），以及选取的像素点的逆深度，因此需要求解残差关于优化变量雅克比矩阵。推导可以参考博客直接法光度误差导数推导。
calcResAndGS()函数计算了高斯牛顿方程的H，b；以及舒尔补之后Hsc，bsc。他们的构建以及推导可以参考博客DSO优化代码中的Schur Complement。
首先计算RKi t r2new_aff，将第一帧图像选取的像素点投影到当前帧，并且投影时要根据像素点属于哪层金字塔选用对应层的金字塔内参，同时会删除投影位置不好的点。

Vec3f pt = RKi * Vec3f(point->u+dx, point->v+dy, 1) + t*point->idepth_new;
			float u = pt[0] / pt[2];
			float v = pt[1] / pt[2];
			float Ku = fxl * u + cxl;
			float Kv = fyl * v + cyl;
			float new_idepth = point->idepth_new/pt[2];

注意DSO采用了像素点的pattern，因此每个像素点的残差是8维的。
残差的构建以及energy的计算：（使用可Huber权重）

Vec3f hitColor = getInterpolatedElement33(colorNew, Ku, Kv, wl);
float rlR = getInterpolatedElement31(colorRef, point->u+dx, point->v+dy, wl);
float residual = hitColor[0] - r2new_aff，[0] * rlR - r2new_aff[1];
float hw = fabs(residual) < setting_huberTH ? 1 : setting_huberTH / fabs(residual);
energy += hw *residual*residual*(2-hw);

关于优化变量的雅克比矩阵可以通过参考博客推导，然后可以构建雅克比矩阵，在构建时采用了SSE指令集加速计算。
关于第二帧的处理已经分析完毕，由于此时return 为false，达不到可以初始化的条件，因此第二帧的处理到此结束。

return snapped && frameID > snappedAt+5;

第三，四，五，六，七帧
按照笔者的理解，第3,4,5,6,7帧的处理与第2帧的处理一样，前一帧的优化结果作为后一帧的优化初值。一直到第8帧才达到满足进行初始化的条件（如果不出现意外的情况）