数据流角度看DSO（二）

上一篇博客介绍到了运行前的准备，包括相关参数文件的读取，图像金字塔的建立以及内参矩阵K的计算，还有获取图像信息并进行光度校准。接上一篇博客介绍到的图像帧处理入口，以第一帧开始分析每个图像帧的处理。
第一帧
1.初始化存储图像帧信息的类，生成类FrameHessian，FrameShell的对象，并设置初值，包括：camToWorld，aff_g2l等，并将当前帧的信息加入到allFrameHistory。

	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);

makeImages()函数解析：

dIp[i] = new Eigen::Vector3f[wG[i]*hG[i]];
absSquaredGrad[i] = new float[wG[i]*hG[i]];

首先对每层金字塔初始化两个数组，数组的类型为Eigen::Vector3f和float；其中dIp[i] absSquaredGrad[i]是金字塔第i层的数组的首地址。dIp[i]数组的元素为三维向量，分别代表像素灰度值，x方向的梯度，y方向的梯度。absSquaredGrad[i]数组的元素代表两个方向的梯度平方和。
对这两个数据的调用方式如下：（在候选点选取的时候会用到此处构建的两个数组，在这里先介绍一下，能够帮助理解这两个数组是如何构建的。）

d=dIp[0];///获取金字塔第0层，若要获取其他层，修改中括号里面即可；
d[idx][0]  表示图像金字塔第0层，idx位置处的像素的像素灰度值;(这是因为DSO中存储图像像素值都是采用一维数组来表示，类似于opencv里面的data数组。)
d[idx][1]  表示图像金字塔第0层，idx位置处的像素的x方向的梯度
d[idx][2]  表示图像金字塔第0层，idx位置处的像素的y方向的梯度
abs=absSquaredGrad[1]; ///获取金字塔第1层，若要获取其他层，修改中括号里面即可；
abs[idx]   表示图像金字塔第1层，，idx位置处的像素x,y方向的梯度平方和

两个数据的构建：主要内容是图像金字塔是如何产生以及梯度的求取，（在上一篇博客中运行前的准备介绍了如何确定使用的金字塔层数。）
上层金字塔图像的像素值是由下层图像的4个像素值均匀采样得到的。下述代码中dI_l表示上层金字塔图像首地址，dI_lm表示下层图像金字塔的首地址。

dI_l[x + y*wl][0] = 0.25f * (dI_lm[2*x   + 2*y*wlm1][0] +dI_lm[2*x+1 + 2*y*wlm1][0] +dI_lm[2*x   + 2*y*wlm1+wlm1][0] +dI_lm[2*x+1 + 2*y*wlm1+wlm1][0]);

梯度的求取：利用前后两个像素的差值作为x方向的梯度，利用上下两个像素的差值作为y方向的梯度，注意会跳过边缘像素点的梯度计算。

float dx = 0.5f*(dI_l[idx+1][0] - dI_l[idx-1][0]);
float dy = 0.5f*(dI_l[idx+wl][0] - dI_l[idx-wl][0]);

最后会根据参数设置，对计算的两个方向的梯度平方和乘以一个权重。会用到函数setGammaFunction()里面计算的Hcalib.B[i]。

3.初始化操作，设置第一帧。

coarseInitializer->setFirst(&Hcalib, fh);

setFirst()函数解析：
首先利用makeK()函数计算每层图像金字塔的内参，计算方法与上一篇博客中提到的一样，并且将当前帧赋给firstFrame。

makeK(HCalib); //计算每层图像金字塔的内参，用于后续的金字塔跟踪模型
firstFrame = newFrameHessian;

然后进行像素点选取的相关类的初始实例化以及变量的初始定义。

PixelSelector sel(w[0],h[0]);
float* statusMap = new float[w[0]*h[0]];
bool* statusMapB = new bool[w[0]*h[0]];
float densities[] = {0.03,0.05,0.15,0.5,1};

接下来对第一帧进行像素点选取，利用for循环对每一层进行选点。变量npts表示选点数量，通过数组statusMap和statusMapB中的对应位置的值表示该位置的像素点是否被选取。
第0层(原始图像)：

npts = sel.makeMaps(firstFrame,statusMap,densities[lvl]*w[0]*h[0],1,false,2);

其他层：

