数据流角度看DSO（五）

第八帧
接上一篇博客，将当前帧作为关键帧进行处理。
1.获取当前帧的camToWorld，并对当前帧的状态进行setEvalPT_scaled()处理。

fh->shell->camToWorld = fh->shell->trackingRef->camToWorld * fh->shell->camToTrackingRef;
fh->setEvalPT_scaled(fh->shell->camToWorld.inverse(),fh->shell->aff_g2l);

在setEvalPT_scaled()函数里，定义了一个10维向量：initial_state，并赋值。然后利用函数setStateScaled()对向量进行处理：乘以相关参数（SCALE_xxxxx），计算了PRE_worldToCam和PRE_camToWorld。
然后利用setStateZero()函数对上一步计算的state进行处理：计算了一些扰动量以及nullspace。（这个地方处理的作用以及为什么要这样做还不太清楚。）

Vec10 initial_state = Vec10::Zero();
		initial_state[6] = aff_g2l.a;
		initial_state[7] = aff_g2l.b;
		this->worldToCam_evalPT = worldToCam_evalPT;
		setStateScaled(initial_state);
		setStateZero(this->get_state());

2.traceNewCoarse(fh);利用当前帧对所有的frameHessians的未成熟点ImmaturePoint进行跟踪，优化其逆深度。注意此时的frameHessians里只有第一帧，且其生成的未成熟点已全部加入到优化中，因此此时这个函数相当于未运行。（后面再分析）

	traceNewCoarse(fh);

3.判断当前帧是否边缘化并标记：关键帧是否边缘化的判断条件可以从论文中得到，这里不说了，若当前帧需要边缘化，则fh->flaggedForMarginalization = true; flagged++;

flagFramesForMarginalization(fh);

4.将当前帧加入到滑窗优化中，（注意，仅对关键帧执行）
当前帧加入到vector：frameHessians和allKeyFramesHistory，并利用insertFrame()加入到优化中，和setPrecalcValues()进行一些预计算。（与对一帧的处理一样，区别在于frameHessians里的帧的数量）

	fh->idx = frameHessians.size();
	frameHessians.push_back(fh);
	fh->frameID = allKeyFramesHistory.size();
	allKeyFramesHistory.push_back(fh->shell);
	ef->insertFrame(fh, &Hcalib);
	setPrecalcValues();

5.利用当前关键帧建立残差项residual.
建立过程为：遍历所有frameHessians，（里面包含当前帧，因此会跳过当前帧）遍历帧的所有pointHessians，建立类PointFrameResidual的对象r，设置r的状态，并push_back到点的residuals，利用insertResidual添加到滑窗优化中，并设置ph->lastResiduals[]。
在insertResidual();函数中，会生成类EFResidual的对象efr（每个残差对应一个efr，r->efResidual = efr;），包含残差，点，主导帧，目标帧的信息,并设置isLinearized=false;在滑窗优化中会使用到。同样按照push_back的顺序，对efr建立idx，每个点会对应多个残差项，存储到点的vector：residualsAll中，其中nResiduals表示残差项的数量。

PointFrameResidual* r = new PointFrameResidual(ph, fh1, fh);
			r->setState(ResState::IN);
			ph->residuals.push_back(r);
			ef->insertResidual(r);
			ph->lastResiduals[1] = ph->lastResiduals[0];
			ph->lastResiduals[0] = std::pair<PointFrameResidual*, ResState>(r, ResState::IN);
			numFwdResAdde+=1;

6.activatePointsMT();利用当前帧的信息优化所有frameHessians的ImmaturePoint，注意此时frameHessians里面包含了当前帧，因此在遍历的时候会跳过。同样，此时由于第一帧中没有未成熟点，此函数相当于未执行。
利用makeIDX()函数重新建立idx。

	activatePointsMT();
	ef->makeIDX();

7.后端滑窗优化
首先获取当前帧的frameEnergyTH，然后利用optimize()函数进行优化，优化结果为

