Vision.Matching.RGBDPatchTracking.Sample

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

function Sample
    input Real gray[:,:] "Raw samples use the existing [0,1] gray convention";
    input Real pixel[pixelDimension] "Zero-based RGB coordinates";
    input Real minimumContrast = 0.001;
    input Integer interpolationMethod = 0 "0 bilinear; 1 C1 Catmull-Rom";
    output Boolean valid;
    output Real descriptor[patchSize];
    output Real jacobian[patchSize,pixelDimension];
    output Real energy;
  protected
    Real samples[patchSize]; Real gradient[patchSize,pixelDimension];
    Real centered[patchSize]; Real meanSample; Real meanGradient[pixelDimension];
    Real projection[pixelDimension]; Real x; Real y; Real fx; Real fy;
    Real a; Real b; Real c; Real d; Real denominator;
    Real weightsX[4]; Real weightsY[4]; Real derivativeX[4]; Real derivativeY[4];
    Real support[4,4];
    Integer x0; Integer y0; Integer slot;
  algorithm
    valid := false; descriptor := zeros(patchSize);
    jacobian := zeros(patchSize,pixelDimension); energy := 0.0;
    // The upper bound leaves a complete derivative cell even at integer pixels.
    // Validate before floor/Integer conversion or any indexed read.
    if interpolationMethod == 0 then
      if not (size(gray,1) >= patchWidth+1 and size(gray,2) >= patchWidth+1
        and pixel[1] >= 3 and pixel[1] < size(gray,2)-4
        and pixel[2] >= 3 and pixel[2] < size(gray,1)-4
        and minimumContrast >= 1e-12 and minimumContrast <= 1) then return; end if;
    elseif interpolationMethod == 1 then
      // A cubic sample needs floor(x)-1..floor(x)+2 on each axis, including
      // zero-weight taps at knots. No padded or clamped boundary is invented.
      if not (size(gray,1) >= patchWidth+3 and size(gray,2) >= patchWidth+3
        and pixel[1] >= 4 and pixel[1] < size(gray,2)-5
        and pixel[2] >= 4 and pixel[2] < size(gray,1)-5
        and minimumContrast >= 1e-12 and minimumContrast <= 1) then return; end if;
    else return; end if;
    for row in 1:patchWidth loop
      for column in 1:patchWidth loop
        slot := (row-1)*patchWidth+column;
        x := pixel[1]+column-4; y := pixel[2]+row-4;
        x0 := integer(floor(x)); y0 := integer(floor(y));
        fx := x-x0; fy := y-y0;
        if interpolationMethod == 0 then
          a := gray[y0+1,x0+1]; b := gray[y0+1,x0+2];
          c := gray[y0+2,x0+1]; d := gray[y0+2,x0+2];
          if not (a >= 0 and a <= 1 and b >= 0 and b <= 1
            and c >= 0 and c <= 1 and d >= 0 and d <= 1) then return; end if;
          samples[slot] := (1-fy)*((1-fx)*a+fx*b)+fy*((1-fx)*c+fx*d);
          gradient[slot,1] := (1-fy)*(b-a)+fy*(d-c);
          gradient[slot,2] := (1-fx)*(c-a)+fx*(d-b);
        else
          (weightsX,derivativeX) := CatmullRomWeights(fx);
          (weightsY,derivativeY) := CatmullRomWeights(fy);
          for v in 1:4 loop
            for u in 1:4 loop
              support[v,u] := gray[y0+v-1,x0+u-1];
              if not (support[v,u] >= 0 and support[v,u] <= 1) then return; end if;
            end for;
          end for;
          // Cubic overshoot is retained. The centered-energy admission below
          // bounds the normalized result without clipping interpolated samples.
          samples[slot] := weightsY*support*weightsX;
          gradient[slot,1] := weightsY*support*derivativeX;
          gradient[slot,2] := derivativeY*support*weightsX;
        end if;
      end for;
    end for;
    meanSample := sum(samples)/patchSize;
    centered := samples-fill(meanSample,patchSize); energy := centered*centered;
    if not (energy > 0 and energy >= patchSize*minimumContrast*minimumContrast and energy <= patchSize) then return; end if;
    denominator := sqrt(energy); descriptor := centered/denominator;
    for axis in 1:pixelDimension loop
      meanGradient[axis] := sum(gradient[:,axis])/patchSize;
      projection[axis] := centered*gradient[:,axis]/energy;
      jacobian[:,axis] := (gradient[:,axis]-fill(meanGradient[axis],patchSize)
        -centered*projection[axis])/denominator;
    end for;
    valid := true;
  end Sample;