SLAM.PoseGraph.RGBDGraphMeasurements.ValidState

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

function ValidState
    input RGBDKeyframes.Catalog catalog;
    input State state;
    output Boolean valid;
  protected
    Boolean chain[nodeCapacity]; Boolean edgeValid;
    Integer oldest;
  algorithm
    chain := fill(false,nodeCapacity); oldest := 1;
    valid := RGBDKeyframes.ValidHeader(catalog)
      and state.generation == catalog.generation
      and state.revision >= 0 and state.revision <= identifierLimit
      and state.nextEdgeId >= 1 and state.nextEdgeId <= identifierLimit;
    if valid then
      valid := state.lastCaptureId == catalog.nextId-1;
      oldest := max(1,catalog.nextId-nodeCapacity);
      for slot in 1:edgeCapacity loop
        if state.edges[slot].enabled then
          edgeValid := state.edges[slot].id >= 1 and state.edges[slot].id < state.nextEdgeId
            and (state.edges[slot].kind == 1 or state.edges[slot].kind == 2)
            and state.edges[slot].referenceId < state.edges[slot].currentId
            and Bound(catalog,state.edges[slot].referenceId,state.edges[slot].referenceSlot,state.edges[slot].referenceEpoch)
            and Bound(catalog,state.edges[slot].currentId,state.edges[slot].currentSlot,state.edges[slot].currentEpoch)
            and RGBDUncertaintyProper(state.edges[slot].rotation)
            and ValidUncertainty(state.edges[slot].covariance,state.edges[slot].information);
          for axis in 1:dimension loop
            edgeValid := edgeValid and abs(state.edges[slot].translation[axis]) <= 1e6;
          end for;
          if state.edges[slot].kind == 1 then
            edgeValid := edgeValid and state.edges[slot].currentId == state.edges[slot].referenceId+1;
            if edgeValid then chain[state.edges[slot].currentSlot] := true; end if;
          end if;
          valid := valid and edgeValid;
          for earlier in 1:slot-1 loop
            if state.edges[earlier].enabled then
              valid := valid and state.edges[earlier].id <> state.edges[slot].id
                and (state.edges[earlier].referenceId <> state.edges[slot].referenceId
                  or state.edges[earlier].currentId <> state.edges[slot].currentId);
            end if;
          end for;
        end if;
      end for;
      // Every consecutive retained pair has a measured edge, so eviction
      // leaves a connected chain rooted at the oldest surviving keyframe.
      for node in 1:nodeCapacity loop
        if catalog.occupied[node] and catalog.ids[node] > oldest then
          valid := valid and chain[node];
        end if;
      end for;
    end if;
  end ValidState;