iterations algorithm

[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;
    }