SLAM.LoopClosure.RGBDLoopVerification.Verify

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

function Verify
    input RGBDKeyframes.Frame reference;
    input RGBDKeyframes.Frame current;
    input Boolean requested;
    input Integer seed = 7; input Integer trials = 96; input Integer refinements = 4;
    input Integer minimumInliers = 12; input Real minimumFraction = 0.5;
    input Real inlierDistance = 0.08; input Real maximumRms = 0.03;
    input Real minimumAge = 5.0; input Real descriptorRatio = 0.8;
    input Real maximumDescriptorDistance = 0.8;
    input Real coordinateLimit = 100.0; input Real rankTolerance = 1e-8;
    input Real localizationSigma = 0.5; input Real depthInflation = 1.0; input Real minimumPivot = 1e-10;
    output Proposal result;
    input Boolean calibratedResiduals = true;
    input Real maximumNormalizedSquared = 9.0;
  protected
    Boolean configuration; Boolean identityValid; Boolean consensusValid; Boolean edgeValid;
    Integer consensusReason; Integer edgeReason; Integer matches; Integer inliers; Integer nextSeed;
    Real referenceMask[featureCapacity]; Real currentMask[featureCapacity];
    Real partners[featureCapacity]; Real pairEnabled[featureCapacity];
    Real sourcePoint[featureCapacity,dimension]; Real targetPoint[featureCapacity,dimension];
    Real nearest[featureCapacity]; Real second[featureCapacity]; Real count; Real matchValid;
    Real invalidReference; Real invalidCurrent; Real mask[featureCapacity];
    Real C[dimension,dimension]; Real t[dimension]; Real rms;
    Real uncertaintyValid; Real uncertaintyReason; Real uncertaintyCount; Real uncertaintyInvalid;
    Real relativeCovariance[poseDimension,poseDimension]; Real unusedCovariance[poseDimension,poseDimension];
    Real normal[poseDimension,poseDimension]; Real noise[poseDimension,poseDimension]; Real J[poseDimension,poseDimension];
    Real pivot; Real D[dimension,dimension]; Real u[dimension];
    Real edgeCovariance[poseDimension,poseDimension]; Real information[poseDimension,poseDimension];
    Real sourceCovariance[featureCapacity,dimension,dimension]; Real targetCovariance[featureCapacity,dimension,dimension];
  algorithm
    result := EmptyProposal(seed);
    if requested then
      configuration := minimumAge >= 0.0 and minimumAge <= 1e6
        and seed >= 1 and seed < randomModulus and trials >= 1 and trials <= maximumTrials
        and refinements >= 1 and refinements <= maximumRefinements
        and minimumInliers >= dimension and minimumInliers <= featureCapacity
        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
        and descriptorRatio > 0.0 and descriptorRatio < 1.0
        and maximumDescriptorDistance >= 0.0 and maximumDescriptorDistance <= 2.0
        and localizationSigma > 0.0 and localizationSigma <= 10.0
        and depthInflation >= 1.0 and depthInflation <= 100.0
        and minimumPivot > 0.0 and minimumPivot < 1.0;
      if calibratedResiduals then
        configuration := configuration and maximumNormalizedSquared > 0 and maximumNormalizedSquared <= 1e6;
      end if;
      result.rejectionReason := 2;
      if configuration then
        identityValid := reference.generation == current.generation and reference.id < current.id
          and reference.epoch < current.epoch and reference.imageTime < current.imageTime
          and current.imageTime-reference.imageTime >= minimumAge
          and reference.vocabularyVersion == current.vocabularyVersion;
        result.rejectionReason := 3;
        if identityValid then
          result.rejectionReason := 4;
          if RGBDKeyframes.ValidFrame(reference) and RGBDKeyframes.ValidFrame(current) then
            // The current sandwich model assumes a common disparity noise and
            // stereo baseline. Refuse incompatible snapshots rather than reuse
            // current calibration for the retained reference image.
            result.rejectionReason := 5;
            if reference.disparityNoise == current.disparityNoise and reference.baseline == current.baseline then
              for slot in 1:featureCapacity loop
                referenceMask[slot] := if reference.enabled[slot] then 1.0 else 0.0;
                currentMask[slot] := if current.enabled[slot] then 1.0 else 0.0;
              end for;
              (partners,pairEnabled,sourcePoint,targetPoint,count,matchValid,invalidReference,invalidCurrent,nearest,second) :=
                MatchRGBDDescriptors(reference.descriptor,current.descriptor,reference.opticalPoint,current.opticalPoint,
                  referenceMask,currentMask,reference.count,current.count,descriptorRatio,maximumDescriptorDistance,
                  0.0,identity(dimension),zeros(dimension),0.0);
              result.matchedCount := integer(count); result.rejectionReason := 6;
              if matchValid == 1.0 and invalidReference == 0.0 and invalidCurrent == 0.0 then
                sourceCovariance := zeros(featureCapacity,dimension,dimension);
                targetCovariance := zeros(featureCapacity,dimension,dimension);
                if calibratedResiduals then
                  for slot in 1:featureCapacity loop
                    if pairEnabled[slot] == 1.0 then
                      sourceCovariance[slot,:,:] := RGBDOpticalPointCovariance(sourcePoint[slot,:],
                        reference.rgbCalibration[1],reference.rgbCalibration[2],localizationSigma,
                        reference.disparityNoise,reference.noiseReferenceFx,reference.baseline,depthInflation);
                      targetCovariance[slot,:,:] := RGBDOpticalPointCovariance(targetPoint[slot,:],
                        current.rgbCalibration[1],current.rgbCalibration[2],localizationSigma,
                        current.disparityNoise,current.noiseReferenceFx,current.baseline,depthInflation);
                    end if;
                  end for;
                end if;
                (consensusValid,consensusReason,C,t,mask,matches,inliers,rms,nextSeed) := Consensus(
                  sourcePoint,targetPoint,pairEnabled,true,seed,trials,refinements,minimumInliers,minimumFraction,
                  inlierDistance,maximumRms,coordinateLimit,rankTolerance,
                  calibratedResiduals=calibratedResiduals,sourceCovariance=sourceCovariance,targetCovariance=targetCovariance,
                  maximumNormalizedSquared=maximumNormalizedSquared,minimumPivot=minimumPivot);
                result.nextSeed := nextSeed; result.rejectionReason := 7;
                if consensusValid then
                  // Preserve measured fit diagnostics even if uncertainty or edge validation rejects it.
                  result.inlierCount := inliers; result.rms := rms;
                  (uncertaintyValid,uncertaintyReason,uncertaintyCount,uncertaintyInvalid,relativeCovariance,
                    unusedCovariance,normal,noise,J,pivot) := RGBDRegistrationSandwich(sourcePoint,targetPoint,
                      mask,featureCapacity,1.0,C,t,reference.bodyRotation,reference.opticalToBody,
                      reference.cameraOriginBody,reference.rgbCalibration[1:2],current.rgbCalibration[1:2],
                      reference.noiseReferenceFx,current.noiseReferenceFx,reference.baseline,localizationSigma,
                      reference.disparityNoise,depthInflation,coordinateLimit,minimumPivot,weighted=calibratedResiduals);
                  result.rejectionReason := 8;
                  if uncertaintyValid == 1.0 then
                    (D,u,J,edgeCovariance,information,edgeValid,edgeReason,pivot) := RGBDOpticalToBodyEdge(C,t,
                      relativeCovariance,reference.opticalToBody,current.opticalToBody,
                      reference.cameraOriginBody,current.cameraOriginBody,true,coordinateLimit,minimumPivot);
                    result.rejectionReason := 9;
                    if edgeValid then
                      result.verified := true; result.rejectionReason := 0;
                      result.generation := current.generation;
                      result.referenceId := reference.id; result.currentId := current.id;
                      result.referenceEpoch := reference.epoch; result.currentEpoch := current.epoch;
                      result.inlierCount := inliers; result.rms := rms;
                      result.opticalRotation := C; result.opticalTranslation := t;
                      result.bodyRotation := D; result.bodyTranslation := u;
                      result.covariance := edgeCovariance; result.information := information;
                      for slot in 1:featureCapacity loop
                        result.inliers[slot] := mask[slot] == 1.0;
                        result.partners[slot] := if mask[slot] == 1.0 then integer(partners[slot]) else 0;
                      end for;
                    end if;
                  end if;
                end if;
              end if;
            end if;
          end if;
        end if;
      end if;
    end if;
  end Verify;