SLAM.PoseGraph.RGBDGraphProcessing.Correct

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

function Correct
    input State previous;
    input Policy policy;
    input Boolean requested;
    output Result result;
  protected
    Boolean valid;
    Boolean anchorAccepted;
    Boolean contextAccepted;
    Boolean changed;
    Integer oldest;
    Integer currentId;
    Integer referenceId;
    Integer anchorSlot;
    Integer currentSlot;
    Integer referenceSlot;
    Integer currentNode;
    Integer referenceNode;
    Integer contextReason;
    Integer slot;
    RGBDGraphMeasurements.Problem problem;
    RGBDGraphEstimatorCommit.Proposal proposal;
    RGBDGraphEstimatorCommit.Result committed;
    RGBDGraphSelectedGauge.Context context;
    RGBDGraphSelectedGauge.Result selected;
    RGBDGraphAnchorBound.Estimate anchor;
    GraphGaugeUncertainty.Binding binding;
    Real finalPosition[nodeCapacity,dimension];
    Real finalRotation[nodeCapacity,dimension,dimension];
    Real activeNodes;
    Real activeEdges;
  algorithm
    result.next := previous;
    result.accepted := false;
    result.reason := 1;
    result.attempted := false;
    result.commitReason := 0;
    result.filterReason := 0;
    result.mapReason := 0;
    result.anchorReason := 0;
    result.selectionReason := 0;
    result.covarianceStatus := 0;
    result.optimizerStatus := 0;
    result.costBefore := 0;
    result.costAfter := 0;
    result.acceptedIterations := 0;
    result.pcgIterations := 0;
    result.projectedCount := 0;
    result.prunedCount := 0;
    result.roundoffCertified := false;
    if requested then
      result.reason := 2;
      valid := Valid(previous) and previous.estimator.localization.initialized
        and previous.estimator.localization.catalog.nextId > 2
        and previous.estimator.localization.estimator.referenceAvailable == 1
        and previous.estimator.localization.referenceBirth.catalogId > 0
        and previous.estimator.localization.graph.revision > previous.attempt.graphRevision
        and previous.estimator.localization.lastProcessedImageEpoch == previous.estimator.localization.catalog.lastEpoch
        and previous.estimator.localization.lastProcessedImageTime == previous.estimator.localization.catalog.lastTime
        and previous.estimator.localization.predictionTime == previous.estimator.localization.catalog.lastTime
        and previous.estimator.poses.revision < identifierLimit-1;
      if valid then
        oldest := max(1,previous.estimator.localization.catalog.nextId-nodeCapacity);
        currentId := previous.estimator.localization.catalog.nextId-1;
        referenceId := previous.estimator.localization.referenceBirth.catalogId;
        anchorSlot := mod(oldest-1,nodeCapacity)+1;
        currentSlot := mod(currentId-1,nodeCapacity)+1;
        referenceSlot := mod(referenceId-1,nodeCapacity)+1;
        valid := RGBDGraphMeasurements.Bound(previous.estimator.localization.catalog,referenceId,
            referenceSlot,previous.estimator.localization.referenceBirth.epoch)
          and previous.captures.sequences[currentSlot] == previous.estimator.localization.steps
          and previous.captures.sequences[referenceSlot] == previous.estimator.localization.referenceBirth.sequence;
      end if;
      if valid then
        result.reason := 3;
        problem := RGBDGraphMeasurements.PrepareProblem(previous.estimator.localization.catalog,
          previous.estimator.localization.graph,true);
        valid := problem.accepted;
      end if;
      if valid then
        // Warm-start from the durable corrected view, not immutable raw
        // captures. The chronological fixed row is mapped by catalogSlot.
        for node in 1:nodeCapacity loop
          if node <= problem.nodeCount then
            slot := problem.catalogSlot[node];
            problem.positions[node,:] := previous.estimator.poses.positions[slot,:];
            problem.rotations[node,:,:] := previous.estimator.poses.rotations[slot,:,:];
          end if;
        end for;
        result.reason := 4;
        (finalPosition,finalRotation,result.optimizerStatus,result.costBefore,result.costAfter,
          result.acceptedIterations,result.pcgIterations,activeNodes,activeEdges) :=
          OptimizeModelicaPoseGraph(problem.positions,problem.rotations,problem.nodeMask,
            problem.edgeMask,problem.fromNode,problem.toNode,problem.measuredTranslation,
            problem.measuredRotation,problem.information,policy.maximumIterations,policy.maximumPCG,
            policy.maximumBacktracks,policy.initialDamping,policy.maximumPositionStep,
            policy.maximumAngleStep,policy.pcgTolerance);
        valid := (result.optimizerStatus == 1 or result.optimizerStatus == 2)
          and result.costAfter >= 0 and result.costAfter <= result.costBefore
          and activeNodes == problem.nodeCount and activeEdges == problem.edgeCount;
        proposal.poses := previous.estimator.poses;
        changed := false;
        for node in 1:nodeCapacity loop
          if node <= problem.nodeCount then
            slot := problem.catalogSlot[node];
            valid := valid and RGBDUncertaintyProper(finalRotation[node,:,:]);
            for axis in 1:dimension loop
              valid := valid and abs(finalPosition[node,axis]) <= 1e6;
              changed := changed or finalPosition[node,axis] <> proposal.poses.positions[slot,axis];
              for column in 1:dimension loop
                changed := changed or finalRotation[node,axis,column] <> proposal.poses.rotations[slot,axis,column];
              end for;
            end for;
            proposal.poses.positions[slot,:] := finalPosition[node,:];
            proposal.poses.rotations[slot,:,:] := finalRotation[node,:,:];
          end if;
        end for;
        // The gauge is exact, not merely close after a numerical update.
        valid := valid and max(abs(proposal.poses.positions[anchorSlot,:]
          -previous.estimator.poses.positions[anchorSlot,:])) == 0
          and max(abs(proposal.poses.rotations[anchorSlot,:,:]
          -previous.estimator.poses.rotations[anchorSlot,:,:])) == 0;
        proposal.poses.revision := previous.estimator.poses.revision+(if changed then 1 else 0);
        proposal.graphRevision := previous.estimator.localization.graph.revision;
        proposal.optimizerAccepted := valid;
      end if;
      if valid then
        result.reason := 5;
        (anchor,anchorAccepted,result.anchorReason) := RGBDGraphAnchorBound.FromCapture(
          previous.anchor,previous.estimator.localization.catalog,previous.captures,
          previous.estimator.localization.sourceRevision,previous.estimator.localization.steps,
          previous.estimator.poses.revision,proposal.poses.positions[anchorSlot,:],
          proposal.poses.rotations[anchorSlot,:,:],proposal.graphRevision,
          policy.anchorWeight,policy.anchorPositionLimit,policy.anchorAngleLimit,true);
        valid := anchorAccepted;
      end if;
      if valid then
        // Context's revision binds the input pose owner. Proposal poses may
        // advance it only at atomic Commit, after all bound checks succeed.
        context.generation := previous.estimator.localization.generation;
        context.sourceRevision := previous.estimator.localization.sourceRevision;
        context.graphRevision := 0;
        context.catalogPoseRevision := 0;
        context.captureIds := fill(0,nodeCapacity);
        context.captureSequences := fill(0,nodeCapacity);
        (context,contextAccepted,contextReason) := RGBDGraphSelectedGauge.ContextFromLedger(
          context,previous.captures,previous.estimator.localization.catalog,
          previous.estimator.localization.graph,previous.estimator.localization.sourceRevision,
          previous.estimator.localization.steps,previous.estimator.poses.revision,true);
        result.reason := 6;
        valid := contextAccepted;
      end if;
      if valid then
        currentNode := currentId-oldest+1;
        referenceNode := referenceId-oldest+1;
        binding.generation := context.generation;
        binding.sourceRevision := context.sourceRevision;
        binding.graphRevision := context.graphRevision;
        binding.catalogPoseRevision := context.catalogPoseRevision;
        binding.anchorId := oldest;
        binding.anchorEpoch := previous.captures.epochs[anchorSlot];
        binding.anchorCaptureSequence := previous.captures.sequences[anchorSlot];
        binding.anchorTime := previous.captures.times[anchorSlot];
        binding.currentId := currentId;
        binding.currentEpoch := previous.captures.epochs[currentSlot];
        binding.currentCaptureSequence := previous.captures.sequences[currentSlot];
        binding.currentTime := previous.captures.times[currentSlot];
        binding.referenceId := referenceId;
        binding.referenceEpoch := previous.captures.epochs[referenceSlot];
        binding.referenceCaptureSequence := previous.captures.sequences[referenceSlot];
        binding.referenceTime := previous.captures.times[referenceSlot];
        binding.chart := GraphGaugeUncertainty.chartENUPositionRightLocalAttitude;
        // Revision-scoped provenance is generated here and cannot be a host
        // assertion of a successful optimizer or an arbitrary covariance.
        binding.factorProvenance := proposal.graphRevision;
        binding.anchorBoundProvenance := anchor.binding.provenance;
        selected := RGBDGraphSelectedGauge.SelectFromAnchor(previous.selected,
          previous.estimator.localization.catalog,previous.estimator.localization.graph,context,
          proposal.poses.positions,proposal.poses.rotations,currentNode,referenceNode,binding,
          anchor,proposal.graphRevision,policy.transportWeight,true,
          policy.covariancePCG,policy.covarianceTolerance);
        result.selectionReason := selected.rejectionReason;
        result.covarianceStatus := selected.covarianceStatus;
        result.roundoffCertified := selected.roundoffCertified;
        valid := selected.accepted;
      end if;
      if valid then
        result.reason := 7;
        proposal.selected := selected.estimate;
        committed := RGBDGraphEstimatorCommit.Commit(previous.estimator,proposal,binding,
          previous.attempt,policy.filter,true,policy.consistencyTolerance,policy.maximumConfidence);
        result.attempted := committed.attempted;
        result.commitReason := committed.reason;
        result.filterReason := committed.filterReason;
        result.mapReason := committed.mapReason;
        // An evaluated graph/filter factor is consumed even when the filter
        // or final map refuses. Preserve the whole numerical owner on refusal.
        if committed.attempted then
          result.next.attempt := committed.nextAttempt;
        end if;
        if committed.accepted then
          result.next.estimator := committed.next;
          result.next.anchor := anchor;
          result.next.selected := selected.estimate;
          result.projectedCount := committed.projectedCount;
          result.prunedCount := committed.prunedCount;
          result.accepted := true;
          result.reason := 0;
        end if;
      end if;
    end if;
  end Correct;