SLAM.PoseGraph.RGBDGraphEstimatorCommit.Commit

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

function Commit
    input State previous;
    input Proposal proposal;
    input GraphGaugeUncertainty.Binding expectedBinding;
    input SchmidtGraphPoseCorrection.Attempt previousAttempt;
    input SchmidtGraphPoseCorrection.Policy policy;
    input Boolean requested;
    input Real consistencyTolerance = 1e-6;
    input Real maximumConfidence = 8;
    output Result result;
  protected
    Boolean valid; Boolean changed; Boolean mapAccepted;
    Integer currentSlot; Integer referenceSlot; Integer anchorSlot;
    SchmidtGraphPoseCorrection.State filterState;
    SchmidtGraphPoseCorrection.Result filterResult;
    State candidate;
  algorithm
    result.next := previous; result.accepted := false; result.reason := 1;
    result.attempted := false; result.nextAttempt := previousAttempt;
    result.filterAccepted := false; result.filterReason := 0;
    result.mapReason := 0; result.reprojectionReason := 0;
    result.projectedCount := 0; result.prunedCount := 0;
    if requested then
      result.reason := 2;
      valid := RGBDLocalizationCatalog.ValidHeader(previous.localization)
        and previous.localization.initialized
        and RGBDLocalizationCatalog.ValidEstimator(previous.localization.estimator)
        and ValidView(previous.localization.catalog,previous.poses,previous.localization.sourceRevision)
        and previous.correctionRevision >= 0 and previous.correctionRevision < identifierLimit
        and previous.graphRevisionUsed >= 0 and previous.graphRevisionUsed <= previous.localization.graph.revision
        and previous.correctionRevision <= previous.graphRevisionUsed
        and ((previous.correctionRevision == 0) == (previous.graphRevisionUsed == 0))
        and previous.localization.catalog.nextId > 2
        and previous.localization.lastProcessedImageEpoch == previous.localization.catalog.lastEpoch
        and previous.localization.lastProcessedImageTime == previous.localization.catalog.lastTime
        and previous.localization.predictionTime == previous.localization.catalog.lastTime
        and GraphGaugeUncertainty.ValidBinding(proposal.selected.binding)
        and GraphGaugeUncertainty.SameBinding(proposal.selected.binding,expectedBinding)
        and proposal.graphRevision == previous.localization.graph.revision
        and expectedBinding.graphRevision == proposal.graphRevision
        and expectedBinding.catalogPoseRevision == previous.poses.revision
        and expectedBinding.generation == previous.localization.generation
        and expectedBinding.sourceRevision == previous.localization.sourceRevision
        and expectedBinding.currentId == previous.localization.catalog.nextId-1
        and expectedBinding.currentEpoch == previous.localization.lastProcessedImageEpoch
        and expectedBinding.currentTime == previous.localization.predictionTime
        and expectedBinding.currentCaptureSequence == previous.localization.steps
        and expectedBinding.anchorId == max(1,previous.localization.catalog.nextId-nodeCapacity)
        and previousAttempt.generation == previous.localization.generation
        and previousAttempt.graphRevision >= previous.graphRevisionUsed
        and (if previousAttempt.graphRevision == 0 then previousAttempt.factorProvenance == 0
          else previousAttempt.factorProvenance > 0)
        and proposal.graphRevision > previousAttempt.graphRevision
        and proposal.selected.binding.factorProvenance <> previousAttempt.factorProvenance;
      if valid then
        currentSlot := mod(expectedBinding.currentId-1,nodeCapacity)+1;
        referenceSlot := mod(expectedBinding.referenceId-1,nodeCapacity)+1;
        anchorSlot := mod(expectedBinding.anchorId-1,nodeCapacity)+1;
        valid := RGBDGraphMeasurements.Bound(previous.localization.catalog,expectedBinding.referenceId,
            referenceSlot,expectedBinding.referenceEpoch)
          and RGBDGraphMeasurements.Bound(previous.localization.catalog,expectedBinding.anchorId,
            anchorSlot,expectedBinding.anchorEpoch)
          and expectedBinding.referenceTime == previous.localization.catalog.imageTimes[referenceSlot]
          and expectedBinding.anchorTime == previous.localization.catalog.imageTimes[anchorSlot];
        if previous.localization.estimator.referenceAvailable == 1 then
          valid := valid and previous.localization.referenceBirth.catalogId == expectedBinding.referenceId
            and previous.localization.referenceBirth.epoch == expectedBinding.referenceEpoch
            and previous.localization.referenceBirth.sequence == expectedBinding.referenceCaptureSequence;
        end if;
      end if;
      if valid then
        result.attempted := true;
        result.nextAttempt.generation := previous.localization.generation;
        result.nextAttempt.graphRevision := proposal.graphRevision;
        result.nextAttempt.factorProvenance := proposal.selected.binding.factorProvenance;
        result.reason := 3;
        valid := proposal.optimizerAccepted
          and RGBDGraphMeasurements.ValidState(previous.localization.catalog,previous.localization.graph)
          and ValidView(previous.localization.catalog,proposal.poses,previous.localization.sourceRevision)
          and consistencyTolerance > 0 and consistencyTolerance <= 1e-6
          and maximumConfidence >= 2 and maximumConfidence <= 100 and floor(maximumConfidence) == maximumConfidence;
      end if;
      if valid then
        result.reason := 4; changed := false;
        for slot in 1:nodeCapacity loop
          if previous.poses.enabled[slot] then
            for axis in 1:dimension loop
              changed := changed or proposal.poses.positions[slot,axis] <> previous.poses.positions[slot,axis];
              for column in 1:dimension loop
                changed := changed or proposal.poses.rotations[slot,axis,column] <> previous.poses.rotations[slot,axis,column];
              end for;
            end for;
          end if;
        end for;
        valid := proposal.poses.revision == previous.poses.revision+(if changed then 1 else 0);
        // The optimizer fixes the chronological first row in the same ENU gauge.
        for axis in 1:dimension loop
          valid := valid and proposal.poses.positions[anchorSlot,axis] == previous.poses.positions[anchorSlot,axis];
          for column in 1:dimension loop
            valid := valid and proposal.poses.rotations[anchorSlot,axis,column] == previous.poses.rotations[anchorSlot,axis,column];
          end for;
        end for;
        for node in 1:2 loop
          for axis in 1:dimension loop
            valid := valid and abs(proposal.selected.positions[node,axis]
              -proposal.poses.positions[if node == 1 then currentSlot else referenceSlot,axis]) <= consistencyTolerance;
            for column in 1:dimension loop
              valid := valid and abs(proposal.selected.rotations[node,axis,column]
                -proposal.poses.rotations[if node == 1 then currentSlot else referenceSlot,axis,column]) <= consistencyTolerance;
            end for;
          end for;
        end for;
      end if;
      if valid then
        filterState := EstimatorState(previous.localization);
        filterResult := SchmidtGraphPoseCorrection.Correct(filterState,proposal.selected,expectedBinding,
          previousAttempt,policy,true);
        result.filterAccepted := filterResult.accepted; result.filterReason := filterResult.reason;
        result.reason := 5; valid := filterResult.accepted;
      end if;
      if valid then
        candidate := previous; candidate.poses.revision := proposal.poses.revision;
        for slot in 1:nodeCapacity loop
          if previous.poses.enabled[slot] then
            candidate.poses.positions[slot,:] := proposal.poses.positions[slot,:];
            candidate.poses.rotations[slot,:,:] := proposal.poses.rotations[slot,:,:];
          end if;
        end for;
        candidate.localization.estimator.position := filterResult.next.position;
        candidate.localization.estimator.velocity := filterResult.next.velocity;
        candidate.localization.estimator.rotation := filterResult.next.rotation;
        candidate.localization.estimator.accelBias := filterResult.next.accelBias;
        candidate.localization.estimator.gyroBias := filterResult.next.gyroBias;
        candidate.localization.estimator.covariance := filterResult.next.covariance;
        candidate.localization.estimator.crossCovariance := filterResult.next.crossCovariance;
        candidate.localization.estimator.referenceCovariance := filterResult.next.referenceCovariance;
        candidate.localization.estimator.referencePosition := filterResult.next.referencePosition;
        candidate.localization.estimator.referenceRotation := filterResult.next.referenceRotation;
        // Preserve acquisition clocks, raw capture poses/noise/calibration,
        // every descriptor, all measured edges and the consumed-image ledger.
        (candidate.localization.map.point,candidate.localization.map.occupied,candidate.localization.map.confidence,
          candidate.localization.map.lastSeen,candidate.localization.map.lastFrame,candidate.localization.map.localPoint,
          candidate.localization.map.anchorId,candidate.localization.map.anchorSlot,
          mapAccepted,result.mapReason,result.reprojectionReason,result.projectedCount,result.prunedCount) :=
          RGBDLandmarkCatalog.Synchronize(
            previous.localization.map.point,previous.localization.map.occupied,previous.localization.map.confidence,
            previous.localization.map.lastSeen,previous.localization.map.lastFrame,previous.localization.map.localPoint,
            previous.localization.map.anchorId,previous.localization.map.anchorSlot,
            previous.localization.map.generation,previous.poses.revision,
            previous.poses.enabled,previous.poses.ids,previous.poses.positions,previous.poses.rotations,
            proposal.poses.enabled,proposal.poses.ids,proposal.poses.positions,proposal.poses.rotations,
            previous.localization.generation,proposal.poses.revision,true,true,false,
            previous.localization.map.imageTime,previous.localization.map.frame,maximumConfidence,1e6,consistencyTolerance);
        result.reason := 6; valid := mapAccepted;
        if valid then
          candidate.correctionRevision := previous.correctionRevision+1;
          candidate.graphRevisionUsed := proposal.graphRevision;
          result.reason := 7;
          valid := RGBDLocalizationCatalog.ValidHeader(candidate.localization)
            and RGBDLocalizationCatalog.ValidEstimator(candidate.localization.estimator);
          if valid then result.next := candidate; result.accepted := true; result.reason := 0; end if;
        end if;
      end if;
    end if;
    if not result.accepted then result.projectedCount := 0; result.prunedCount := 0; end if;
  end Commit;