SLAM.LoopClosure.RGBDKeyframes.Store

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

function Store
    input Catalog previous;
    input Frame candidate;
    input Boolean requested;
    output Catalog next;
    output Boolean accepted;
    output Integer storedSlot;
    output Integer evictedId "Invalidate this generation/id in graph and landmark owners";
  algorithm
    next := previous;
    accepted := false;
    storedSlot := 0;
    evictedId := 0;
    if requested and ValidHeader(previous) and previous.nextId < identifierLimit then
      accepted := candidate.generation == previous.generation
        and candidate.vocabularyVersion == previous.vocabularyVersion
        and candidate.id == previous.nextId and candidate.epoch > previous.lastEpoch
        and (previous.nextId == 1 or candidate.imageTime > previous.lastTime)
        and ValidFrame(candidate);
      if accepted then
        storedSlot := previous.nextSlot;
        evictedId := if previous.occupied[storedSlot] then previous.ids[storedSlot] else 0;
        next.generations[storedSlot] := candidate.generation;
        next.ids[storedSlot] := candidate.id;
        next.epochs[storedSlot] := candidate.epoch;
        next.imageTimes[storedSlot] := candidate.imageTime;
        next.counts[storedSlot] := candidate.count;
        next.featureEnabled[storedSlot,:] := candidate.enabled;
        next.descriptors[storedSlot,:,:] := candidate.descriptor;
        next.opticalPoints[storedSlot,:,:] := candidate.opticalPoint;
        next.pixelCoordinates[storedSlot,:,:] := candidate.pixels;
        next.rgbSizes[storedSlot,:] := candidate.rgbSize;
        next.depthSizes[storedSlot,:] := candidate.depthSize;
        next.rgbCalibrations[storedSlot,:] := candidate.rgbCalibration;
        next.depthCalibrations[storedSlot,:] := candidate.depthCalibration;
        next.opticalToBodyRotations[storedSlot,:,:] := candidate.opticalToBody;
        next.cameraOriginsBody[storedSlot,:] := candidate.cameraOriginBody;
        next.disparityNoises[storedSlot] := candidate.disparityNoise;
        next.noiseReferenceFocals[storedSlot] := candidate.noiseReferenceFx;
        next.baselines[storedSlot] := candidate.baseline;
        next.bodyRotations[storedSlot,:,:] := candidate.bodyRotation;
        next.bodyPositions[storedSlot,:] := candidate.bodyPosition;
        next.poseCovariances[storedSlot,:,:] := candidate.poseCovariance;
        next.vocabularyVersions[storedSlot] := candidate.vocabularyVersion;
        next.histograms[storedSlot,:] := candidate.histogram;
        next.occupied[storedSlot] := true;
        next.nextId := previous.nextId+1;
        next.nextSlot := mod(previous.nextSlot,keyframeCapacity)+1;
        next.lastEpoch := candidate.epoch;
        next.lastTime := candidate.imageTime;
      end if;
    end if;
  end Store;