dso的FEJ部分代码

//投影点关于各个参数的导数。其是组成雅克比矩阵的元素
//有2个看一处看不懂的地方。除了相机内参和位姿用了FEJ
//对逆深度感觉也是用FEJ了呢.对光度参数和图像梯度没用FEJ能看出来
//高翔的博客是说位姿和光度用了FEJ   而宫益群的前面写的是对位姿和光度
//用了FEJ ，后面优化部分又写对相机内参和位姿用了FEJ 。前后矛盾，但是
//后面的说法和代码比较吻合。奇怪奇怪   




// 注意固定线性化点。也就是计算雅克比时求导变量的值要固定，
// 而不是用每次迭代更新以后的值去求雅克比，这就是FEJ。
// ========> 这里只有位姿和是相机参数固定了  所以它们只对他们
// 做了FEJ     ----合理了------        
// 逆深度看似用了固定的位姿去求，但是逆深度的值是在
// 随着优化时刻变化的 所以其没被固定  也就不存在FEJ了



double PointFrameResidual::linearize(CalibHessian* HCalib)
{
	state_NewEnergyWithOutlier = -1;

	if(state_state == ResState::OOB)
		{ state_NewState = ResState::OOB; return state_energy; }

	FrameFramePrecalc* precalc = &(host->targetPrecalc[target->idx]);
	float energyLeft=0;
	const Eigen::Vector3f* dIl = target->dI;
	//FEJ真正的计算位置
	//const float* const Il = target->I;
	//PRE_KRKiTll   PRE_KtTll是随着优化不断变化的位姿量
	const Mat33f &PRE_KRKiTll = precalc->PRE_KRKiTll;
	const Vec3f &PRE_KtTll = precalc->PRE_KtTll;
	//PRE_RTll_0   PRE_tTll_0是固定量在计算之前固定 不随着优化变化而变化
	// FEJ的位置
	const Mat33f &PRE_RTll_0x = precalc->PRE_RTll_0;
	const Vec3f &PRE_tTll_0 = precalc->PRE_tTll_0;
	const float * const color = point->color;
	const float * const weights = point->weights;
      // 这个仿射的系数也是随着优化在变化
	Vec2f affLL = precalc->PRE_aff_mode;
	float b0 = precalc->PRE_b0_mode;


	Vec6f d_xi_x, d_xi_y;
	Vec4f d_C_x, d_C_y;
	float d_d_x, d_d_y;
	{
		float drescale, u, v, new_idepth;
		float Ku, Kv;
		Vec3f KliP;
              //点越界直接返回
		if(!projectPoint(point->u, point->v, point->idepth_zero_scaled, 0, 0, HCalib,
				PRE_RTll_0x, PRE_tTll_0, drescale, u, v, Ku, Kv, KliP, new_idepth))
			{ state_NewState = ResState::OOB; return state_energy; }
		

		centerProjectedTo = Vec3f(Ku, Kv, new_idepth);


		// diff d_idepth
		//target帧点坐标对逆深度求偏导  这块确实不理解 逆深度也相当于FEJ了啊 不懂了
		d_d_x = drescale * (PRE_tTll_0[0] - PRE_tTll_0[2] * u) * SCALE_IDEPTH * HCalib->fxl();
		d_d_y = drescale * (PRE_tTll_0[1] - PRE_tTll_0[2] * v) * SCALE_IDEPTH * HCalib->fyl();


		// diff calib
		//target帧点坐标对相机参数求偏导
		d_C_x[2] = drescale*(PRE_RTll_0x(2, 0) * u - PRE_RTll_0x(0, 0));
		d_C_x[3] = HCalib->fxl() * drescale * (PRE_RTll_0x(2, 1) * u-PRE_RTll_0x(0, 1)) * HCalib->fyli();
		d_C_x[0] = KliP[0] * d_C_x[2];
		d_C_x[1] = KliP[1] * d_C_x[3];

		d_C_y[2] = HCalib->fyl() * drescale*(PRE_RTll_0x(2, 0) * v - PRE_RTll_0x(1, 0)) * HCalib->fxli();
		d_C_y[3] = drescale * (PRE_RTll_0x(2, 1) * v - PRE_RTll_0x(1, 1));
		d_C_y[0] = KliP[0] * d_C_y[2];
		d_C_y[1] = KliP[1] * d_C_y[3];

		d_C_x[0] = (d_C_x[0] + u) * SCALE_F;
		d_C_x[1] *= SCALE_F;
		d_C_x[2] = (d_C_x[2] + 1) * SCALE_C;
		d_C_x[3] *= SCALE_C;

		d_C_y[0] *= SCALE_F;
		d_C_y[1] = (d_C_y[1] + v) * SCALE_F;
		d_C_y[2] *= SCALE_C;
		d_C_y[3] = (d_C_y[3] + 1) * SCALE_C;

		// target帧点坐标对位姿增量求偏导  DSO追踪与优化公式56
		// 对应论文公式13 dso详解博客公式
		// 几何雅克比只用最原始的PRE_RTll_0以及PRE_tTll_0来计算偏导 属于固定了
		// PRE_RTll_0与 PRE_tTll_0
		d_xi_x[0] = new_idepth * HCalib->fxl();
		d_xi_x[1] = 0;
		d_xi_x[2] = -new_idepth * u * HCalib->fxl();
		d_xi_x[3] = -u * v * HCalib->fxl();
		d_xi_x[4] = (1 + u * u) * HCalib->fxl();
		d_xi_x[5] = -v * HCalib->fxl();

		d_xi_y[0] = 0;
		d_xi_y[1] = new_idepth * HCalib->fyl();
		d_xi_y[2] = -new_idepth * v * HCalib->fyl();
		d_xi_y[3] = -(1 + v * v) * HCalib->fyl();
		d_xi_y[4] = u * v * HCalib->fyl();
		d_xi_y[5] = u * HCalib->fyl();
	}


	{
		//位姿参数雅克比矩阵   =====>坐标对位姿求偏导
		J->Jpdxi[0] = d_xi_x;
		J->Jpdxi[1] = d_xi_y;
             //相机参数雅克比矩阵     =====>坐标对相机参数求偏导
		J->Jpdc[0] = d_C_x;
		J->Jpdc[1] = d_C_y;
              //逆深度雅克比矩阵           =====>坐标对逆深度求偏导
		J->Jpdd[0] = d_d_x;
		J->Jpdd[1] = d_d_y;

	}

	float JIdxJIdx_00 = 0, JIdxJIdx_11 = 0, JIdxJIdx_10 = 0;
	float JabJIdx_00 = 0, JabJIdx_01 = 0, JabJIdx_10 = 0, JabJIdx_11 = 0;
	float JabJab_00 = 0, JabJab_01 = 0, JabJab_11 = 0;

	float wJI2_sum = 0;
//图像坐标的偏导数 以及光度仿射变换的偏导，是对8个点求
//是像素坐标关于梯度增量 光度增量等的偏导
	for(int idx = 0; idx < patternNum; idx++)
	{
		float Ku, Kv;
		if(!projectPoint(point->u+patternP[idx][0], point->v+patternP[idx][1], point->idepth_scaled, PRE_KRKiTll, PRE_KtTll, Ku, Kv))
			{ state_NewState = ResState::OOB; return state_energy; }

		projectedTo[idx][0] = Ku;
		projectedTo[idx][1] = Kv;
		
		Vec3f hitColor = (getInterpolatedElement33(dIl, Ku, Kv, wG[0]));
		float residual = hitColor[0] - (float)(affLL[0] * color[idx] + affLL[1]);
             //点坐标对光度变化增量求偏导
		float drdA = (color[idx] - b0);
		if(!std::isfinite((float)hitColor[0]))
		{ state_NewState = ResState::OOB; return state_energy; }

		float w = sqrtf(setting_outlierTHSumComponent / (setting_outlierTHSumComponent + hitColor.tail<2>().squaredNorm()));
		w = 0.5f * (w + weights[idx]);

		float hw = fabsf(residual) < setting_huberTH ? 1 : setting_huberTH / fabsf(residual);
		//hub范数
		energyLeft += w * w * hw * residual * residual * (2 - hw);

		{
			if(hw < 1) hw = sqrtf(hw);
			hw = hw * w;

			hitColor[1]*=hw;
			hitColor[2]*=hw;


			J->resF[idx] = residual * hw;
                   //残差对图像坐标求偏导
			J->JIdx[0][idx] = hitColor[1];
			J->JIdx[1][idx] = hitColor[2];
			//残差对光度求偏导
			J->JabF[0][idx] = drdA * hw;
			J->JabF[1][idx] = hw;

			JIdxJIdx_00+=hitColor[1] * hitColor[1];
			JIdxJIdx_11+=hitColor[2] * hitColor[2];
			JIdxJIdx_10+=hitColor[1] * hitColor[2];
                    //转置计算体现在这  +表示的Σ
			JabJIdx_00+= drdA * hw * hitColor[1];
			JabJIdx_01+= drdA * hw * hitColor[2];
			JabJIdx_10+= hw * hitColor[1];
			JabJIdx_11+= hw * hitColor[2];

			JabJab_00+= drdA * hw * drdA * hw;
			JabJab_01+= drdA * hw * hw;
			JabJab_11+= hw * hw;


			wJI2_sum += hw * hw * (hitColor[1] * hitColor[1] + hitColor[2] * hitColor[2]);

			if(setting_affineOptModeA < 0) J->JabF[0][idx] = 0;
			if(setting_affineOptModeB < 0) J->JabF[1][idx] = 0;

		}
	}
       //(残差 /像素坐标)转置*(残差 /像素坐标)
	J->JIdx2(0,0) = JIdxJIdx_00;
	J->JIdx2(0,1) = JIdxJIdx_10;
	J->JIdx2(1,0) = JIdxJIdx_10;
	J->JIdx2(1,1) = JIdxJIdx_11;
	//(残差 /光度参数)转置*(残差 /像素坐标)
	J->JabJIdx(0,0) = JabJIdx_00;
	J->JabJIdx(0,1) = JabJIdx_01;
	J->JabJIdx(1,0) = JabJIdx_10;
	J->JabJIdx(1,1) = JabJIdx_11;
	//(残差 /光度参数)转置*(残差 /光度参数)
	J->Jab2(0,0) = JabJab_00;
	J->Jab2(0,1) = JabJab_01;
	J->Jab2(1,0) = JabJab_01;
	J->Jab2(1,1) = JabJab_11;

	state_NewEnergyWithOutlier = energyLeft;

	if(energyLeft > std::max<float>(host->frameEnergyTH, target->frameEnergyTH) || wJI2_sum < 2)
	{
		energyLeft = std::max<float>(host->frameEnergyTH, target->frameEnergyTH);
		//异常点
		state_NewState = ResState::OUTLIER;
	}
	else
	{
	      //正常点
		state_NewState = ResState::IN;
	}

	state_NewEnergy = energyLeft;
	//返回值是残差值
	return energyLeft;
}