SLAM.PoseGraph.RGBDGraphSelectedGauge.SelectFromAnchor

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

function SelectFromAnchor
    input GraphGaugeUncertainty.Estimate previous;
    input RGBDKeyframes.Catalog catalog;
    input RGBDGraphMeasurements.State graph;
    input Context context;
    input Real finalPositionsBySlot[nodeCapacity,3]; input Real finalRotationsBySlot[nodeCapacity,3,3];
    input Integer currentNode; input Integer referenceNode;
    input GraphGaugeUncertainty.Binding binding;
    input RGBDGraphAnchorBound.Estimate anchor;
    input Integer factorProvenance; input Real beta; input Boolean requested = true;
    input Integer maximumPCG = 48; input Real tolerance = 1e-10;
    output Result result;
  protected Boolean valid; Integer id; Integer slot;
  algorithm
    result.estimate := previous; result.accepted := false; result.rejectionReason := 1;
    result.covarianceStatus := 0; result.transportReason := 0; result.roundoffCertified := false;
    if requested then
      result.rejectionReason := 7;
      valid := RGBDKeyframes.ValidHeader(catalog) and catalog.nextId > 1
        and GraphGaugeUncertainty.ValidBinding(binding);
      if valid then
        id := max(1,catalog.nextId-nodeCapacity); slot := mod(id-1,nodeCapacity)+1;
        valid := anchor.binding.generation == catalog.generation
          and anchor.binding.generation == context.generation and anchor.binding.generation == binding.generation
          and anchor.binding.sourceRevision == context.sourceRevision and anchor.binding.sourceRevision == binding.sourceRevision
          and anchor.binding.id == id and anchor.binding.id == binding.anchorId
          and anchor.binding.slot == slot and catalog.occupied[slot] and catalog.ids[slot] == id
          and context.captureIds[slot] == id
          and anchor.binding.epoch == catalog.epochs[slot] and anchor.binding.epoch == binding.anchorEpoch
          and anchor.binding.sequence == context.captureSequences[slot]
          and anchor.binding.sequence == binding.anchorCaptureSequence
          and anchor.binding.catalogPoseRevision == context.catalogPoseRevision
          and anchor.binding.catalogPoseRevision == binding.catalogPoseRevision
          and anchor.binding.imageTime == catalog.imageTimes[slot] and anchor.binding.imageTime == binding.anchorTime
          and anchor.binding.provenance >= 1 and anchor.binding.provenance <= identifierLimit
          and anchor.binding.provenance == binding.anchorBoundProvenance
          and max(abs(anchor.position-finalPositionsBySlot[slot,:])) == 0
          and max(abs(anchor.rotation-finalRotationsBySlot[slot,:,:])) == 0;
      end if;
      if valid then
        // The typed owner supplies a local ERROR SECOND-MOMENT bound about
        // this exact mean; it is not a centered/global covariance certificate.
        result := SelectAndTransport(previous,catalog,graph,context,finalPositionsBySlot,finalRotationsBySlot,
          currentNode,referenceNode,binding,anchor.position,anchor.rotation,anchor.bound,
          anchor.binding.provenance,factorProvenance,beta,true,maximumPCG,tolerance);
      end if;
    end if;
  end SelectFromAnchor;