SLAM.LoopClosure.RGBDLoopVerification.Consensus

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

function Consensus
    input Real sourcePoint[:,dimension];
    input Real targetPoint[size(sourcePoint,1),dimension];
    input Real pairEnabled[size(sourcePoint,1)];
    input Boolean requested;
    input Integer seed; input Integer trials; input Integer refinements;
    input Integer minimumInliers; input Real minimumFraction;
    input Real inlierDistance; input Real maximumRms;
    input Real coordinateLimit; input Real rankTolerance;
    output Boolean valid;
    output Integer rejectionReason;
    output Real rotation[dimension,dimension]; output Real translation[dimension];
    output Real inlierMask[size(sourcePoint,1)];
    output Integer matchCount; output Integer inlierCount;
    output Real rms; output Integer nextSeed;
    input Boolean calibratedResiduals = false;
    input Real sourceCovariance[size(sourcePoint,1),dimension,dimension] = zeros(size(sourcePoint,1),dimension,dimension);
    input Real targetCovariance[size(sourcePoint,1),dimension,dimension] = zeros(size(sourcePoint,1),dimension,dimension);
    input Real maximumNormalizedSquared = 9.0;
    input Real minimumPivot = 1e-10;
  protected
    Boolean configuration; Boolean payload; Boolean eligible; Boolean stable; Boolean weightedValid;
    Boolean covarianceValid;
    Integer slots[size(sourcePoint,1)]; Integer first; Integer second; Integer third;
    Integer chosen[dimension]; Integer state; Integer count; Integer bestCount;
    Real sampleSource[dimension,dimension]; Real sampleTarget[dimension,dimension];
    Real candidateRotation[dimension,dimension]; Real candidateTranslation[dimension];
    Real candidateMask[size(sourcePoint,1)]; Real bestMask[size(sourcePoint,1)];
    Real workingMask[size(sourcePoint,1)]; Real cost; Real bestCost;
    Real fitAccepted; Real fitReason; Real fitCount; Real invalidCount; Real rank;
    Real fitCost; Real fitRms; Real gap; Real sourceCentroid[dimension]; Real targetCentroid[dimension];
    Real unusedScore;
  algorithm
    valid := false; rejectionReason := 1;
    rotation := identity(dimension); translation := zeros(dimension);
    inlierMask := zeros(size(sourcePoint,1)); matchCount := 0; inlierCount := 0;
    rms := 0.0; nextSeed := seed;
    if requested then
      configuration := size(sourcePoint,1) >= dimension and size(sourcePoint,1) <= featureCapacity
        and seed >= 1 and seed < randomModulus and trials >= 1 and trials <= maximumTrials
        and refinements >= 1 and refinements <= maximumRefinements
        and minimumInliers >= dimension and minimumInliers <= size(sourcePoint,1)
        and minimumFraction > 0.0 and minimumFraction <= 1.0
        and inlierDistance > 0.0 and inlierDistance <= 10.0
        and maximumRms >= 0.0 and maximumRms <= inlierDistance
        and coordinateLimit > 0.0 and coordinateLimit <= 1e4
        and rankTolerance >= 1e-12 and rankTolerance <= 1e-2;
      if calibratedResiduals then
        configuration := configuration and maximumNormalizedSquared > 0 and maximumNormalizedSquared <= 1e6
          and minimumPivot > 0 and minimumPivot < 1;
      end if;
      rejectionReason := 2;
      if configuration then
        slots := fill(0,size(sourcePoint,1)); payload := true;
        for slot in 1:size(sourcePoint,1) loop
          payload := payload and (pairEnabled[slot] == 0.0 or pairEnabled[slot] == 1.0);
          if pairEnabled[slot] == 1.0 then
            matchCount := matchCount+1; slots[matchCount] := slot;
            for axis in 1:dimension loop
              payload := payload and abs(sourcePoint[slot,axis]) <= coordinateLimit
                and abs(targetPoint[slot,axis]) <= coordinateLimit;
            end for;
            if calibratedResiduals then
              (covarianceValid,unusedScore) := RegistrationWhitenedResidual3(zeros(dimension),sourceCovariance[slot,:,:],minimumPivot);
              payload := payload and covarianceValid;
              (covarianceValid,unusedScore) := RegistrationWhitenedResidual3(zeros(dimension),targetCovariance[slot,:,:],minimumPivot);
              payload := payload and covarianceValid;
            end if;
          end if;
        end for;
        rejectionReason := 3;
        if payload then
          rejectionReason := 4;
          if matchCount >= minimumInliers then
            bestCount := 0; bestCost := 1e100; bestMask := zeros(size(sourcePoint,1)); state := seed;
            for hypothesis in 1:trials loop
              state := NextSample(state); first := mod(state,matchCount)+1;
              state := NextSample(state); second := mod(state,matchCount-1)+1;
              if second >= first then second := second+1; end if;
              state := NextSample(state); third := mod(state,matchCount-2)+1;
              if third >= min(first,second) then third := third+1; end if;
              if third >= max(first,second) then third := third+1; end if;
              chosen := {slots[first],slots[second],slots[third]};
              for point in 1:dimension loop
                sampleSource[point,:] := sourcePoint[chosen[point],:];
                sampleTarget[point,:] := targetPoint[chosen[point],:];
              end for;
              (fitAccepted,fitReason,candidateRotation,candidateTranslation,fitCount,invalidCount,
                rank,fitCost,fitRms,gap,sourceCentroid,targetCentroid) := FitRigidPointPairs(
                  sampleSource,sampleTarget,ones(dimension),dimension,coordinateLimit,rankTolerance,
                  if calibratedResiduals then 1e6 else inlierDistance);
              if fitAccepted == 1.0 then
                (candidateMask,count,cost) := ScorePairs(sourcePoint,targetPoint,pairEnabled,
                  candidateRotation,candidateTranslation,inlierDistance,calibratedResiduals=calibratedResiduals,
                  sourceCovariance=sourceCovariance,targetCovariance=targetCovariance,
                  maximumNormalizedSquared=maximumNormalizedSquared,minimumPivot=minimumPivot);
                if count > bestCount or (count == bestCount and cost < bestCost) then
                  bestCount := count; bestCost := cost; bestMask := candidateMask;
                end if;
              end if;
            end for;
            nextSeed := state;
            eligible := bestCount >= minimumInliers and bestCount >= minimumFraction*matchCount;
            stable := false; workingMask := bestMask; rejectionReason := 5;
            // Stop doing fits after stability/refusal, while keeping a bounded
            // compact loop. The final fit and covariance must use one mask.
            for refinement in 1:refinements loop
              if eligible and not stable then
                (fitAccepted,fitReason,candidateRotation,candidateTranslation,fitCount,invalidCount,
                  rank,fitCost,fitRms,gap,sourceCentroid,targetCentroid) := FitRigidPointPairs(
                    sourcePoint,targetPoint,workingMask,size(sourcePoint,1),coordinateLimit,rankTolerance,
                    if calibratedResiduals then 1e6 else maximumRms);
                if fitAccepted == 1.0 and calibratedResiduals then
                  (weightedValid,candidateRotation,candidateTranslation,fitRms) := RegistrationWeighted.WeightedRefine(
                    sourcePoint,targetPoint,workingMask,sourceCovariance,targetCovariance,
                    candidateRotation,candidateTranslation,minimumPivot);
                  if not weightedValid then fitAccepted := 0.0; end if;
                end if;
                eligible := fitAccepted == 1.0;
                if not eligible then rejectionReason := 5; end if;
                if eligible then
                  (candidateMask,count,cost) := ScorePairs(sourcePoint,targetPoint,pairEnabled,
                    candidateRotation,candidateTranslation,inlierDistance,calibratedResiduals=calibratedResiduals,
                    sourceCovariance=sourceCovariance,targetCovariance=targetCovariance,
                    maximumNormalizedSquared=maximumNormalizedSquared,minimumPivot=minimumPivot);
                  eligible := count >= minimumInliers and count >= minimumFraction*matchCount;
                  stable := true;
                  for slot in 1:size(sourcePoint,1) loop
                    stable := stable and candidateMask[slot] == workingMask[slot];
                  end for;
                  if eligible and stable then
                    rotation := candidateRotation; translation := candidateTranslation;
                    inlierMask := workingMask; inlierCount := count; rms := fitRms;
                    valid := true; rejectionReason := 0;
                  else
                    workingMask := candidateMask;
                    rejectionReason := if eligible then 6 else 5;
                  end if;
                end if;
              end if;
            end for;
          end if;
        end if;
      end if;
    end if;
  end Consensus;