Vision.Registration.RegistrationWeighted.WeightedRefine

Parameters, inputs, outputs, defaults and equations from the source file.

function WeightedRefine
    input Real sourcePoint[:,3]; input Real targetPoint[size(sourcePoint,1),3];
    input Real enabled[size(sourcePoint,1)];
    input Real sourceCovariance[size(sourcePoint,1),3,3]; input Real targetCovariance[size(sourcePoint,1),3,3];
    input Real initialRotation[3,3]; input Real initialTranslation[3]; input Real minimumPivot = 1e-10;
    output Boolean valid; output Real rotation[3,3]; output Real translation[3]; output Real rms;
  protected
    Real H[6,6]; Real inverseH[6,6]; Real gradient[6]; Real step[6]; Real cost; Real pivot;
    Real trialH[6,6]; Real trialGradient[6]; Real trialCost; Real trialRms; Real scale;
    Real trialRotation[3,3]; Real trialTranslation[3]; Boolean normalValid; Boolean inverseValid;
    Boolean trialValid; Boolean accepted; Boolean running;
  algorithm
    rotation := initialRotation; translation := initialTranslation;
    (normalValid,cost,H,gradient,rms) := WeightedNormal(sourcePoint,targetPoint,enabled,sourceCovariance,targetCovariance,rotation,translation,minimumPivot);
    (inverseH,inverseValid,pivot) := WeightedInverse6(H,minimumPivot);
    valid := normalValid and inverseValid; running := valid;
    step := zeros(6); scale := 1.0; accepted := false;
    for iteration in 1:8 loop
      if running then
        step := -inverseH*gradient;
        scale := min(1.0,min(0.5/max(1e-12,sqrt(step[1:3]*step[1:3])),0.1/max(1e-12,sqrt(step[4:6]*step[4:6]))));
        accepted := false;
      end if;
      // Keep the bounded loop outside the conditional so the pinned Rumoca
      // release can lower this function; guarded updates preserve early stop.
      for backtrack in 1:8 loop
          if running and not accepted then
            trialRotation := WeightedExp(scale*step[4:6])*rotation; trialTranslation := translation+scale*step[1:3];
            (trialValid,trialCost,trialH,trialGradient,trialRms) := WeightedNormal(sourcePoint,targetPoint,enabled,sourceCovariance,targetCovariance,trialRotation,trialTranslation,minimumPivot);
            if trialValid and trialCost <= cost then
              rotation := trialRotation; translation := trialTranslation; cost := trialCost;
              H := trialH; gradient := trialGradient; rms := trialRms; accepted := true;
            else scale := scale*0.5; end if;
          end if;
      end for;
      if running then
        running := accepted and scale*sqrt(step*step) > 1e-7;
        (inverseH,inverseValid,pivot) := WeightedInverse6(H,minimumPivot);
        valid := valid and inverseValid; running := running and valid;
      end if;
    end for;
  end WeightedRefine;