Comprensión y comentarios del código DSO trace traceON

Combinado con el blog de Lin Breakthrough Great God, agregó un comentario

Capturas de pantalla de lugares clave:

1. Construya la fórmula de Gauss-Newton para la derivación de la fórmula de optimización 

Se puede imitar este empuje, pero el valor inverso de la profundidad se convierte en una pequeña variable en la línea epipolar, luego la última se convierte en [dx, dy] .t ().

2. La última fórmula de posición de actualización mínima y máxima de profundidad inversa

ImmaturePointStatus ImmaturePoint::traceOn(FrameHessian* frame,//当前帧
const Mat33f &hostToFrame_KRKi,//旋转矩阵
const Vec3f &hostToFrame_Kt, //平移矩阵
const Vec2f& hostToFrame_affine,
CalibHessian* HCalib, 
bool debugPrint)
{
	if(lastTraceStatus == ImmaturePointStatus::IPS_OOB) return lastTraceStatus;

	debugPrint = false;//rand()%100==0;
	//对极线搜索最大长度限制 -------->纯纯的经验阈值啊 
	float maxPixSearch = (wG[0] + hG[0]) * setting_maxPixSearch;


//	const float stepsize = 1.0;				// stepsize for initial discrete search.
//	const int GNIterations = 3;				// max # GN iterations
//	const float GNThreshold = 0.1;				// GN stop after this stepsize.
//	const float extraSlackOnTH = 1.2;			// for energy-based outlier check, be slightly more relaxed by this factor.
//	const float slackInterval = 0.8;			// if pixel-interval is smaller than this, leave it be.
//	const float minImprovementFactor = 2;		// if pixel-interval is smaller than this, leave it be.
	// ============== project min and max. return if one of them is OOB ===================
	//hostToFrame_KRKi是两帧之间相对位姿------>是关键帧到当前帧的位姿变化
	//pr 是关键帧中的未成熟点在当前帧中坐标
	//---------------------------------------------------------------------------------------------> uMin  vMin  计算
	Vec3f pr = hostToFrame_KRKi * Vec3f(u,v, 1); //齐次坐标
	Vec3f ptpMin = pr + hostToFrame_Kt * idepth_min;
	//设逆深度idepth_min时关键帧未成熟点投影到当前帧后得到的
	//像素坐标 ------->在当前帧图像上
	
	float uMin = ptpMin[0] / ptpMin[2];
	float vMin = ptpMin[1] / ptpMin[2];

	//越界异常判断
	if(!(uMin > 4 && vMin > 4 && uMin < wG[0] - 5 && vMin < hG[0] - 5))
	{
		if(debugPrint) printf("OOB uMin %f %f - %f %f %f (id %f-%f)!\n",
				u,v,uMin, vMin,  ptpMin[2], idepth_min, idepth_max);
		lastTraceUV = Vec2f(-1, -1);
		lastTracePixelInterval = 0;
		return lastTraceStatus = ImmaturePointStatus::IPS_OOB;//点越界了
	}

	//---------------------------------------------------------------------------------------------> uMax  vMax  计算
	float dist;
	float uMax;
	float vMax;
	Vec3f ptpMax;
	
	//这段主要是为了获得uMax vMax 以及dist(对极线上最大搜索范围)
	//当该未成熟点第一次催熟的时候会进入第二个分支因为初始
	//idepth_max会被设置为NAN。否则进入第一分支
	if(std::isfinite(idepth_max))//isfinite函数用于检查其参数是否是无穷大。 有效数字返回true
	{
	//printf("----------------------\n");
		ptpMax = pr + hostToFrame_Kt*idepth_max;
		uMax = ptpMax[0] / ptpMax[2];
		vMax = ptpMax[1] / ptpMax[2];

		if(!(uMax > 4 && vMax > 4 && uMax < wG[0]-5 && vMax < hG[0]-5))
		{
			if(debugPrint) printf("OOB uMax  %f %f - %f %f!\n",u,v, uMax, vMax);
			lastTraceUV = Vec2f(-1, -1);
			lastTracePixelInterval=0;
			return lastTraceStatus = ImmaturePointStatus::IPS_OOB;//点越界了
		}

		// ============== check their distance. everything below 2px is OK (-> skip). ===================
		dist = (uMin - uMax) * (uMin - uMax) + (vMin - vMax) * (vMin - vMax);//对极线上的最大搜索距离 
		dist = sqrtf(dist);
		//小于这个阈值对极线区域太短了
		if(dist < setting_trace_slackInterval)
		{
			if(debugPrint)
				printf("TOO CERTAIN ALREADY (dist %f)!\n", dist);

			lastTraceUV = Vec2f(uMax + uMin, vMax + vMin) * 0.5;
			lastTracePixelInterval = dist;
			return lastTraceStatus = ImmaturePointStatus::IPS_SKIPPED;
		}
		assert(dist>0);
	}
	else
	{
		dist = maxPixSearch;

		// project to arbitrary depth to get direction.
		//接下来确定极线，随便设一个逆深度0.01， 得到另一个投影点的坐标
		ptpMax = pr + hostToFrame_Kt * 0.01;
		uMax = ptpMax[0] / ptpMax[2];
		vMax = ptpMax[1] / ptpMax[2];

		// direction.
		float dx = uMax - uMin;
		float dy = vMax - vMin;
		float d = 1.0f / sqrtf(dx * dx+dy * dy);

		// set to [setting_maxPixSearch].
		uMax = uMin + dist * dx * d;
		vMax = vMin + dist * dy * d;

		// may still be out!
		if(!(uMax > 4 && vMax > 4 && uMax < wG[0] - 5 && vMax < hG[0] - 5))
		{
			if(debugPrint) printf("OOB uMax-coarse %f %f %f!\n", uMax, vMax,  ptpMax[2]);
			lastTraceUV = Vec2f(-1, -1);
			lastTracePixelInterval = 0;
			return lastTraceStatus = ImmaturePointStatus::IPS_OOB;
		}
		assert(dist>0);
	}


	// set OOB if scale change too big.
	//异常处理
	if(!(idepth_min < 0 || (ptpMin[2] > 0.75 && ptpMin[2] < 1.5)))
	{
		if(debugPrint) printf("OOB SCALE %f %f %f!\n", uMax, vMax,  ptpMin[2]);
		lastTraceUV = Vec2f(-1,-1);
		lastTracePixelInterval=0;
		return lastTraceStatus = ImmaturePointStatus::IPS_OOB;
	}
	
	//* * * * * * * 至此求解出该点最大最小的x y的范围(图像坐标)* * * * * * * 在当前帧上

	
/*计算像素中的结果的误差范围。 如果新的间隔不是旧的间隔的
至少1/2，则跳过*/
	// ============== compute error-bounds on result in pixel. if the new interval is not at least 1/2 of the old, SKIP ===================
//得到了极线的方向
	float dx = setting_trace_stepsize * (uMax - uMin);//x最大距离
	float dy = setting_trace_stepsize * (vMax - vMin);//y最大距离
//transpose 转置
//     gradH = dxdx  dxdy 
//                 dxdy  dydy
	float a = (Vec2f(dx, dy).transpose() * gradH * Vec2f(dx, dy));
	float b = (Vec2f(dy, -dx).transpose() * gradH * Vec2f(dy, -dx));

	//errorInPixel 是极线与梯度的点乘的平方
	//这个有文献叫什么几何误差  是LSD的公式
	//是一个经验值。其表示的是极线和梯度的夹角度量
	//如果errorInPixel大就意味着极线和梯度夹角接近90度 无对极线搜索必要
	float errorInPixel = 0.2f + 0.2f * (a + b) / a; 
	if(errorInPixel * setting_trace_minImprovementFactor > dist && std::isfinite(idepth_max))
	{
	//又一个给非成熟点赋标签的分支
		if(debugPrint)
			printf("NO SIGNIFICANT IMPROVMENT (%f)!\n", errorInPixel);
		lastTraceUV = Vec2f(uMax + uMin, vMax + vMin) * 0.5;
		lastTracePixelInterval = dist;
		return lastTraceStatus = ImmaturePointStatus::IPS_BADCONDITION;
	}

	if(errorInPixel >10) errorInPixel = 10;

	// ============== do the discrete search ===================
	//正切
	dx /= dist;
	//余切
	dy /= dist;


//依然是限制搜索范围的最大范围已定maxPixSearch
	if(dist > maxPixSearch)
	{
		uMax = uMin + maxPixSearch * dx;
		vMax = vMin + maxPixSearch * dy;
		dist = maxPixSearch;
	}

	int numSteps = 1.9999f + dist / setting_trace_stepsize;
	//取左上角2*2部分 三维到二维的旋转矩阵变换吗
	Mat22f Rplane = hostToFrame_KRKi.topLeftCorner<2,2>();
	//小数部分0~1之间的一个数
	float randShift = uMin * 1000 - floorf(uMin * 1000);
	//printf("===============randShift==%f,\n", randShift);
	//起始搜索位置从uMin，vMin 加上个随机值开始
	float ptx = uMin - randShift * dx;
	float pty = vMin - randShift * dy;

	Vec2f rotatetPattern[MAX_RES_PER_POINT];
	//计算未成熟点其周围8个伙伴在当前帧图像上的坐标
	for(int idx = 0; idx < patternNum; idx++)
		rotatetPattern[idx] = Rplane * Vec2f(patternP[idx][0], patternP[idx][1]);


	if(!std::isfinite(dx) || !std::isfinite(dy))
	{
		//printf("COUGHT INF / NAN dxdy (%f %f)!\n", dx, dx);

		lastTracePixelInterval = 0;
		lastTraceUV = Vec2f(-1, -1);
		return lastTraceStatus = ImmaturePointStatus::IPS_OOB;
	}

	float errors[100];
	float bestU=0, bestV=0, bestEnergy=1e10;
	int bestIdx=-1;
	//控制搜索次数呢
	if(numSteps >= 100) numSteps = 99;
//搜索小残差位置呢每次跳动dx dy大小的偏移
//errors   把残差值都记录了
	for(int i = 0; i < numSteps; i++)
	{
		float energy = 0;
		for(int idx = 0;idx < patternNum; idx++)//8个像素的计算
		{
		//得到插值元素
			float hitColor = getInterpolatedElement31(frame->dI,
										(float)(ptx + rotatetPattern[idx][0]),
										(float)(pty + rotatetPattern[idx][1]),
										wG[0]);

			if(!std::isfinite(hitColor)) {energy+=1e5; continue;}
			//残差论文中的公式4
			float residual = hitColor - (float)(hostToFrame_affine[0] * color[idx] + hostToFrame_affine[1]);
			float hw = fabs(residual) < setting_huberTH ? 1 : setting_huberTH / fabs(residual);
			//huber加权系数
			energy += hw * residual * residual * (2 - hw);
		}

		errors[i] = energy;
		if(energy < bestEnergy)
		{
		//最小残差出现的坐标位置 bestU   bestV
			bestU = ptx; 
			bestV = pty;
			bestEnergy = energy; 
			bestIdx = i;
		}
             //dx  dy 相当于移动1个像素的distance ，在x  y方向的移动分量。
		ptx += dx;
		pty += dy;
	}
	
//在[0，bestIdx - setting_minTraceTestRadius)与(bestIdx + setting_minTraceTestRadius, numSteps)两个区间
//找一个最小残差值记做secondBest
	// find best score outside a +-2px radius.
	//找次小残差位置呢
	float secondBest=1e10;
	for(int i = 0; i< numSteps; i++)
	{
		if((i < bestIdx - setting_minTraceTestRadius || i > bestIdx+setting_minTraceTestRadius) && errors[i] < secondBest)
			secondBest = errors[i];
	}
	
	float newQuality = secondBest / bestEnergy;
	//给quality这个值赋值的位置 在此之前给赋初值10000
	if(newQuality < quality || numSteps > 10) quality = newQuality;

	
//做高斯牛顿优化 针对最小残差位置
//最后在最小误差的位置上进行高斯牛顿优化（
//只有一个变量----->沿对极线变化量），每次迭代过程中如果误差大于前
//面得到的最小误差，就缩小优化步长重新来过，当增
//量小于一定值时停止 优化的变量值是bestU bestV
	// ============== do GN optimization ===================
	float uBak = bestU, vBak = bestV,  gnstepsize = 1, stepBack = 0;
	if(setting_trace_GNIterations>0) bestEnergy = 1e5;
	int gnStepsGood=0, gnStepsBad=0;
	for(int it = 0; it < setting_trace_GNIterations; it++)//高斯牛顿迭代
	{
		float H = 1, b = 0, energy = 0;
		for(int idx = 0; idx < patternNum; idx++)
		{
			Vec3f hitColor = getInterpolatedElement33(frame->dI,
					(float)(bestU + rotatetPattern[idx][0]),
					(float)(bestV + rotatetPattern[idx][1]), wG[0]);

			if(!std::isfinite((float)hitColor[0])) 
			{
				energy += 1e5; 
				continue;
			}
			// 常规残差计算公式  下面仿照途金戈公式40推导即可。
			//这个方程唯一变量是d(沿着对极线移动的距离)
			//残差对d求偏导分为两部分，链式法则 残差对x(图像坐标求偏导) * x
			//对d求偏导。   
			//残差对x求偏导结果为[gx， gy](点在此位置的梯度)
			//x对d求偏导 结果为[dx, dy].t() 就是此点在对极线的方向
			float residual = hitColor[0] - (hostToFrame_affine[0] * color[idx] + hostToFrame_affine[1]);
			//dResdDist 就是 J = [gx ,gy] * [dx, dy].t()
			float dResdDist = dx * hitColor[1] + dy * hitColor[2];
			//胡贝尔权重系数
			float hw = fabs(residual) < setting_huberTH ? 1 : setting_huberTH / fabs(residual);
//标准公式  J.t() * J = H       J.t() * r = b.
			H += hw * dResdDist * dResdDist;
			b += hw * residual * dResdDist;
			
			//加权后的残差值
			energy += weights[idx] * weights[idx] * hw * residual * residual * (2 - hw);
		}

		if(energy > bestEnergy)
		{
			gnStepsBad++;

			// do a smaller step from old point.
			stepBack *= 0.5;
			bestU = uBak + stepBack * dx;
			bestV = vBak + stepBack * dy;
		}
		else
		{
			gnStepsGood++;

			float step = -gnstepsize * b / H;
			if(step < -0.5) step = -0.5;
			else if(step > 0.5) step = 0.5;

			if(!std::isfinite(step)) step=0;

			uBak = bestU;
			vBak = bestV;
			stepBack = step;

			bestU += step * dx;
			bestV += step * dy;
			bestEnergy = energy;
		}

		if(fabsf(stepBack) < setting_trace_GNThreshold) break;
	}

	// ============== detect energy-based outlier. ===================
//	float absGrad0 = getInterpolatedElement(frame->absSquaredGrad[0],bestU, bestV, wG[0]);
//	float absGrad1 = getInterpolatedElement(frame->absSquaredGrad[1],bestU*0.5-0.25, bestV*0.5-0.25, wG[1]);
//	float absGrad2 = getInterpolatedElement(frame->absSquaredGrad[2],bestU*0.25-0.375, bestV*0.25-0.375, wG[2]);
//又删除一波点      ---------------------->根据残差值 
	if(!(bestEnergy < energyTH * setting_trace_extraSlackOnTH))
	{
		lastTracePixelInterval=0;
		lastTraceUV = Vec2f(-1, -1);
		if(lastTraceStatus == ImmaturePointStatus::IPS_OUTLIER)
			return lastTraceStatus = ImmaturePointStatus::IPS_OOB;
		else
			return lastTraceStatus = ImmaturePointStatus::IPS_OUTLIER;
	}

	
//重新计算一下idepth_min 与idepth_max的值  刷新一次范围
//根据梯度方向方向分为两个分支来更新idepth_min  idepth_max
//第一个分支 x方向梯度大于y方向。   如下if  ,反之如下else
//公式推导看林突破的博客就好
	// ============== set new interval ===================
	if(dx * dx > dy * dy)
	{
		idepth_min = (pr[2] * (bestU - errorInPixel * dx) - pr[0]) / (hostToFrame_Kt[0] - hostToFrame_Kt[2] * (bestU - errorInPixel * dx));
		idepth_max = (pr[2] * (bestU + errorInPixel * dx) - pr[0]) / (hostToFrame_Kt[0] - hostToFrame_Kt[2] * (bestU + errorInPixel * dx));
	}
	else
	{
		idepth_min = (pr[2] * (bestV - errorInPixel * dy) - pr[1]) / (hostToFrame_Kt[1] - hostToFrame_Kt[2] * (bestV - errorInPixel * dy));
		idepth_max = (pr[2] * (bestV + errorInPixel * dy) - pr[1]) / (hostToFrame_Kt[1] - hostToFrame_Kt[2] * (bestV + errorInPixel * dy));
	}
	if(idepth_min > idepth_max) 
		std::swap<float>(idepth_min, idepth_max);


	if(!std::isfinite(idepth_min) || !std::isfinite(idepth_max) || (idepth_max < 0))
	{
		//printf("COUGHT INF / NAN minmax depth (%f %f)!\n", idepth_min, idepth_max);
		lastTracePixelInterval=0;
		lastTraceUV = Vec2f(-1, -1);
		return lastTraceStatus = ImmaturePointStatus::IPS_OUTLIER;
	}

	lastTracePixelInterval = 2 * errorInPixel;
	lastTraceUV = Vec2f(bestU, bestV);


	if(sqrtf((u - bestU) * (u - bestU) + (v - bestV) * (v - bestV)) <= 1)
	{
		return lastTraceStatus = ImmaturePointStatus::IPS_OUTLIER;
	}
	
	return lastTraceStatus = ImmaturePointStatus::IPS_GOOD;
}