SLAM.Localization.RGBDLocalizationCatalog

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

package RGBDLocalizationCatalog
  import ES15HeldIntervalValid = SLAM.Inertial.ES15HeldIntervalValid;
  import RGBDCatalogFrame = SLAM.Mapping.RGBDCatalogFrame;
  import RGBDCatalogMapping = SLAM.Mapping.RGBDCatalogMapping;
  import RGBDGraphMeasurements = SLAM.PoseGraph.RGBDGraphMeasurements;
  import RGBDKeyframePolicy = SLAM.LoopClosure.RGBDKeyframePolicy;
  import RGBDKeyframes = SLAM.LoopClosure.RGBDKeyframes;
  import RGBDLocalizationFrame = SLAM.Localization.RGBDLocalizationFrame;
  import RGBDMapAnchors = SLAM.Mapping.RGBDMapAnchors;
  import SLAMCovariancePSDCheck = SLAM.Inertial.SLAMCovariancePSDCheck;
  import SLAMExactRealEqual = SLAM.Inertial.SLAMExactRealEqual;

  constant Integer dimension = RGBDKeyframes.dimension;
  constant Integer currentDimension = RGBDLocalizationFrame.currentDimension;
  constant Integer referenceDimension = RGBDKeyframes.poseDimension;
  constant Integer featureCapacity = RGBDKeyframes.featureCapacity;
  constant Integer descriptorSize = RGBDKeyframes.descriptorSize;
  constant Integer identifierLimit = RGBDKeyframes.identifierLimit;

  record Estimator
    Real position[dimension]; Real velocity[dimension]; Real rotation[dimension,dimension];
    Real accelBias[dimension]; Real gyroBias[dimension];
    Real nullspaceDirection[9] "Carried global-yaw nullspace direction of the observability-constrained prediction";
    Real covariance[currentDimension,currentDimension];
    Real crossCovariance[currentDimension,referenceDimension];
    Real referenceCovariance[referenceDimension,referenceDimension];
    Real referencePosition[dimension]; Real referenceRotation[dimension,dimension];
    Real referenceAvailable; Real referenceEpoch; Real referenceUsed; Real lastUsedEpoch;
    Real referenceDescriptor[featureCapacity,descriptorSize];
    Real referencePoint[featureCapacity,dimension]; Real referenceEnabled[featureCapacity];
    Real referencePixels[featureCapacity,2]; Real referenceCount;
    Real referenceRgbCalibration[4]; Real referenceDepthCalibration[4];
    Real referenceNoiseReferenceFx; Real referenceDisparityNoise; Real referenceBaseline;
    Real referenceOpticalToBody[dimension,dimension]; Real referenceCameraOriginBody[dimension];
  end Estimator;

  record ReferenceBirth
    Integer generation; Integer epoch;
    Integer sequence "Accepted processing-step identity at reference birth, not a replacement counter";
    Integer catalogId "0 means this local reference has no catalog capture binding";
  end ReferenceBirth;

  record State
    Estimator estimator;
    RGBDKeyframes.Catalog catalog;
    RGBDGraphMeasurements.State graph;
    RGBDCatalogMapping.State map;
    Integer generation; Integer sourceRevision;
    Boolean initialized;
    Real predictionTime; Integer steps;
    Integer lastProcessedImageEpoch; Real lastProcessedImageTime;
    ReferenceBirth referenceBirth;
  end State;

  record Result
    State next;
    Boolean accepted; Integer rejectionReason "0 accepted; 1 idle; 2 binding; 3 chronology; 4 producer; 5 estimator; 6 reference lifecycle";
    Boolean imageCompleted; Boolean mappingAccepted;
    Integer frameRejectionReason;
    RGBDKeyframePolicy.Decision decision;
    RGBDCatalogMapping.Diagnostics mapDiagnostics;
    RGBDGraphMeasurements.Problem problem;
  end Result;

  record Receipt "The outcome of a publication without its state, for callers that commit next themselves"
    Boolean accepted; Integer rejectionReason "0 accepted; 1 idle; 2 binding; 3 chronology; 4 producer; 5 estimator; 6 reference lifecycle";
    Boolean imageCompleted; Boolean mappingAccepted;
    Integer frameRejectionReason;
    RGBDKeyframePolicy.Decision decision;
    RGBDCatalogMapping.Diagnostics mapDiagnostics;
    RGBDGraphMeasurements.Problem problem;
  end Receipt;

  function HeldReceipt "The receipt of a publication that was not requested: idle, nothing completed"
    input Integer frameRejectionReason;
    output Receipt receipt;
  algorithm
    receipt.accepted := false; receipt.rejectionReason := 1;
    receipt.imageCompleted := false; receipt.mappingAccepted := false;
    receipt.frameRejectionReason := frameRejectionReason;
    receipt.decision.valid := false; receipt.decision.captureRequested := false; receipt.decision.reason := 1;
    receipt.decision.enabledCount := 0; receipt.decision.elapsed := 0.0;
    receipt.decision.translationSquared := 0.0; receipt.decision.rotationCosine := 1.0;
    receipt.mapDiagnostics := RGBDCatalogMapping.EmptyDiagnostics();
    receipt.problem := RGBDGraphMeasurements.EmptyProblem();
  end HeldReceipt;

  function ReceiptOf
    input Result result;
    output Receipt receipt;
  algorithm
    receipt.accepted := result.accepted; receipt.rejectionReason := result.rejectionReason;
    receipt.imageCompleted := result.imageCompleted; receipt.mappingAccepted := result.mappingAccepted;
    receipt.frameRejectionReason := result.frameRejectionReason;
    receipt.decision := result.decision;
    receipt.mapDiagnostics := result.mapDiagnostics;
    receipt.problem := result.problem;
  end ReceiptOf;

  function EmptyEstimator
    input Real position[dimension] = zeros(dimension);
    input Real velocity[dimension] = zeros(dimension);
    input Real rotation[dimension,dimension] = identity(dimension);
    input Real accelBias[dimension] = zeros(dimension);
    input Real gyroBias[dimension] = zeros(dimension);
    input Real covariance[currentDimension,currentDimension] = diagonal({0.25,0.25,0.25,
      0.04,0.04,0.04,0.01,0.01,0.01,0.0004,0.0004,0.0004,0.000025,0.000025,0.000025});
    input Real nullspaceDirection[9] = zeros(9);
    output Estimator result;
  algorithm
    result.position := position; result.velocity := velocity; result.rotation := rotation;
    result.accelBias := accelBias; result.gyroBias := gyroBias; result.covariance := covariance;
    result.nullspaceDirection := nullspaceDirection;
    result.crossCovariance := zeros(currentDimension,referenceDimension);
    result.referenceCovariance := zeros(referenceDimension,referenceDimension);
    result.referencePosition := zeros(dimension); result.referenceRotation := identity(dimension);
    result.referenceAvailable := 0.0; result.referenceEpoch := 0.0;
    result.referenceUsed := 0.0; result.lastUsedEpoch := -1.0;
    result.referenceDescriptor := zeros(featureCapacity,descriptorSize);
    result.referencePoint := zeros(featureCapacity,dimension); result.referenceEnabled := zeros(featureCapacity);
    result.referencePixels := zeros(featureCapacity,2); result.referenceCount := 0.0;
    result.referenceRgbCalibration := {1.0,1.0,0.0,0.0};
    result.referenceDepthCalibration := {1.0,1.0,0.0,0.0};
    result.referenceNoiseReferenceFx := 1.0; result.referenceDisparityNoise := 0.08;
    result.referenceBaseline := 0.05; result.referenceOpticalToBody := identity(dimension);
    result.referenceCameraOriginBody := zeros(dimension);
  end EmptyEstimator;

  function Empty
    input Estimator estimator;
    input Integer generation = 1;
    input Integer sourceRevision = 1;
    input Integer vocabularyVersion = 1;
    input Real worldFrame = 0.0;
    output State result;
  algorithm
    result.estimator := estimator;
    result.catalog := RGBDKeyframes.Empty(generation,vocabularyVersion);
    result.graph := RGBDGraphMeasurements.Empty(generation);
    result.map := RGBDCatalogMapping.Empty(generation,worldFrame);
    result.generation := generation; result.sourceRevision := sourceRevision;
    result.initialized := false; result.predictionTime := 0.0; result.steps := 0;
    result.lastProcessedImageEpoch := -1; result.lastProcessedImageTime := 0.0;
    result.referenceBirth.generation := generation; result.referenceBirth.epoch := -1;
    result.referenceBirth.sequence := 0; result.referenceBirth.catalogId := 0;
  end Empty;

  function ValidEstimator
    input Estimator value;
    output Boolean valid;
  protected
    Real joint[currentDimension+referenceDimension,currentDimension+referenceDimension];
  algorithm
    valid := RGBDMapAnchors.ProperRotation(value.rotation)
      and SLAMCovariancePSDCheck(value.covariance,1e-12) == 1.0
      and (SLAMExactRealEqual(value.referenceAvailable,0.0) or SLAMExactRealEqual(value.referenceAvailable,1.0))
      and (SLAMExactRealEqual(value.referenceUsed,0.0) or SLAMExactRealEqual(value.referenceUsed,1.0))
      and value.lastUsedEpoch >= -1.0 and value.lastUsedEpoch <= identifierLimit
      and SLAMExactRealEqual(value.lastUsedEpoch,floor(value.lastUsedEpoch));
    for axis in 1:dimension loop
      valid := valid and abs(value.position[axis]) <= 1e6 and abs(value.velocity[axis]) <= 1e6
        and abs(value.accelBias[axis]) <= 2.0 and abs(value.gyroBias[axis]) <= 0.3;
    end for;
    // Unavailable reference buffers stay opaque, matching the propagation owner.
    if SLAMExactRealEqual(value.referenceAvailable,0.0) then
      valid := valid and SLAMExactRealEqual(value.referenceUsed,0.0);
    elseif SLAMExactRealEqual(value.referenceAvailable,1.0) then
      valid := valid and RGBDMapAnchors.ProperRotation(value.referenceRotation)
        and value.referenceEpoch >= 0.0 and value.referenceEpoch <= identifierLimit
        and SLAMExactRealEqual(value.referenceEpoch,floor(value.referenceEpoch))
        and ((SLAMExactRealEqual(value.referenceUsed,0.0) and value.referenceEpoch > value.lastUsedEpoch)
          or (SLAMExactRealEqual(value.referenceUsed,1.0) and value.referenceEpoch <= value.lastUsedEpoch));
      for axis in 1:dimension loop
        valid := valid and abs(value.referencePosition[axis]) <= 1e6;
      end for;
      joint := cat(1,cat(2,value.covariance,value.crossCovariance),
        cat(2,transpose(value.crossCovariance),value.referenceCovariance));
      valid := valid and SLAMCovariancePSDCheck(joint,1e-12) == 1.0;
    end if;
  end ValidEstimator;

  function ValidHeader
    input State state;
    output Boolean valid;
  algorithm
    valid := state.generation >= 1 and state.generation <= identifierLimit
      and state.sourceRevision >= 1 and state.sourceRevision <= identifierLimit
      and state.steps >= 0 and state.steps < identifierLimit
      and state.predictionTime >= 0.0 and state.predictionTime <= 1e9
      and state.lastProcessedImageEpoch >= -1 and state.lastProcessedImageEpoch <= identifierLimit
      and state.lastProcessedImageTime >= 0.0 and state.lastProcessedImageTime <= state.predictionTime
      and RGBDKeyframes.ValidHeader(state.catalog)
      and state.catalog.generation == state.generation and state.graph.generation == state.generation
      and state.map.generation == state.generation
      and state.graph.revision >= 0 and state.graph.revision < identifierLimit
      and state.graph.lastCaptureId == state.catalog.nextId-1
      and state.graph.nextEdgeId >= 1 and state.graph.nextEdgeId <= identifierLimit
      and state.map.catalogRevision >= 0 and state.map.catalogRevision <= state.graph.revision
      and state.catalog.lastEpoch <= state.lastProcessedImageEpoch
      and state.map.imageEpoch >= -1 and state.map.imageEpoch <= state.lastProcessedImageEpoch
      and state.catalog.lastTime <= state.predictionTime and state.map.imageTime >= 0.0 and state.map.imageTime <= state.predictionTime
      and state.map.frame >= 0.0 and state.map.frame < identifierLimit
      and SLAMExactRealEqual(state.map.frame,floor(state.map.frame))
      and abs(state.map.worldFrame) <= 1e9
      and state.referenceBirth.generation == state.generation;
    if state.initialized then
      valid := valid and state.steps >= 1;
    else
      valid := valid and state.steps == 0 and SLAMExactRealEqual(state.predictionTime,0.0)
        and state.lastProcessedImageEpoch == -1 and state.catalog.nextId == 1
        and state.graph.revision == 0 and SLAMExactRealEqual(state.map.frame,0.0)
        and SLAMExactRealEqual(state.estimator.referenceAvailable,0.0)
        and SLAMExactRealEqual(state.estimator.lastUsedEpoch,-1.0);
    end if;
    if SLAMExactRealEqual(state.estimator.referenceAvailable,1.0) then
      valid := valid and state.referenceBirth.sequence >= 1 and state.referenceBirth.sequence <= state.steps
        and state.referenceBirth.epoch >= 0 and state.referenceBirth.epoch <= state.lastProcessedImageEpoch
        and SLAMExactRealEqual(state.estimator.referenceEpoch,state.referenceBirth.epoch)
        and state.referenceBirth.catalogId >= 0 and state.referenceBirth.catalogId < state.catalog.nextId;
    else
      valid := valid and state.referenceBirth.sequence == 0 and state.referenceBirth.epoch == -1
        and state.referenceBirth.catalogId == 0;
    end if;
  end ValidHeader;

  function FrameBound
    input State previous; input Estimator proposed; input RGBDKeyframes.Frame measurement;
    input Integer imageEpoch; input Real imageTime;
    input Real observationAccepted; input Real captureAccepted; input Boolean frameAccepted;
    output Boolean frameBound;
  algorithm
    frameBound := frameAccepted and measurement.generation == previous.generation
      and measurement.id == previous.catalog.nextId and measurement.epoch == imageEpoch
      and SLAMExactRealEqual(measurement.imageTime,imageTime)
      and measurement.vocabularyVersion == previous.catalog.vocabularyVersion
      and (SLAMExactRealEqual(observationAccepted,1.0) or SLAMExactRealEqual(captureAccepted,1.0));
    if frameBound then
      for axis in 1:dimension loop
        frameBound := frameBound and SLAMExactRealEqual(measurement.bodyPosition[axis],proposed.position[axis]);
        for column in 1:dimension loop
          frameBound := frameBound and SLAMExactRealEqual(measurement.bodyRotation[axis,column],proposed.rotation[axis,column]);
        end for;
      end for;
      for row in 1:referenceDimension loop
        for column in 1:referenceDimension loop
          frameBound := frameBound and SLAMExactRealEqual(measurement.poseCovariance[row,column],
            proposed.covariance[RGBDLocalizationFrame.poseIndices[row],RGBDLocalizationFrame.poseIndices[column]]);
        end for;
      end for;
    end if;
  end FrameBound;

  function ImageFresh
    input State previous; input Integer imageEpoch; input Real imageTime;
    output Boolean fresh;
  algorithm
    fresh := imageEpoch >= 0 and imageEpoch <= identifierLimit
      and imageEpoch > previous.lastProcessedImageEpoch
      and imageEpoch > previous.catalog.lastEpoch and imageEpoch > previous.map.imageEpoch
      and imageTime >= 0.0 and imageTime <= 1e9
      and (previous.lastProcessedImageEpoch == -1 or imageTime > previous.lastProcessedImageTime);
  end ImageFresh;

  function CanAdvance
    input State previous; input Real intervalTime; input Real h;
    input Boolean initializing; input Boolean requested;
    output Boolean allowed;
  algorithm
    allowed := false;
    if requested then
      allowed := ValidHeader(previous) and intervalTime >= 0.0 and intervalTime <= 1e9;
      if initializing then
        allowed := allowed and not previous.initialized
          and SLAMExactRealEqual(intervalTime,0.0) and SLAMExactRealEqual(h,0.0);
      else
        allowed := allowed and previous.initialized and ES15HeldIntervalValid(h)
          and intervalTime > previous.predictionTime
          and abs(intervalTime-previous.predictionTime-h) <= 1e-12*max(1.0,abs(intervalTime));
      end if;
    end if;
  end CanAdvance;

  function ReferenceHeld
    input Estimator previous; input Estimator proposed;
    output Boolean held;
  algorithm
    held := SLAMExactRealEqual(previous.referenceAvailable,proposed.referenceAvailable)
      and SLAMExactRealEqual(previous.referenceEpoch,proposed.referenceEpoch);
    if SLAMExactRealEqual(previous.referenceAvailable,1.0) then
      held := held and SLAMExactRealEqual(previous.referenceCount,proposed.referenceCount)
        and SLAMExactRealEqual(previous.referenceNoiseReferenceFx,proposed.referenceNoiseReferenceFx)
        and SLAMExactRealEqual(previous.referenceDisparityNoise,proposed.referenceDisparityNoise)
        and SLAMExactRealEqual(previous.referenceBaseline,proposed.referenceBaseline);
      for axis in 1:dimension loop
        held := held and SLAMExactRealEqual(previous.referencePosition[axis],proposed.referencePosition[axis])
          and SLAMExactRealEqual(previous.referenceCameraOriginBody[axis],proposed.referenceCameraOriginBody[axis]);
        for column in 1:dimension loop
          held := held and SLAMExactRealEqual(previous.referenceRotation[axis,column],proposed.referenceRotation[axis,column])
            and SLAMExactRealEqual(previous.referenceOpticalToBody[axis,column],proposed.referenceOpticalToBody[axis,column]);
        end for;
      end for;
      for coordinate in 1:4 loop
        held := held and SLAMExactRealEqual(previous.referenceRgbCalibration[coordinate],proposed.referenceRgbCalibration[coordinate])
          and SLAMExactRealEqual(previous.referenceDepthCalibration[coordinate],proposed.referenceDepthCalibration[coordinate]);
      end for;
      for row in 1:referenceDimension loop
        for column in 1:referenceDimension loop
          held := held and SLAMExactRealEqual(previous.referenceCovariance[row,column],proposed.referenceCovariance[row,column]);
        end for;
      end for;
      for feature in 1:featureCapacity loop
        held := held and SLAMExactRealEqual(previous.referenceEnabled[feature],proposed.referenceEnabled[feature]);
        if SLAMExactRealEqual(previous.referenceEnabled[feature],1.0) then
          for axis in 1:dimension loop
            held := held and SLAMExactRealEqual(previous.referencePoint[feature,axis],proposed.referencePoint[feature,axis]);
          end for;
          for coordinate in 1:2 loop
            held := held and SLAMExactRealEqual(previous.referencePixels[feature,coordinate],proposed.referencePixels[feature,coordinate]);
          end for;
          for coordinate in 1:descriptorSize loop
            held := held and SLAMExactRealEqual(previous.referenceDescriptor[feature,coordinate],proposed.referenceDescriptor[feature,coordinate]);
          end for;
        end if;
      end for;
    end if;
  end ReferenceHeld;

  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;
end RGBDLocalizationCatalog;