Vision.Registration.FitRigidPointPairsRobust

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

function FitRigidPointPairsRobust
  import RegistrationWeighted = Vision.Registration.RegistrationWeighted;
  import FitRigidPointPairs = Vision.Registration.FitRigidPointPairs;
  import RegistrationPairResidual = Vision.Registration.RegistrationPairResidual;
  import RegistrationWhitenedResidual3 = Vision.Registration.RegistrationWhitenedResidual3;

  input Real sourcePoint[:,:];
  input Real targetPoint[:,:];
  input Real pairEnabled[:];
  input Real activeCount;
  input Real coordinateLimit;
  input Real rankTolerance;
  input Real maximumRms;
  input Integer maximumHypotheses = 64;
  input Real minimumConsensusFraction = 0.5;
  output Real accepted;
  output Real rejectionReason;
  output Real rotation[3,3];
  output Real translation[3];
  output Real validCount;
  output Real invalidCount;
  output Real rank;
  output Real cost;
  output Real rms;
  output Real eigenGap;
  output Real sourceCentroid[3];
  output Real targetCentroid[3];
  output Real finalInlierMask[size(pairEnabled,1)];
  output Real rejectedCount;
  input Boolean useCovariance = false;
  input Real sourceCovariance[size(pairEnabled,1),3,3] = zeros(size(pairEnabled,1),3,3);
  input Real targetCovariance[size(pairEnabled,1),3,3] = zeros(size(pairEnabled,1),3,3);
  input Real maximumNormalizedSquared = 9.0 "Squared radius in whitened 3D residual space";
  input Real covarianceMinimumPivot = 1e-10;
protected
  constant Integer dimension = 3;
  constant Integer maximumRefinements = 4;
  constant Integer stateModulus = 2147483647;
  constant Integer stateMultiplier = 16807;
  constant Integer stateQuotient = 127773;
  constant Integer stateRemainder = 2836;
  Boolean configurationValid; Boolean searching; Boolean stable;
  Boolean residualValid; Boolean covarianceValid; Boolean allInliers;
  Integer enabledIndices[size(pairEnabled,1)]; Integer enabledCount;
  Integer sampleIndices[dimension]; Integer lowIndex; Integer highIndex;
  Integer state; Integer candidateCount; Integer bestCount; Integer retainedCount;
  Integer minimumConsensus; Integer index;
  Real originalValidCount; Real squaredLimit; Real residual; Real squaredResidual;
  Real candidateCost; Real bestCost;
  Boolean weightedValid;
  Real fitMaximumRms; Real unusedScore;
  Real candidateMask[size(pairEnabled,1)]; Real bestMask[size(pairEnabled,1)];
  Real sampleSource[dimension,dimension]; Real sampleTarget[dimension,dimension];
  Real fitAccepted; Real fitReason; Real fitRotation[dimension,dimension]; Real fitTranslation[dimension];
  Real fitValidCount; Real fitInvalidCount; Real fitRank; Real fitCost; Real fitRms; Real fitEigenGap;
  Real fitSourceCentroid[dimension]; Real fitTargetCentroid[dimension];
