function

SLAM.LoopClosure.RGBDCatalogLoopVerification.VerifyCandidate

Public declarations

Explicit source declarations; dimensions, modifications and defaults are shown as written, without evaluation.

Role	Name	Type	Dimensions / attributes / default
input	`catalog`	`RGBDKeyframes.Catalog`
input	`current`	`RGBDKeyframes.Frame`
input	`candidateId`	`Integer`
input	`candidateSlot`	`Integer`
input	`appearanceScore`	`Real`
input	`requested`	`Boolean`
input	`seed`	`Integer`	`= 7`
input	`trials`	`Integer`	`= 96`
input	`refinements`	`Integer`	`= 4`
input	`minimumInliers`	`Integer`	`= 12`
input	`minimumFraction`	`Real`	`= 0.5`
input	`inlierDistance`	`Real`	`= 0.08`
input	`maximumRms`	`Real`	`= 0.03`
input	`minimumAge`	`Real`	`= 5.0`
input	`minimumSimilarity`	`Real`	`= 0.35`
input	`descriptorRatio`	`Real`	`= 0.8`
input	`maximumDescriptorDistance`	`Real`	`= 0.8`
input	`coordinateLimit`	`Real`	`= 100.0`
input	`rankTolerance`	`Real`	`= 1e-8`
input	`localizationSigma`	`Real`	`= 0.5`
input	`depthInflation`	`Real`	`= 1.0`
input	`minimumPivot`	`Real`	`= 1e-10`
output	`proposal`	`RGBDLoopVerification.Proposal`
input	`calibratedResiduals`	`Boolean`	`= true`
input	`maximumNormalizedSquared`	`Real`	`= 9.0`
input	`poseConsistentLoops`	`Boolean`	`= false`
input	`maximumPoseInnovation`	`Real`	`= 2.0`
input	`poseConsistencySquared`	`Real`	`= 9.0`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function VerifyCandidate
    input RGBDKeyframes.Catalog catalog;
    input RGBDKeyframes.Frame current;
    input Integer candidateId; input Integer candidateSlot;
    input Real appearanceScore; 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 minimumSimilarity = 0.35;
    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 RGBDLoopVerification.Proposal proposal;
    input Boolean calibratedResiduals = true;
    input Real maximumNormalizedSquared = 9.0;
    input Boolean poseConsistentLoops = false;
    input Real maximumPoseInnovation = 2.0;
    input Real poseConsistencySquared = 9.0;
  protected
    RGBDKeyframes.Frame reference;
    Boolean bound; Boolean poseConsistent; Real poseSquared;
  algorithm
    proposal := RGBDLoopVerification.EmptyProposal(seed);
    if requested then
      // Check domains before reading a slot. Capture will replace nextSlot,
      // so that identity cannot supply a retained loop endpoint.
      bound := RGBDKeyframes.ValidHeader(catalog)
        and candidateSlot >= 1 and candidateSlot <= RGBDKeyframes.keyframeCapacity
        and candidateSlot <> catalog.nextSlot and candidateId >= 1 and candidateId < catalog.nextId
        and current.generation == catalog.generation
        and current.vocabularyVersion == catalog.vocabularyVersion
        and current.id == catalog.nextId and current.epoch > catalog.lastEpoch
        and (catalog.nextId == 1 or current.imageTime > catalog.lastTime)
        and minimumSimilarity >= 0 and minimumSimilarity <= 1
        and appearanceScore >= minimumSimilarity and appearanceScore <= 1;
      proposal.rejectionReason := 10 "Catalog/appearance identity binding refused";
      if bound then
        bound := catalog.occupied[candidateSlot] and catalog.ids[candidateSlot] == candidateId
          and catalog.generations[candidateSlot] == current.generation
          and catalog.vocabularyVersions[candidateSlot] == current.vocabularyVersion;
        if bound then
          reference := RGBDKeyframes.ReadSlot(catalog,candidateSlot);
          proposal := RGBDLoopVerification.Verify(reference,current,true,seed,trials,refinements,
            minimumInliers,minimumFraction,inlierDistance,maximumRms,minimumAge,descriptorRatio,
            maximumDescriptorDistance,coordinateLimit,rankTolerance,localizationSigma,depthInflation,minimumPivot,
            calibratedResiduals=calibratedResiduals,maximumNormalizedSquared=maximumNormalizedSquared);
          if proposal.verified and poseConsistentLoops then
            (poseConsistent,poseSquared) := PoseConsistent(reference.bodyPosition,current.bodyPosition,
              reference.bodyRotation,reference.poseCovariance,current.poseCovariance,
              proposal.bodyTranslation,proposal.covariance[1:3,1:3],minimumPivot=minimumPivot,
              maximumSquared=poseConsistencySquared,maximumTranslationInnovation=maximumPoseInnovation);
            if not poseConsistent then
              proposal.verified := false; proposal.rejectionReason := 11;
            end if;
          end if;
        end if;
      end if;
    end if;
  end VerifyCandidate;