SLAM.PoseGraph.RGBDGraphMeasurements.Insert

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

function Insert
    // Internal primitive: Capture certifies the graph, proposal and kind
    // before calling. Direct callers must satisfy the same preconditions.
    input State previous;
    input RGBDLoopVerification.Proposal proposal;
    input Integer kind;
    output Insertion result;
  protected
    Integer freeSlot; Integer oldestLoop; Integer oldestId;
  algorithm
    result.state := previous; result.accepted := false; result.duplicate := false; result.replacedLoops := 0;
    freeSlot := 0; oldestLoop := 0; oldestId := identifierLimit;
    for slot in 1:edgeCapacity loop
      if previous.edges[slot].enabled then
        result.duplicate := result.duplicate or (previous.edges[slot].referenceId == proposal.referenceId
          and previous.edges[slot].currentId == proposal.currentId);
        if previous.edges[slot].kind == 2 and previous.edges[slot].id < oldestId then
          oldestLoop := slot; oldestId := previous.edges[slot].id;
        end if;
      elseif freeSlot == 0 then freeSlot := slot;
      end if;
    end for;
    if result.duplicate then
      result.accepted := true;
    elseif previous.nextEdgeId < identifierLimit then
      if freeSlot == 0 then freeSlot := oldestLoop; result.replacedLoops := if oldestLoop > 0 then 1 else 0; end if;
      if freeSlot > 0 then
        result.state.edges[freeSlot] := FromProposal(proposal,previous.nextEdgeId,kind);
        result.state.nextEdgeId := previous.nextEdgeId+1;
        result.accepted := true;
      end if;
    end if;
  end Insert;