SLAM.PoseGraph.RGBDGraphMeasurements.Capture

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

function Capture
    input RGBDKeyframes.Catalog previousCatalog;
    input RGBDKeyframes.Catalog capturedCatalog "Already admitted capture; immutable old payload is trusted";
    input State previous;
    input RGBDLoopVerification.Proposal sequential;
    input RGBDLoopVerification.Proposal loops[proposalCapacity];
    input Boolean requested;
    input Boolean reset = false;
    input Real inlierDistance = 0.08; input Real maximumRms = 0.03;
    output Update result;
    input Boolean calibratedResiduals = true;
    input Real maximumNormalizedSquared = 9.0;
    input Real localizationSigma = 0.5; input Real depthInflation = 1.0; input Real minimumPivot = 1e-10;
    input Real loopInlierDistance = inlierDistance;
    input Real loopMaximumRms = maximumRms;
    input Boolean loopCalibratedResiduals = calibratedResiduals;
    input Real loopMaximumNormalizedSquared = maximumNormalizedSquared;
  protected
    State working; Insertion insertion;
    Boolean valid; Integer reason; Integer removed; Integer sequentialCount; Integer loopCount; Integer duplicates;
  algorithm
    result.state := previous; result.accepted := false; result.rejectionReason := 1;
    result.removedEdges := 0; result.admittedSequential := 0; result.admittedLoops := 0; result.duplicateLoops := 0;
    removed := 0; sequentialCount := 0; loopCount := 0; duplicates := 0;
    if requested then
      reason := 2;
      valid := RGBDKeyframes.ValidHeader(capturedCatalog) and capturedCatalog.nextId >= 2
        and inlierDistance > 0 and inlierDistance <= 10 and maximumRms >= 0 and maximumRms <= inlierDistance;
      if calibratedResiduals then
        valid := valid and maximumNormalizedSquared > 0 and maximumNormalizedSquared <= 1e6
          and localizationSigma > 0 and localizationSigma <= 10
          and depthInflation >= 1 and depthInflation <= 100 and minimumPivot > 0 and minimumPivot < 1;
      end if;
      if reset then
        // A deliberate new generation can recover a damaged old payload.
        valid := valid and previous.generation >= 1 and previous.generation < identifierLimit
          and capturedCatalog.generation > previous.generation and capturedCatalog.nextId == 2;
        working := Empty(capturedCatalog.generation);
      else
        working := previous;
        valid := valid and RGBDKeyframes.ValidHeader(previousCatalog)
          and previousCatalog.nextId < identifierLimit
          and capturedCatalog.generation == previousCatalog.generation
          and capturedCatalog.vocabularyVersion == previousCatalog.vocabularyVersion
          and capturedCatalog.nextId == previousCatalog.nextId+1
          and capturedCatalog.lastEpoch > previousCatalog.lastEpoch
          and (previousCatalog.nextId == 1 or capturedCatalog.lastTime > previousCatalog.lastTime);
        if valid then
          for node in 1:nodeCapacity loop
            if previousCatalog.occupied[node] and node <> previousCatalog.nextSlot then
              valid := valid and capturedCatalog.occupied[node]
                and capturedCatalog.ids[node] == previousCatalog.ids[node]
                and capturedCatalog.epochs[node] == previousCatalog.epochs[node]
                and capturedCatalog.imageTimes[node] == previousCatalog.imageTimes[node];
            end if;
          end for;
          if valid then reason := 3; valid := ValidState(previousCatalog,previous) and previous.revision < identifierLimit; end if;
        end if;
      end if;
      if valid then
        for slot in 1:edgeCapacity loop
          if working.edges[slot].enabled then
            if not Bound(capturedCatalog,working.edges[slot].referenceId,working.edges[slot].referenceSlot,working.edges[slot].referenceEpoch)
              or not Bound(capturedCatalog,working.edges[slot].currentId,working.edges[slot].currentSlot,working.edges[slot].currentEpoch) then
              working.edges[slot] := EmptyEdge(); removed := removed+1;
            end if;
          end if;
        end for;
        reason := 4;
        if not reset and previousCatalog.nextId > 1 then
          valid := sequential.referenceId == previousCatalog.nextId-1
            and ValidProposal(capturedCatalog,sequential,inlierDistance,maximumRms,
              calibratedResiduals=calibratedResiduals,maximumNormalizedSquared=maximumNormalizedSquared,
              localizationSigma=localizationSigma,depthInflation=depthInflation,minimumPivot=minimumPivot);
          if valid then
            insertion := Insert(working,sequential,1); working := insertion.state;
            valid := insertion.accepted and not insertion.duplicate;
            removed := removed+insertion.replacedLoops; sequentialCount := if valid then 1 else 0;
            if not valid then reason := 6; end if;
          end if;
        else valid := not sequential.verified;
        end if;
        for rank in 1:proposalCapacity loop
          if valid and loops[rank].verified then
            reason := 5; valid := ValidProposal(capturedCatalog,loops[rank],loopInlierDistance,loopMaximumRms,
              calibratedResiduals=loopCalibratedResiduals,maximumNormalizedSquared=loopMaximumNormalizedSquared,
              localizationSigma=localizationSigma,depthInflation=depthInflation,minimumPivot=minimumPivot);
            if valid then
              insertion := Insert(working,loops[rank],2); working := insertion.state; valid := insertion.accepted;
              removed := removed+insertion.replacedLoops;
              duplicates := duplicates+(if insertion.duplicate then 1 else 0);
              loopCount := loopCount+(if insertion.accepted and not insertion.duplicate then 1 else 0);
              if not valid then reason := 6; end if;
            end if;
          end if;
        end for;
        if valid then
          working.lastCaptureId := capturedCatalog.nextId-1;
          working.revision := if reset then 1 else previous.revision+1;
          reason := 7; valid := ValidState(capturedCatalog,working);
        end if;
        if valid then
          result.state := working; result.accepted := true; result.rejectionReason := 0;
          result.removedEdges := removed; result.admittedSequential := sequentialCount;
          result.admittedLoops := loopCount; result.duplicateLoops := duplicates;
        else result.rejectionReason := reason;
        end if;
      else result.rejectionReason := reason;
      end if;
    end if;
  end Capture;