SLAM.Localization.RGBDLocalizationCatalog.Publish

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

function Publish
    input State previous;
    input Estimator proposed "Same compiled localization invocation; no host reconstruction";
    input RGBDKeyframes.Frame measurement;
    input Real producerAccepted "Prediction accepted, or explicit initialization accepted";
    input Real observationAccepted; input Real captureAccepted;
    input Boolean imageOn; input Integer imageEpoch; input Real imageTime; input Real h;
    input Boolean initializing;
    input Boolean frameAccepted; input Integer frameRejectionReason;
    input Boolean requested;
    input Real candidatePoint[featureCapacity,dimension]; input Real candidateEnabled[featureCapacity];
    input Real vocabulary[RGBDKeyframes.wordCapacity,descriptorSize];
    input Real vocabularyEnabled[RGBDKeyframes.wordCapacity];
    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 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 posePositions[RGBDKeyframes.keyframeCapacity,dimension] = previous.catalog.bodyPositions;
    input Real poseRotations[RGBDKeyframes.keyframeCapacity,dimension,dimension] = previous.catalog.bodyRotations;
    input Integer poseRevision = previous.map.catalogRevision;
    output Result result;
    input Real loopMinimumSimilarity = 0.35;
    input Real loopMinimumAge = 5.0;
    input Integer loopMinimumInliers = 12;
    input Real loopMinimumFraction = 0.5;
    input Real loopInlierDistance = 0.08;
    input Real loopMaximumRms = 0.03;
    input Boolean loopCalibratedResiduals = true;
    input Real loopMaximumNormalizedSquared = 9.0;
    input Real loopMaximumPoseInnovation = 2.0;
    input Real loopPoseConsistencySquared = 9.0;
    input Boolean loopPoseConsistency = true;
  protected
    Boolean valid; Boolean frameBound;
    RGBDCatalogMapping.Result mapped;
  algorithm
    result.next := previous; result.accepted := false; result.rejectionReason := 1;
    result.imageCompleted := false; result.mappingAccepted := false;
    result.frameRejectionReason := frameRejectionReason;
    result.decision.valid := false; result.decision.captureRequested := false; result.decision.reason := 1;
    result.decision.enabledCount := 0; result.decision.elapsed := 0.0;
    result.decision.translationSquared := 0.0; result.decision.rotationCosine := 1.0;
    result.mapDiagnostics := RGBDCatalogMapping.EmptyDiagnostics();
    result.problem := RGBDGraphMeasurements.EmptyProblem();
    if requested then
      result.rejectionReason := 2;
      valid := ValidHeader(previous) and ValidEstimator(previous.estimator);
      if valid then
        result.rejectionReason := 3;
        valid := imageTime >= 0.0 and imageTime <= 1e9;
        if initializing then
          valid := valid and not previous.initialized and imageOn
            and SLAMExactRealEqual(imageTime,0.0) and SLAMExactRealEqual(h,0.0);
        else
          valid := valid and previous.initialized and ES15HeldIntervalValid(h)
            and imageTime > previous.predictionTime
            and abs(imageTime-previous.predictionTime-h) <= 1e-12*max(1.0,abs(imageTime));
        end if;
        if imageOn then valid := valid and ImageFresh(previous,imageEpoch,imageTime); end if;
        if valid then
          result.rejectionReason := 4;
          valid := SLAMExactRealEqual(producerAccepted,1.0)
            and (SLAMExactRealEqual(observationAccepted,0.0) or SLAMExactRealEqual(observationAccepted,1.0))
            and (SLAMExactRealEqual(captureAccepted,0.0) or SLAMExactRealEqual(captureAccepted,1.0))
            and (imageOn or (SLAMExactRealEqual(observationAccepted,0.0) and SLAMExactRealEqual(captureAccepted,0.0)))
            and (not initializing or SLAMExactRealEqual(observationAccepted,0.0));
          if valid then
            result.rejectionReason := 5; valid := ValidEstimator(proposed);
            if valid then
              result.rejectionReason := 6;
              valid := proposed.lastUsedEpoch >= previous.estimator.lastUsedEpoch
                and (not imageOn or proposed.lastUsedEpoch <= imageEpoch);
              if proposed.lastUsedEpoch > previous.estimator.lastUsedEpoch then
                valid := valid and imageOn and SLAMExactRealEqual(proposed.lastUsedEpoch,imageEpoch)
                  and SLAMExactRealEqual(proposed.referenceUsed,1.0);
              end if;
              if SLAMExactRealEqual(observationAccepted,1.0) then
                valid := valid and SLAMExactRealEqual(proposed.lastUsedEpoch,imageEpoch)
                  and SLAMExactRealEqual(proposed.referenceUsed,1.0)
                  and SLAMExactRealEqual(captureAccepted,0.0);
              end if;
              if SLAMExactRealEqual(captureAccepted,1.0) then
                valid := valid and previous.referenceBirth.sequence < identifierLimit
                  and SLAMExactRealEqual(proposed.referenceAvailable,1.0)
                  and SLAMExactRealEqual(proposed.referenceEpoch,imageEpoch)
                  and SLAMExactRealEqual(proposed.referenceUsed,0.0)
                  and imageEpoch > previous.estimator.lastUsedEpoch;
                for axis in 1:dimension loop
                  valid := valid and SLAMExactRealEqual(proposed.referencePosition[axis],proposed.position[axis]);
                  for column in 1:dimension loop
                    valid := valid and SLAMExactRealEqual(proposed.referenceRotation[axis,column],proposed.rotation[axis,column]);
                  end for;
                end for;
                for row in 1:currentDimension loop
                  for column in 1:referenceDimension loop
                    valid := valid and SLAMExactRealEqual(proposed.crossCovariance[row,column],
                      proposed.covariance[row,RGBDLocalizationFrame.poseIndices[column]]);
                  end for;
                end for;
                for row in 1:referenceDimension loop
                  for column in 1:referenceDimension loop
                    valid := valid and SLAMExactRealEqual(proposed.referenceCovariance[row,column],
                      proposed.covariance[RGBDLocalizationFrame.poseIndices[row],RGBDLocalizationFrame.poseIndices[column]]);
                  end for;
                end for;
              else
                valid := valid and ReferenceHeld(previous.estimator,proposed)
                  and proposed.referenceUsed >= previous.estimator.referenceUsed;
              end if;
              if not imageOn then
                valid := valid and SLAMExactRealEqual(proposed.lastUsedEpoch,previous.estimator.lastUsedEpoch)
                  and SLAMExactRealEqual(proposed.referenceUsed,previous.estimator.referenceUsed);
              end if;
              if valid then
                // Completed localization and its raw-noise ledger commit even
                // when optional Frame/BoW/registration/map admission refuses.
                result.next.estimator := proposed; result.next.initialized := true;
                if SLAMExactRealEqual(captureAccepted,0.0) then
                  // Retained image payload is immutable, including unavailable
                  // and disabled cells. Copy its owner rather than comparing NaN
                  // payload with numerical equality or inventing canonical data.
                  result.next.estimator.referenceDescriptor := previous.estimator.referenceDescriptor;
                  result.next.estimator.referencePoint := previous.estimator.referencePoint;
                  result.next.estimator.referenceEnabled := previous.estimator.referenceEnabled;
                  result.next.estimator.referencePixels := previous.estimator.referencePixels;
                  result.next.estimator.referenceCount := previous.estimator.referenceCount;
                  result.next.estimator.referenceRgbCalibration := previous.estimator.referenceRgbCalibration;
                  result.next.estimator.referenceDepthCalibration := previous.estimator.referenceDepthCalibration;
                  result.next.estimator.referenceNoiseReferenceFx := previous.estimator.referenceNoiseReferenceFx;
                  result.next.estimator.referenceDisparityNoise := previous.estimator.referenceDisparityNoise;
                  result.next.estimator.referenceBaseline := previous.estimator.referenceBaseline;
                  result.next.estimator.referenceOpticalToBody := previous.estimator.referenceOpticalToBody;
                  result.next.estimator.referenceCameraOriginBody := previous.estimator.referenceCameraOriginBody;
                  result.next.estimator.referencePosition := previous.estimator.referencePosition;
                  result.next.estimator.referenceRotation := previous.estimator.referenceRotation;
                  result.next.estimator.referenceCovariance := previous.estimator.referenceCovariance;
                  if SLAMExactRealEqual(previous.estimator.referenceAvailable,0.0) then
                    result.next.estimator.crossCovariance := previous.estimator.crossCovariance;
                  end if;
                end if;
                result.next.predictionTime := imageTime; result.next.steps := previous.steps+1;
                result.accepted := true; result.rejectionReason := 0;
                if SLAMExactRealEqual(captureAccepted,1.0) then
                  result.next.referenceBirth.generation := previous.generation;
                  result.next.referenceBirth.epoch := imageEpoch;
                  result.next.referenceBirth.sequence := previous.steps+1;
                  result.next.referenceBirth.catalogId := 0;
                end if;
                if imageOn then
                  result.imageCompleted := true;
                  result.next.lastProcessedImageEpoch := imageEpoch;
                  result.next.lastProcessedImageTime := imageTime;
                  // Check binding in addition to the builder's own certificate.
                  // Pose/covariance are from the same accepted localization.
                  frameBound := FrameBound(previous,proposed,measurement,imageEpoch,imageTime,
                    observationAccepted,captureAccepted,frameAccepted);
                  if frameAccepted and not frameBound then result.frameRejectionReason := 8; end if;
                  if frameBound then
                    (mapped,result.decision) := RGBDCatalogFrame.Advance(previous.catalog,previous.graph,previous.map,
                      measurement,vocabulary,vocabularyEnabled,candidatePoint,candidateEnabled,true,1.0,true,
                      minimumInterval=minimumInterval,maximumInterval=maximumInterval,
                      translationThreshold=translationThreshold,rotationThreshold=rotationThreshold,minimumFeatures=minimumFeatures,
                      voxelWidth=voxelWidth,mergeRadius=mergeRadius,maximumDistance=maximumDistance,
                      tentativeLifetime=tentativeLifetime,confirmedLifetime=confirmedLifetime,
                      confirmationObservations=confirmationObservations,maximumConfidence=maximumConfidence,maximumTentative=maximumTentative,
                      nodePosition=posePositions,nodeRotation=poseRotations,poseRevision=poseRevision,
                      loopMinimumSimilarity=loopMinimumSimilarity,
                      loopMinimumAge=loopMinimumAge,
                      loopMinimumInliers=loopMinimumInliers,
                      loopMinimumFraction=loopMinimumFraction,
                      loopInlierDistance=loopInlierDistance,
                      loopMaximumRms=loopMaximumRms,
                      loopCalibratedResiduals=loopCalibratedResiduals,
                      loopMaximumNormalizedSquared=loopMaximumNormalizedSquared,
      loopMaximumPoseInnovation=loopMaximumPoseInnovation, loopPoseConsistencySquared=loopPoseConsistencySquared, loopPoseConsistency=loopPoseConsistency);
                    result.mapDiagnostics := mapped.diagnostics; result.problem := mapped.problem;
                    if mapped.accepted then
                      result.next.catalog := mapped.catalog; result.next.graph := mapped.graph; result.next.map := mapped.map;
                      result.mappingAccepted := true;
                      if result.next.catalog.nextId == previous.catalog.nextId+1
                        and result.next.referenceBirth.epoch == imageEpoch
                        and result.next.referenceBirth.generation == previous.generation then
                        result.next.referenceBirth.catalogId := measurement.id;
                      end if;
                    end if;
                  end if;
                end if;
              end if;
            end if;
          end if;
        end if;
      end if;
    end if;
  end Publish;