fh->frameEnergyTH = frameHessians.back()->frameEnergyTH;
float rmse = optimize(setting_maxOptIterations);

optimize()函数解析：
（1）首先需要明确当前通过insertPoint ，insertFrame，insertResidual加入到优化中的数据，目前只包括第一帧，第一帧的Point，第八帧，以第一帧为主导帧，第八帧为目标帧的residual。
确定迭代次数：当前frameHessians.size()=2，迭代次数为20。

if(frameHessians.size() < 2) return 0;
if(frameHessians.size() < 3) mnumOptIts = 20;
if(frameHessians.size() < 4) mnumOptIts = 15;

（2）遍历所有帧的所有点的所有residual，将isLinearized为false（利用构造函数进行构造时，该值均为false）的残差加入到activeResiduals中。然后利用函数resetOOB()reset残差项，设置的变量有：

		state_NewEnergy = state_energy = 0;
		state_NewState = ResState::OUTLIER;
		setState(ResState::IN);

（3）计算Energy：lastEnergy，lastEnergyL，lastEnergyM，并进行相关导数的求取。

	Vec3 lastEnergy = linearizeAll(false);
	double lastEnergyL = calcLEnergy();
	double lastEnergyM = calcMEnergy();

linearizeAll(false)：定义vector：toRemove，（应该是用于边缘化，在fixLinearization=true时会用到）执行linearizeAll_Reductor()，执行setNewFrameEnergyTH();，由于fixLinearization=false后面语句不运行。
linearizeAll_Reductor()：遍历所有activeResiduals，执行：(*stats)[0] += r->linearize(&Hcalib);
linearize()：首先获取当前残差的主导帧和目标帧之间的一些预计算量，点的灰度值和权重，然后通过投影函数projectPoint()将点投影到目标帧，并判断投影位置，若不满足要求则设置：state_NewState = ResState::OOB。对满足要求的点进行导数计算：这里不仅计算了对点的逆深度的导数，相对位姿的导数，还计算了对相机内参的导数，导数推导见博客直接法光度误差导数推导和DSOwindowed optimization 代码 (1)。注意，此处计算的导数是投影点对这些优化变量的导数，并不是残差对他们的导数。接下来考虑像素点的Pattern计算残差项和energyLeft，并计算残差对优化变量的导数，并且进行用于构建高斯牛顿方程的相关计算。对energyLeft进行判断，若过大（大于std::max(host->frameEnergyTH, target->frameEnergyTH)）则设置state_NewState = ResState::OUTLIER;并设置energyLeft为两帧的frameEnergyTH的最大值。(*stats)[0]存储所有残差项计算的energyLeft之和，然后赋值给lastEnergyP。
setNewFrameEnergyTH()：设置当前帧的frameEnergyTH。
calcLEnergy()：利用ef->calcLEnergyF_MT()函数计算并返回计算值，calcMEnergy()：利用ef->calcMEnergyF()函数计算并返回。两个函数都是利用delta(前面建立的扰动)进行计算，为什么要这么做？
（4）applyRes_Reductor():利用activeResiduals[k]->applyRes(true);对state_NewState == ResState::IN的残差项进行处理：设置efResidual->isActiveAndIsGoodNEW=true;，利用takeDataF()计算JI_JI_Jd，JpJdF。
（5）debugPlotTracking();这个函数的作用不太懂。
（6）开始迭代优化，利用for循环实现。
相关初始参数设置：

double lambda = 1e-1;  float stepsize=1;
VecX previousX = VecX::Constant(CPARS+ 8*frameHessians.size(), NAN);