npts = makePixelStatus(firstFrame->dIp[lvl], statusMapB, w[lvl], h[lvl], densities[lvl]*w[0]*h[0]);

在对每层图像选点之后，将点存储起来，numPoints[lvl]表示lvl层选取的像素点数量。

points[lvl] = new Pnt[npts];  ///lvl表示金字塔层数
Pnt* pl = points[lvl];  ///pl为lvl层的首地址，与makeImages()函数中类似
////选取的像素点相关值的初始化，nl为像素点的ID
pl[nl].u = x+0.1;
pl[nl].v = y+0.1;
pl[nl].idepth = 1;
pl[nl].iR = 1;
pl[nl].isGood=true;
pl[nl].energy.setZero();
pl[nl].lastHessian=0;
pl[nl].lastHessian_new=0;
pl[nl].my_type= (lvl!=0) ? 1 : statusMap[x+y*wl];

选点之后，利用makeNN()函数建立KDtree。

makeNN();

设置一些变量的值，thisToNext表示当前帧到下一帧的位姿变换，snapped frameID snappedAt这三个变量在后续判断是否跟踪了足够多的图像帧能够初始化时用到。

	thisToNext=SE3();
	snapped = false;
	frameID = snappedAt = 0;

至此，setFirst()函数的步骤已经全部分析完，其中还有几个函数下面具体分析。

4.makeMaps()函数解析：
首先利用makeHists(fh)函数计算阈值，阈值计算方法为：将图像分成32×32的块，计算出每个像素的梯度平方和，（如果大于48则按48计）利用数组hist0[]存储梯度平方和（从1-49）相同的像素个数，hist0[0]存储整个块中的像素数量。然后利用computeHistQuantil(hist0,setting_minGradHistCut)计算阈值1，阈值1是（hist0[0]×setting_minGradHistCut+0.5）依次减去梯度平方和从1-90的像素个数（为什么是到90？）直到（hist0[0]×setting_minGradHistCut+0.5）小于0,阈值1是此时的梯度平方和。若（hist0[0]×setting_minGradHistCut+0.5）一直不为0，则此时阈值1=90。
然后该块的阈值为ths[x+y*w32]=阈值1+setting_minGradHistAdd。
计算了所有的块的阈值之后，会对其进行平滑处理，得到最终每个块的阈值：thsSmoothed[x+y*w32]。
计算了阈值之后，使用以下函数进行像素点选取：（会使用到thsSmoothed[x+y*w32]）

Eigen::Vector3i n = this->select(fh, map_out,currentPotential, thFactor);

像素点选取的标准是：像素点在第0层图像的梯度平方和大于pixelTH0*thFactor，像素点在第1层图像的梯度平方和大于pixelTH1*thFactor，像素点在第2层图像的梯度平方和大于pixelTH2*thFactor。这三个条件是有先后顺序的，先判断前面，如果满足就不判断后面的条件，并且并不是满足此三个条件就会被选择，而是有资格被选择，具体下面分析。
像素点选取方法是：利用for循环实现4倍步长，2倍步长，1倍步长的遍历像素点选择满足上述像素点选取标准的像素点，然后像素点的dirNorm还需大于在当前步长区域中上一个选取的像素点dirNorm。（所以并不是选取最大的，而是要大于在当前步长区域内已选择的像素点）通过不同标准选取的像素点，其对应位置的statusMap值不一样，条件1的值为1,条件2的值为2,条件3的值为4。
最后，会对选取点的数量进行判断，然后调整步长重新选点。若选点过多，则增大步长，反之则减小步长。

5.makePixelStatus()函数解析：
利用函数gridMaxSelection<>()来选取像素点，选取标准是：sqgd > TH*TH，同样不是满足条件就会被选取。选取方法是：仅在一倍步长里面进行选取，然后满足上述条件的像素点需要在：dx，dy ，dx+dy， dx-dy这四个表达式中任意一个大于在当前步长区域上一个被选取的点。同样，也会根据选点数量来调整步长。

6.makeNN()函数解析：
每一金字塔层选取的像素点构成一个KD数，为每层的点找到最近邻的10个点。pts[i].neighbours[myidx]=ret_index[k];并且会在上一层找到该层像素点的parent，（最高层除外）pts[i].parent = ret_index[0];。用于后续提供初值，加速优化。