algorithm
  // In calibrated mode the geometric solver estimates an unweighted rigid
  // transform; per-pair covariance certifies its residuals below. A fixed
  // metric RMS must not reject a noisy hypothesis before that test can run.
  fitMaximumRms := if useCovariance then 1e6 else maximumRms;
  (accepted,rejectionReason,rotation,translation,validCount,invalidCount,rank,cost,rms,
    eigenGap,sourceCentroid,targetCentroid) := FitRigidPointPairs(
      sourcePoint,targetPoint,pairEnabled,activeCount,coordinateLimit,rankTolerance,fitMaximumRms);
  finalInlierMask := zeros(size(pairEnabled,1)); rejectedCount := 0.0;
  configurationValid := maximumHypotheses >= 1 and maximumHypotheses <= 1024
    and minimumConsensusFraction > 0.0 and minimumConsensusFraction <= 1.0
    and maximumRms >= 0.0 and maximumRms <= 1e6;
  if useCovariance then
    configurationValid := configurationValid and maximumNormalizedSquared > 0.0
      and maximumNormalizedSquared <= 1e6 and covarianceMinimumPivot > 0.0 and covarianceMinimumPivot < 1.0;
    for pair in 1:size(pairEnabled,1) loop
      if pair <= activeCount and pairEnabled[pair] == 1.0 then
        (covarianceValid,unusedScore) := RegistrationWhitenedResidual3(zeros(3),sourceCovariance[pair,:,:],covarianceMinimumPivot);
        configurationValid := configurationValid and covarianceValid;
        (covarianceValid,unusedScore) := RegistrationWhitenedResidual3(zeros(3),targetCovariance[pair,:,:],covarianceMinimumPivot);
        configurationValid := configurationValid and covarianceValid;
      end if;
    end for;
    if accepted > 0.5 then
      // Preserve the exact geometric fast path when there is no residual to
      // refine; numerical roundoff must not change an already exact seed.
      weightedValid := true;
      if rms > 1e-12 then
        (weightedValid,rotation,translation,rms) := RegistrationWeighted.WeightedRefine(sourcePoint,targetPoint,pairEnabled,
          sourceCovariance,targetCovariance,rotation,translation,covarianceMinimumPivot);
      end if;
      if not weightedValid then accepted := 0.0; rejectionReason := 7.0; end if;
      allInliers := weightedValid;
      for pair in 1:size(pairEnabled,1) loop
        if pair <= activeCount and pairEnabled[pair] == 1.0 then
          (residualValid,squaredResidual) := RegistrationPairResidual(sourcePoint[pair,:],targetPoint[pair,:],
            rotation,translation,true,sourceCovariance[pair,:,:],targetCovariance[pair,:,:],covarianceMinimumPivot);
          allInliers := allInliers and residualValid and squaredResidual <= maximumNormalizedSquared;
        end if;
      end for;
      if not allInliers then
        accepted := 0.0; rejectionReason := 6.0; rotation := identity(dimension); translation := zeros(dimension);
      end if;
    end if;
  end if;
  if not configurationValid then
    accepted := 0.0; rejectionReason := 1.0; rotation := identity(dimension); translation := zeros(dimension);
  elseif accepted > 0.5 then
    for pair in 1:size(pairEnabled,1) loop
      finalInlierMask[pair] := if pair <= activeCount and pairEnabled[pair] >= 1.0 and pairEnabled[pair] <= 1.0 then 1.0 else 0.0;
    end for;
  elseif rejectionReason >= 6.0 and rejectionReason <= 6.0 then
    // The original reason6 proves all active requested pairs valid and finite.
    originalValidCount := validCount; enabledCount := 0;
    enabledIndices := fill(0,size(pairEnabled,1));
    for pair in 1:size(pairEnabled,1) loop
      if pair <= activeCount and pairEnabled[pair] >= 1.0 and pairEnabled[pair] <= 1.0 then
        enabledCount := enabledCount+1; enabledIndices[enabledCount] := pair;
      end if;
    end for;
    minimumConsensus := max(dimension,integer(ceil(minimumConsensusFraction*originalValidCount)));
    squaredLimit := if useCovariance then maximumNormalizedSquared else maximumRms*maximumRms;
    candidateMask := zeros(size(pairEnabled,1)); bestMask := zeros(size(pairEnabled,1));
    bestCount := 0; bestCost := 0.0; state := 1;
    sampleSource := zeros(dimension,dimension); sampleTarget := zeros(dimension,dimension);
    for hypothesis in 1:maximumHypotheses loop
      // Schrage's form of the31-bit Park-Miller state keeps every intermediate
      // inside signed32-bit range; no host RNG or unbounded duplicate retry.
      state := stateMultiplier*mod(state,stateQuotient)-stateRemainder*div(state,stateQuotient);
      if state <= 0 then state := state+stateModulus; end if;
      sampleIndices[1] := 1+mod(state,enabledCount);
      state := stateMultiplier*mod(state,stateQuotient)-stateRemainder*div(state,stateQuotient);
      if state <= 0 then state := state+stateModulus; end if;
      sampleIndices[2] := 1+mod(state,enabledCount-1);
      if sampleIndices[2] >= sampleIndices[1] then sampleIndices[2] := sampleIndices[2]+1; end if;
      lowIndex := min(sampleIndices[1],sampleIndices[2]); highIndex := max(sampleIndices[1],sampleIndices[2]);
      state := stateMultiplier*mod(state,stateQuotient)-stateRemainder*div(state,stateQuotient);
      if state <= 0 then state := state+stateModulus; end if;
      sampleIndices[3] := 1+mod(state,enabledCount-2);
      if sampleIndices[3] >= lowIndex then sampleIndices[3] := sampleIndices[3]+1; end if;
      if sampleIndices[3] >= highIndex then sampleIndices[3] := sampleIndices[3]+1; end if;
      for sample in 1:dimension loop
        index := enabledIndices[sampleIndices[sample]];
        for axis in 1:dimension loop
          sampleSource[sample,axis] := sourcePoint[index,axis];
          sampleTarget[sample,axis] := targetPoint[index,axis];
        end for;
      end for;
      (fitAccepted,fitReason,fitRotation,fitTranslation,fitValidCount,fitInvalidCount,fitRank,
        fitCost,fitRms,fitEigenGap,fitSourceCentroid,fitTargetCentroid) := FitRigidPointPairs(
          sampleSource,sampleTarget,ones(dimension),dimension,coordinateLimit,rankTolerance,fitMaximumRms);
      if fitAccepted > 0.5 then
        candidateCount := 0; candidateCost := 0.0;
        for compact in 1:enabledCount loop
          index := enabledIndices[compact];
          if useCovariance then
            (residualValid,squaredResidual) := RegistrationPairResidual(sourcePoint[index,:],targetPoint[index,:],
              fitRotation,fitTranslation,true,sourceCovariance[index,:,:],targetCovariance[index,:,:],covarianceMinimumPivot);
          else
            // Preserve the original clean/Euclidean operation order.
            residualValid := true; squaredResidual := 0.0;
            for axis in 1:dimension loop
              residual := fitTranslation[axis]-targetPoint[index,axis];
              for column in 1:dimension loop residual := residual+fitRotation[axis,column]*sourcePoint[index,column]; end for;
              squaredResidual := squaredResidual+residual*residual;
            end for;
          end if;
          candidateMask[index] := if residualValid and squaredResidual >= 0.0 and squaredResidual <= squaredLimit then 1.0 else 0.0;
          if candidateMask[index] > 0.5 then
            candidateCount := candidateCount+1; candidateCost := candidateCost+squaredResidual;
          end if;
        end for;
        if candidateCount > bestCount or (candidateCount == bestCount and candidateCost < bestCost) then
          bestCount := candidateCount; bestCost := candidateCost; bestMask := candidateMask;
        end if;
      end if;
    end for;
    searching := bestCount >= minimumConsensus; stable := false;
    for refinement in 1:maximumRefinements loop
      if searching then
        (fitAccepted,fitReason,fitRotation,fitTranslation,fitValidCount,fitInvalidCount,fitRank,
          fitCost,fitRms,fitEigenGap,fitSourceCentroid,fitTargetCentroid) := FitRigidPointPairs(
            sourcePoint,targetPoint,bestMask,activeCount,coordinateLimit,rankTolerance,fitMaximumRms);
        if fitAccepted > 0.5 and useCovariance then
          (weightedValid,fitRotation,fitTranslation,fitRms) := RegistrationWeighted.WeightedRefine(sourcePoint,targetPoint,bestMask,
            sourceCovariance,targetCovariance,fitRotation,fitTranslation,covarianceMinimumPivot);
          if not weightedValid then fitAccepted := 0.0; end if;
        end if;
        if fitAccepted > 0.5 then
          retainedCount := 0;
          for compact in 1:enabledCount loop
            index := enabledIndices[compact]; candidateMask[index] := 0.0;
            if bestMask[index] > 0.5 then
              if useCovariance then
                (residualValid,squaredResidual) := RegistrationPairResidual(sourcePoint[index,:],targetPoint[index,:],
                  fitRotation,fitTranslation,true,sourceCovariance[index,:,:],targetCovariance[index,:,:],covarianceMinimumPivot);
              else
                residualValid := true; squaredResidual := 0.0;
                for axis in 1:dimension loop
                  residual := fitTranslation[axis]-targetPoint[index,axis];
                  for column in 1:dimension loop residual := residual+fitRotation[axis,column]*sourcePoint[index,column]; end for;
                  squaredResidual := squaredResidual+residual*residual;
                end for;
              end if;
              if residualValid and squaredResidual >= 0.0 and squaredResidual <= squaredLimit then
                candidateMask[index] := 1.0; retainedCount := retainedCount+1;
              end if;
            end if;
          end for;
          stable := retainedCount == bestCount and retainedCount >= minimumConsensus;
          if stable then
            accepted := fitAccepted; rejectionReason := fitReason;
            rotation := fitRotation; translation := fitTranslation;
            validCount := fitValidCount; invalidCount := fitInvalidCount; rank := fitRank;
            cost := fitCost; rms := fitRms; eigenGap := fitEigenGap;
            sourceCentroid := fitSourceCentroid; targetCentroid := fitTargetCentroid;
            finalInlierMask := bestMask; rejectedCount := originalValidCount-fitValidCount;
            searching := false;
          else
            bestMask := candidateMask; bestCount := retainedCount;
            searching := retainedCount >= minimumConsensus;
          end if;
        else
          searching := false;
        end if;
      end if;
    end for;
    if not stable then
      accepted := 0.0; rejectionReason := 7.0;
      rotation := identity(dimension); translation := zeros(dimension);
      // Retain original full-fit diagnostics, but never publish an unproved mask.
      finalInlierMask := zeros(size(pairEnabled,1)); rejectedCount := 0.0;
    end if;
  end if;
end FitRigidPointPairsRobust;