backupState(iteration!=0)：存储当前状态，误差发散情况下用于状态的恢复。
solveSystem(iteration, lambda)：进行优化的求解，获取迭代更新量。首先利用getNullspaces()获取nullspace，然后利用ef->solveSystemF()进行相关矩阵的求取：HA_top，bA_top，HL_top，bL_top，H_sc，b_sc。关于这一部分可以参考博客：DSO windowed optimization 代码 (2)，DSO windowed optimization 代码 (3)，DSO windowed optimization 代码 (4)。
doStepFromBackup()：进行迭代增量的更新并对迭代更新增量进行判断，得到canbreak，即增量过小即退出优化。
计算增量更新之后的energy：

		Vec3 newEnergy = linearizeAll(false);
		double newEnergyL = calcLEnergy();
		double newEnergyM = calcMEnergy();

如果energy减小则接受更新，在applyRes_Reductor()处理，并且将新计算的energy赋值给last，然后继续进行迭代，在达到迭代次数以及增量过小时退出。如果energy增大了则利用loadSateBackup();加载未更新前的状态，调整lambda的值继续迭代优化。
（7）在迭代优化之后，利用优化的结果重新计算相关量：

ewStateZero.segment<2>(6) = frameHessians.back()->get_state().segment<2>(6);
frameHessians.back()->setEvalPT(frameHessians.back()->PRE_worldToCam, newStateZero);
EFDeltaValid=false; EFAdjointsValid=false;
ef->setAdjointsF(&Hcalib);
setPrecalcValues();

（8）lastEnergy = linearizeAll(true);注意此时参数为true。因此，此函数在执行时会进行固定线性化点的操作，同时会将r->efResidual->isActive()为false的残差项加入toRemove。（猜测应该是跟边缘化有关）
（9）优化的收尾工作：判断是否跟踪失败，将setEvalPT()计算的PRE_camToWorld赋值给：camToWorld，返回给rmse的值为：sqrtf((float)(lastEnergy[0] / (patternNum*ef->resInA)))。
8.根据优化返回的结果rmse和allKeyFramesHistory.size()判断是否初始化成功。此时仅有2个关键帧，若rmse大于20*benchmark_initializerSlackFactor，则初始化失败。
9.removeOutliers();对未构成residual的残差点进行处理：利用dropPointsF()函数执行removePoint(p)，然后重新makeIDX();。

fh->pointHessiansOut.push_back(ph);
ph->efPoint->stateFlag = EFPointStatus::PS_DROP;
fh->pointHessians[i] = fh->pointHessians.back();
fh->pointHessians.pop_back();
i--;
numPointsDropped++;

10.setCoarseTrackingRef(frameHessians)：设置当前帧为下次跟踪的参考帧，并通过makeCoarseDepthL0()将目标帧是当前帧的点（即构建残差时投影到当前帧的点）优化的逆深度建立idepth[0]，weightSums[0]，然后通过对下层采样获取金字塔各层的idepth_l = idepth[lvl]和weightSums_l = weightSums[lvl]。
11.标记需要边缘化的点并对其进行边缘化操作，前面已经将未构成residual的点进行了删除，此处为了计算的实时性，会对满足一定条件的点进行边缘化处理（具体的不说了，后面会考虑写一篇专门介绍边缘化的博客）。

	flagPointsForRemoval();
	ef->dropPointsF();
	getNullspaces(ef->lastNullspaces_pose, ef->lastNullspaces_scale, ef->lastNullspaces_affA, ef->lastNullspaces_affB);
	ef->marginalizePointsF();

12.makeNewTraces(fh, 0);对当前帧利用pixelSelector->makeMaps()进行选点操作，并且生成ImmaturePoint，push_back到newFrame->immaturePoints。

int numPointsTotal = pixelSelector->makeMaps(newFrame, selectionMap,setting_desiredImmatureDensity);
ImmaturePoint* impt = new ImmaturePoint(x,y,newFrame, selectionMap[i], &Hcalib);
newFrame->immaturePoints.push_back(impt);

13.边缘化关键帧：marginalizeFrame(frameHessians[i]);，边缘化前面标记的关键帧。（同样，具体细节利用边缘化的博客来分析。）
至此，makeKeyFrame()函数全部执行完毕。