iterations algorithm

[zhouq123]

No description provided.

[zhouq123]

for (int iterNo = 0; iterNo < noIterationsPerLevel[levelId]; iterNo++)
	{
		// evaluate error function and gradients
		noValidPoints_new = this->ComputeGandH(f_new, nabla_new, hessian_new, approxInvPose);

		// check if error increased. If so, revert
		if ((noValidPoints_new <= 0) || (f_new > f_old)) {
			trackingState->pose_d->SetFrom(&lastKnownGoodPose);
			approxInvPose = trackingState->pose_d->GetInvM();
			lambda *= 10.0f;
		}
		else {
			lastKnownGoodPose.SetFrom(trackingState->pose_d);
			f_old = f_new;
			noValidPoints_old = noValidPoints_new;

			for (int i = 0; i < 6 * 6; ++i) hessian_good[i] = hessian_new[i] / noValidPoints_new;
			for (int i = 0; i < 6; ++i) nabla_good[i] = nabla_new[i] / noValidPoints_new;
			lambda /= 10.0f;
		}
		for (int i = 0; i < 6 * 6; ++i) A[i] = hessian_good[i];
		for (int i = 0; i < 6; ++i) A[i + i * 6] *= 1.0f + lambda;

		// compute a new step and make sure we've got an SE3
		ComputeDelta(step, nabla_good, A, iterationType != TRACKER_ITERATION_BOTH);
		ApplyDelta(approxInvPose, step, approxInvPose);
		trackingState->pose_d->SetInvM(approxInvPose);
		trackingState->pose_d->Coerce();
		approxInvPose = trackingState->pose_d->GetInvM();

		// if step is small, assume it's going to decrease the error and finish
		if (HasConverged(step)) break;
	}