function

SLAM.Mapping.RGBDCatalogPoseMapping.Advance

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	`graph`	`RGBDGraphMeasurements.State`
input	`map`	`RGBDCatalogMapping.State`
input	`poses`	`PoseView`
input	`sourceRevision`	`Integer`
input	`measurement`	`RGBDKeyframes.Frame`
input	`vocabulary`	`Real`	`[RGBDKeyframes.wordCapacity,RGBDKeyframes.descriptorSize]`
input	`vocabularyEnabled`	`Real`	`[RGBDKeyframes.wordCapacity]`
input	`candidatePoint`	`Real`	`[RGBDKeyframes.featureCapacity,RGBDKeyframes.dimension]`
input	`candidateEnabled`	`Real`	`[RGBDKeyframes.featureCapacity]`
input	`imageFresh`	`Boolean`
input	`poseAccepted`	`Real`
input	`requested`	`Boolean`
input	`minimumInterval`	`Real`	`= 0.5`
input	`maximumInterval`	`Real`	`= 2.0`
input	`translationThreshold`	`Real`	`= 0.6`
input	`rotationThreshold`	`Real`	`= 0.25`
input	`minimumFeatures`	`Integer`	`= 12`
input	`seeds`	`Integer`	`[RGBDKeyframeRetrieval.proposalCapacity] = fill(7,RGBDKeyframeRetrieval.proposalCapacity)`
input	`sequentialSeed`	`Integer`	`= 7`
input	`maximumWordDistanceSquared`	`Real`	`= 0.8`
input	`minimumAssignments`	`Integer`	`= 8`
input	`minimumSimilarity`	`Real`	`= 0.35`
input	`minimumAge`	`Real`	`= 5.0`
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	`descriptorRatio`	`Real`	`= 0.8`
input	`maximumDescriptorDistance`	`Real`	`= 0.8`
input	`registrationCoordinateLimit`	`Real`	`= 100.0`
input	`rankTolerance`	`Real`	`= 1e-8`
input	`localizationSigma`	`Real`	`= 0.5`
input	`depthInflation`	`Real`	`= 1.0`
input	`minimumPivot`	`Real`	`= 1e-10`
input	`coordinateLimit`	`Real`	`= 1e6`
input	`voxelWidth`	`Real`	`= 0.25`
input	`mergeRadius`	`Real`	`= 0.15`
input	`maximumDistance`	`Real`	`= 80.0`
input	`tentativeLifetime`	`Real`	`= 0.5`
input	`confirmedLifetime`	`Real`	`= 5.0`
input	`confirmationObservations`	`Real`	`= 3.0`
input	`maximumConfidence`	`Real`	`= 8.0`
input	`maximumTentative`	`Real`	`= 700.0`
input	`consistencyTolerance`	`Real`	`= 1e-6`
output	`result`	`Result`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function Advance
    input RGBDKeyframes.Catalog catalog;
    input RGBDGraphMeasurements.State graph;
    input RGBDCatalogMapping.State map;
    input PoseView poses; input Integer sourceRevision;
    input RGBDKeyframes.Frame measurement;
    input Real vocabulary[RGBDKeyframes.wordCapacity,RGBDKeyframes.descriptorSize];
    input Real vocabularyEnabled[RGBDKeyframes.wordCapacity];
    input Real candidatePoint[RGBDKeyframes.featureCapacity,RGBDKeyframes.dimension];
    input Real candidateEnabled[RGBDKeyframes.featureCapacity];
    input Boolean imageFresh;
    input Real poseAccepted;
    input Boolean requested;
    input Real minimumInterval = 0.5; input Real maximumInterval = 2.0;
    input Real translationThreshold = 0.6; input Real rotationThreshold = 0.25;
    input Integer minimumFeatures = 12;
    input Integer seeds[RGBDKeyframeRetrieval.proposalCapacity] = fill(7,RGBDKeyframeRetrieval.proposalCapacity);
    input Integer sequentialSeed = 7;
    input Real maximumWordDistanceSquared = 0.8; input Integer minimumAssignments = 8;
    input Real minimumSimilarity = 0.35; input Real minimumAge = 5.0;
    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 descriptorRatio = 0.8; input Real maximumDescriptorDistance = 0.8;
    input Real registrationCoordinateLimit = 100.0; input Real rankTolerance = 1e-8;
    input Real localizationSigma = 0.5; input Real depthInflation = 1.0; input Real minimumPivot = 1e-10;
    input Real coordinateLimit = 1e6; input Real voxelWidth = 0.25;
    input Real mergeRadius = 0.15; input Real maximumDistance = 80.0;
    input Real tentativeLifetime = 0.5; input Real confirmedLifetime = 5.0;
    input Real confirmationObservations = 3.0; input Real maximumConfidence = 8.0;
    input Real maximumTentative = 700.0; input Real consistencyTolerance = 1e-6;
    output Result result;
  protected Integer slot;
  algorithm
    result.poses := poses; result.accepted := false; result.rejectionReason := 1;
    result.mapping.catalog := catalog; result.mapping.graph := graph; result.mapping.map := map;
    result.mapping.accepted := false; result.mapping.rejectionReason := 1;
    result.mapping.diagnostics := RGBDCatalogMapping.EmptyDiagnostics(); result.mapping.problem := RGBDGraphMeasurements.EmptyProblem();
    result.decision.valid := false; result.decision.captureRequested := false; result.decision.reason := 1;
    result.decision.enabledCount := 0; result.decision.elapsed := 0; result.decision.translationSquared := 0; result.decision.rotationCosine := 1;
    if requested then
      result.rejectionReason := 2;
      if ValidView(catalog,poses,sourceRevision) then
        (result.mapping,result.decision) := RGBDCatalogFrame.Advance(
          catalog,graph,map,measurement,vocabulary,vocabularyEnabled,candidatePoint,candidateEnabled,imageFresh,poseAccepted,requested,minimumInterval,maximumInterval,translationThreshold,rotationThreshold,minimumFeatures,seeds,sequentialSeed,maximumWordDistanceSquared,minimumAssignments,minimumSimilarity,minimumAge,trials,refinements,minimumInliers,minimumFraction,inlierDistance,maximumRms,descriptorRatio,maximumDescriptorDistance,registrationCoordinateLimit,rankTolerance,localizationSigma,depthInflation,minimumPivot,coordinateLimit,voxelWidth,mergeRadius,maximumDistance,tentativeLifetime,confirmedLifetime,confirmationObservations,maximumConfidence,maximumTentative,consistencyTolerance,poses.positions,poses.rotations,poses.revision);
        result.rejectionReason := 3;
        if result.mapping.accepted then
          if result.mapping.catalog.nextId == catalog.nextId+1 then
            slot := catalog.nextSlot;
            result.poses.revision := poses.revision+1; result.poses.catalogNextId := result.mapping.catalog.nextId;
            result.poses.enabled[slot] := true; result.poses.ids[slot] := result.mapping.catalog.ids[slot];
            result.poses.positions[slot,:] := result.mapping.catalog.bodyPositions[slot,:];
            result.poses.rotations[slot,:,:] := result.mapping.catalog.bodyRotations[slot,:,:];
          end if;
          result.accepted := true; result.rejectionReason := 0;
        end if;
      end if;
    end if;
  end Advance;