Vision.Matching.RGBDPatchTracking.TrackDetailed

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

function TrackDetailed
    input Real gray[:,:];
    input Real referenceDescriptor[patchSize];
    input Real seed[pixelDimension];
    input Boolean enabled = true;
    input Settings settings = Settings();
    output Boolean accepted;
    output Real pixel[pixelDimension];
    output Integer reason "0 accepted/disabled; 1 config; 2 reference; 3 sample/window; 4 aperture; 5 search; 6 descent/iterations; 7 SSD";
    output Integer iterations;
    output Real ssd;
    output Real minimumEigenvalue;
    output Diagnostics diagnostics;
  protected
    Real descriptor[patchSize]; Real jacobian[patchSize,pixelDimension]; Real energy;
    Real trialDescriptor[patchSize]; Real trialJacobian[patchSize,pixelDimension]; Real trialEnergy;
    Real residual[patchSize]; Real normal[pixelDimension,pixelDimension]; Real rhs[pixelDimension];
    Real step[pixelDimension]; Real trial[pixelDimension]; Real current[pixelDimension];
    Real determinant; Real traceValue; Real discriminant; Real maximumEigenvalue;
    Real normStep; Real scale; Real trialCost; Real referenceEnergy;
    Boolean valid; Boolean trialValid; Boolean moved; Boolean converged;
  algorithm
    accepted := false; pixel := zeros(pixelDimension); reason := 0;
    iterations := 0; ssd := 0; minimumEigenvalue := 0;
    diagnostics := Diagnostics();
    // Disabled padding is opaque, including seed/template poison.
    if not enabled then return; end if;
    reason := 1; diagnostics.stopDetail := 2;
    if not (settings.maximumIterations >= 1 and settings.maximumIterations <= 40
      and (settings.interpolationMethod == 0 or settings.interpolationMethod == 1)
      and settings.maximumBacktracks >= 1 and settings.maximumBacktracks <= 12
      and settings.searchRadius > 0 and settings.searchRadius <= 16
      and settings.maximumStep > 0 and settings.maximumStep <= settings.searchRadius
      and settings.convergenceTolerance > 0 and settings.convergenceTolerance <= settings.maximumStep
      and settings.minimumContrast >= 1e-12 and settings.minimumContrast <= 1
      and settings.minimumEigenvalue > 0 and settings.minimumEigenvalue <= 1
      and settings.minimumEigenvalueRatio > 0 and settings.minimumEigenvalueRatio <= 1
      and settings.maximumSsd > 0 and settings.maximumSsd <= 4) then return; end if;
    reason := 2; diagnostics.stopDetail := 3;
    for slot in 1:patchSize loop
      if not (referenceDescriptor[slot] >= -1 and referenceDescriptor[slot] <= 1) then return; end if;
    end for;
    referenceEnergy := referenceDescriptor*referenceDescriptor;
    if not (abs(referenceEnergy-1) <= 1e-8 and abs(sum(referenceDescriptor)) <= 1e-8) then return; end if;
    current := seed; converged := false;
    for iteration in 1:settings.maximumIterations loop
      iterations := iteration; reason := 3; diagnostics.stopDetail := 4;
      (valid,descriptor,jacobian,energy) := Sample(gray,current,settings.minimumContrast,settings.interpolationMethod);
      if not valid then return; end if;
      diagnostics.evaluated := true; diagnostics.lastEvaluatedPixel := current;
      diagnostics.lastEnergy := energy;
      residual := descriptor-referenceDescriptor; ssd := residual*residual;
      if iteration == 1 then diagnostics.initialSsd := ssd; end if;
      normal := transpose(jacobian)*jacobian; rhs := transpose(jacobian)*residual;
      diagnostics.lastGradient := rhs;
      traceValue := normal[1,1]+normal[2,2];
      discriminant := sqrt(max(0,(normal[1,1]-normal[2,2])^2+4*normal[1,2]^2));
      minimumEigenvalue := (traceValue-discriminant)/2;
      maximumEigenvalue := (traceValue+discriminant)/2; reason := 4;
      diagnostics.maximumEigenvalue := maximumEigenvalue; diagnostics.stopDetail := 5;
      if not (minimumEigenvalue >= settings.minimumEigenvalue
        and minimumEigenvalue >= settings.minimumEigenvalueRatio*maximumEigenvalue) then return; end if;
      determinant := normal[1,1]*normal[2,2]-normal[1,2]*normal[2,1];
      if not (determinant > 0) then return; end if;
      step := {normal[1,2]*rhs[2]-normal[2,2]*rhs[1],
        normal[2,1]*rhs[1]-normal[1,1]*rhs[2]}/determinant;
      normStep := sqrt(step*step);
      if not (normStep >= 0 and normStep <= 1e6) then reason := 6; diagnostics.stopDetail := 6; return; end if;
      diagnostics.stepValid := true; diagnostics.lastRawStep := step; diagnostics.lastRawStepNorm := normStep;
      if normStep <= settings.convergenceTolerance then converged := true; break; end if;
      if normStep > settings.maximumStep then step := step*(settings.maximumStep/normStep); end if;
      scale := 1; moved := false; reason := 6;
      for backtrack in 1:settings.maximumBacktracks loop
        trial := current+scale*step;
        diagnostics.lastBacktrack := backtrack; diagnostics.lastTrialScale := scale;
        diagnostics.lastTrialPixel := trial; diagnostics.lastTrialInSearch := false;
        diagnostics.lastTrialValid := false; diagnostics.lastTrialSsd := 0;
        if (trial-seed)*(trial-seed) <= settings.searchRadius*settings.searchRadius then
          diagnostics.lastTrialInSearch := true;
          (trialValid,trialDescriptor,trialJacobian,trialEnergy) := Sample(gray,trial,settings.minimumContrast,settings.interpolationMethod);
          if trialValid then
            residual := trialDescriptor-referenceDescriptor; trialCost := residual*residual;
            diagnostics.lastTrialValid := true; diagnostics.lastTrialSsd := trialCost;
            if trialCost < ssd then
              diagnostics.acceptedTrial := true; diagnostics.lastAcceptedTrialPixel := trial;
              current := trial; moved := true; break;
            end if;
            diagnostics.nonDecreaseRejects := diagnostics.nonDecreaseRejects+1;
          else
            diagnostics.sampleRejects := diagnostics.sampleRejects+1;
          end if;
        else
          diagnostics.windowRejects := diagnostics.windowRejects+1;
        end if;
        scale := scale/2;
      end for;
      if not moved then
        diagnostics.stopDetail := 7;
        if (current+step-seed)*(current+step-seed) > settings.searchRadius*settings.searchRadius then reason := 5; diagnostics.stopDetail := 8; end if;
        return;
      end if;
    end for;
    reason := 6; diagnostics.stopDetail := 9; if not converged then return; end if;
    reason := 7; diagnostics.stopDetail := 10; if not (ssd <= settings.maximumSsd) then return; end if;
    accepted := true; pixel := current; reason := 0;
    diagnostics.stopDetail := 1;
  end TrackDetailed;