LSD-SLAM的SE3Tracking线程笔记

就SE3Tracker.cpp中trackFrame

// ============ track frame ============
    //将初始估计记录下来(记录了参考帧到当前帧的刚度变换),然后定义一个6自由度矩阵的误差判别计算对象ls,定义cell数量以及最终的残差
	Sophus::SE3f referenceToFrame = frameToReference_initialEstimate.inverse().cast<float>();
	LGS6 ls;


	int numCalcResidualCalls[PYRAMID_LEVELS];  //cell数量  5层　金字塔
	int numCalcWarpUpdateCalls[PYRAMID_LEVELS];

	float last_residual = 0;  //最终残差


	for(int lvl=SE3TRACKING_MAX_LEVEL-1;lvl >= SE3TRACKING_MIN_LEVEL;lvl--) //金字塔的迭代从level-4到level-1
	{
		numCalcResidualCalls[lvl] = 0;
		numCalcWarpUpdateCalls[lvl] = 0;

		//有参考帧的逆深度信息和对应的像素点坐标  计算在参考帧坐标系下的3D点
		reference->makePointCloud(lvl);


		//对参考帧当前层构建点云，然后使用callOptimized函数调用calcResidualAndBuffers计算参考帧3D点变换到当前帧的残差(灰度)和(对应点像素)梯度,
        /// 参数为后面括号里面的所有数据，这是调用的宏定义
		callOptimized(calcResidualAndBuffers, (reference->posData[lvl], reference->colorAndVarData[lvl], SE3TRACKING_MIN_LEVEL == lvl ? reference->pointPosInXYGrid[lvl] : 0, reference->numData[lvl], frame, referenceToFrame, lvl, (plotTracking && lvl == SE3TRACKING_MIN_LEVEL)));

		// buf_warped_size 记录了　匹配点数量　包括好的匹配点　和　不好的匹配点　数量　
		//判断这个warp的好坏(如果改变的太多，已经超过了这一层图片的1%)，那么我们认为差别太大，Tracking失败，返回一个空的SE3
		if(buf_warped_size < MIN_GOODPERALL_PIXEL_ABSMIN * (width>>lvl)*(height>>lvl))
		{
			diverged = true;
			trackingWasGood = false;
			return SE3();
		}

		//是否使用了简单的仿射变换,
        // 那么把通过calcResidualAndBuffers函数更新的affineEstimation_a_lastIt以及affineEstimation_b_lastIt，赋值给仿射变换系数
		if(useAffineLightningEstimation)
		{
			affineEstimation_a = affineEstimation_a_lastIt;
			affineEstimation_b = affineEstimation_b_lastIt;
		}
		//调用calcWeightsAndResidual用方差归一化残差，并记录调用次数
        ///这里直接使用calcResidualAndBuffers的结果.
		float lastErr = callOptimized(calcWeightsAndResidual,(referenceToFrame));

		numCalcResidualCalls[lvl]++;


		float LM_lambda = settings.lambdaInitial[lvl];   //λ 　LM优化的　调整量  与高斯牛顿的区别之处 (A+λI)x=b

		for(int iteration=0; iteration < settings.maxItsPerLvl[lvl]; iteration++)
		{

		    //计算雅克比向量以及A和b
            // 计算　加权平均误差　以及误差权重
            ///这一步跟上面的calcWeightsAndResidual没有关系.
			callOptimized(calculateWarpUpdate,(ls));

			numCalcWarpUpdateCalls[lvl]++;

			iterationNumber = iteration;

			int incTry=0;
			while(true)
			{
				// solve LS system with current lambda
				Vector6 b = -ls.b;
				Matrix6x6 A = ls.A;
				for(int i=0;i<6;i++) A(i,i) *= 1+LM_lambda;  //(A+λI)  LM 调整量　
				//计算收敛的delta更新SE3
                //LDLT矩阵分解 dse3李代数更新量
				Vector6 inc = A.ldlt().solve(b);
				incTry++;

				// apply increment. pretty sure this way round is correct, but hard to test.
                 // 对　变换矩阵　左乘　se3指数映射　进行　更新
				Sophus::SE3f new_referenceToFrame = Sophus::SE