SLAM.Mapping.RGBDCatalogMapping

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

package RGBDCatalogMapping
  import RGBDCatalogGraphCapture = SLAM.PoseGraph.RGBDCatalogGraphCapture;
  import RGBDGraphMeasurements = SLAM.PoseGraph.RGBDGraphMeasurements;
  import RGBDKeyframeRetrieval = SLAM.LoopClosure.RGBDKeyframeRetrieval;
  import RGBDKeyframes = SLAM.LoopClosure.RGBDKeyframes;
  import RGBDMapAnchors = SLAM.Mapping.RGBDMapAnchors;
  import UpdateCatalogLandmarkMap = SLAM.Mapping.UpdateCatalogLandmarkMap;

  constant Integer mapCapacity = RGBDMapAnchors.mapCapacity;
  constant Integer featureCapacity = RGBDKeyframes.featureCapacity;
  constant Integer loopCapacity = RGBDKeyframeRetrieval.proposalCapacity;
  constant Integer dimension = RGBDKeyframes.dimension;

  record State
    Real point[mapCapacity,dimension]; Real occupied[mapCapacity];
    Real confidence[mapCapacity]; Real lastSeen[mapCapacity]; Real lastFrame[mapCapacity];
    Real localPoint[mapCapacity,dimension];
    Integer anchorId[mapCapacity]; Integer anchorSlot[mapCapacity];
    Integer generation; Integer catalogRevision;
    Real imageTime; Integer imageEpoch;
    Real frame "Consecutive accepted map-observation counter, not camera epoch";
    Real worldFrame;
  end State;

  function Empty
    input Integer generation = 1;
    input Real worldFrame = 0.0;
    output State result;
  algorithm
    result.point := zeros(mapCapacity,dimension); result.occupied := zeros(mapCapacity);
    result.confidence := zeros(mapCapacity); result.lastSeen := zeros(mapCapacity); result.lastFrame := zeros(mapCapacity);
    result.localPoint := zeros(mapCapacity,dimension);
    result.anchorId := fill(0,mapCapacity); result.anchorSlot := fill(0,mapCapacity);
    result.generation := generation; result.catalogRevision := 0;
    result.imageTime := 0.0; result.imageEpoch := -1; result.frame := 0.0; result.worldFrame := worldFrame;
  end Empty;

  record Diagnostics
    Real mapAccepted; Real catalogUpdateRejectionReason;
    Integer keyframeRejectionReason; Integer sequentialRejectionReason;
    Integer catalogRejectionReason; Integer correctionReason;
    Real updateRejectionReason; Real mapRejectionReason; Integer anchorRejectionReason;
    Real occupiedCount; Real confirmedCount; Real tentativeCount;
    Real insertedCount; Real mergedCount; Real prunedCount; Real droppedCount; Real invalidCandidateCount;
    Integer projectedCount; Integer evictedCount; Integer assignedCount; Integer retainedCount; Integer clearedCount;
    Boolean loopAttempted;
    Integer loopRetrievalReason;
    Integer loopGraphReason;
    Integer loopCandidateCount;
    Integer loopCandidateReferenceId[RGBDKeyframeRetrieval.proposalCapacity];
    Integer loopCandidateCurrentId[RGBDKeyframeRetrieval.proposalCapacity];
    Real loopCandidateScore[RGBDKeyframeRetrieval.proposalCapacity];
    Integer loopCandidateReason[RGBDKeyframeRetrieval.proposalCapacity];
    Integer loopCandidateMatchedCount[RGBDKeyframeRetrieval.proposalCapacity];
    Integer loopCandidateInliers[RGBDKeyframeRetrieval.proposalCapacity];
    Real loopCandidateRms[RGBDKeyframeRetrieval.proposalCapacity];
    Boolean loopCandidateVerified[RGBDKeyframeRetrieval.proposalCapacity];
    Integer loopReferenceId[loopCapacity] "Retained keyframe id of each verified loop, 0 when the rank holds none";
    Integer loopCurrentId[loopCapacity] "Captured keyframe id of each verified loop, 0 when the rank holds none";
    Real loopScore[loopCapacity] "Appearance similarity of each verified loop's candidate, 0 when the rank holds none";
  end Diagnostics;

  function EmptyDiagnostics
    output Diagnostics result;
  algorithm
    result.mapAccepted := 0.0; result.catalogUpdateRejectionReason := 0.0;
    result.keyframeRejectionReason := 0; result.sequentialRejectionReason := 0;
    result.catalogRejectionReason := 0; result.correctionReason := 0;
    result.updateRejectionReason := 0.0; result.mapRejectionReason := 0.0; result.anchorRejectionReason := 0;
    result.occupiedCount := 0.0; result.confirmedCount := 0.0; result.tentativeCount := 0.0;
    result.insertedCount := 0.0; result.mergedCount := 0.0; result.prunedCount := 0.0;
    result.droppedCount := 0.0; result.invalidCandidateCount := 0.0;
    result.projectedCount := 0; result.evictedCount := 0; result.assignedCount := 0;
    result.retainedCount := 0; result.clearedCount := 0;
    result.loopAttempted := false;
    result.loopRetrievalReason := 0;
    result.loopGraphReason := 0;
    result.loopCandidateCount := 0;
    result.loopCandidateReferenceId := fill(0,RGBDKeyframeRetrieval.proposalCapacity);
    result.loopCandidateCurrentId := fill(0,RGBDKeyframeRetrieval.proposalCapacity);
    result.loopCandidateScore := fill(0.0,RGBDKeyframeRetrieval.proposalCapacity);
    result.loopCandidateReason := fill(0,RGBDKeyframeRetrieval.proposalCapacity);
    result.loopCandidateMatchedCount := fill(0,RGBDKeyframeRetrieval.proposalCapacity);
    result.loopCandidateInliers := fill(0,RGBDKeyframeRetrieval.proposalCapacity);
    result.loopCandidateRms := fill(0.0,RGBDKeyframeRetrieval.proposalCapacity);
    result.loopCandidateVerified := fill(false,RGBDKeyframeRetrieval.proposalCapacity);
    result.loopReferenceId := fill(0,loopCapacity); result.loopCurrentId := fill(0,loopCapacity);
    result.loopScore := fill(0.0,loopCapacity);
  end EmptyDiagnostics;

  record Result
    RGBDKeyframes.Catalog catalog;
    RGBDGraphMeasurements.State graph;
    State map;
    Boolean accepted;
    Integer rejectionReason "1 idle; 2 visual; 3 binding; 4 projection; 5 map";
    Diagnostics diagnostics "Map-stage diagnostics are proposals, not committed counts";
    RGBDGraphMeasurements.Problem problem;
  end Result;

  function Capture
    input RGBDKeyframes.Catalog previousCatalog;
    input RGBDGraphMeasurements.State previousGraph;
    input State previous;
    input RGBDCatalogGraphCapture.Result visual "From the same old catalog/graph owners";
    input Real candidatePoint[featureCapacity,dimension];
    input Real candidateEnabled[featureCapacity];
    input Real poseAccepted;
    input Boolean requested;
    input Real coordinateLimit = 1e6; input Real voxelWidth = 0.25;
    input Real mergeRadius = 0.15; input Real maximumDistance = 80.0;
    input Real tentativeLifetime = 0.5; input Real confirmedLifetime = 5.0;
    input Real confirmationObservations = 3.0; input Real maximumConfidence = 8.0;
    input Real maximumTentative = 700.0; input Real consistencyTolerance = 1e-6;
    input Real previousNodePosition[RGBDKeyframes.keyframeCapacity,3] = previousCatalog.bodyPositions;
    input Real previousNodeRotation[RGBDKeyframes.keyframeCapacity,3,3] = previousCatalog.bodyRotations;
    input Integer previousPoseRevision = previous.catalogRevision;
    output Result result;
  protected
    State proposed;
    RGBDKeyframes.Frame measurement;
    Diagnostics diagnostics;
    Real confirmed[mapCapacity];
    Integer slot; Boolean valid; Boolean candidateValid; Real expectedWorld[dimension];
    Real nodePosition[RGBDKeyframes.keyframeCapacity,3]; Real nodeRotation[RGBDKeyframes.keyframeCapacity,3,3];
  algorithm
    result.catalog := previousCatalog; result.graph := previousGraph; result.map := previous;
    result.accepted := false; result.rejectionReason := 1;
    result.diagnostics := EmptyDiagnostics(); result.problem := RGBDGraphMeasurements.EmptyProblem();
    if requested then
      result.rejectionReason := 2;
      if visual.accepted then
        result.rejectionReason := 3;
        valid := RGBDKeyframes.ValidHeader(previousCatalog) and RGBDKeyframes.ValidHeader(visual.catalog)
          and previous.generation == previousCatalog.generation
          and visual.catalog.generation == previousCatalog.generation
          and previousCatalog.nextId < RGBDKeyframes.identifierLimit
          and visual.catalog.nextId >= 2
          and previous.catalogRevision >= 0 and previous.catalogRevision < RGBDKeyframes.identifierLimit
          and previous.frame >= 0 and previous.frame < RGBDKeyframes.identifierLimit
          and floor(previous.frame) == previous.frame
          and previous.imageEpoch >= -1 and previous.imageEpoch <= RGBDKeyframes.identifierLimit
          and ((previous.frame == 0 and previous.imageEpoch == -1)
            or (previous.frame > 0 and previous.imageEpoch >= 0))
          and visual.graph.generation == previousGraph.generation
          and previousGraph.revision >= 0 and previousGraph.revision < RGBDKeyframes.identifierLimit
          and coordinateLimit > 0 and coordinateLimit <= 1e6
          and consistencyTolerance > 0 and consistencyTolerance <= 0.01
          and visual.storedSlot >= 1 and visual.storedSlot <= RGBDKeyframes.keyframeCapacity
          and visual.problem.accepted;
        if valid then
          valid := visual.catalog.nextId == previousCatalog.nextId+1
            and visual.graph.revision == previousGraph.revision+1
            and visual.graph.lastCaptureId == visual.catalog.nextId-1
            and visual.storedSlot == previousCatalog.nextSlot;
          if valid then
            slot := visual.storedSlot;
            measurement := RGBDKeyframes.ReadSlot(visual.catalog,slot);
            valid := measurement.id == previousCatalog.nextId
              and measurement.epoch == visual.catalog.lastEpoch
              and measurement.epoch > previous.imageEpoch
              and measurement.imageTime == visual.catalog.lastTime
              and RGBDKeyframes.ValidFrame(measurement)
              and poseAccepted == 1.0;
            result.rejectionReason := 4;
            if valid then
              for feature in 1:featureCapacity loop
                candidateValid := candidateEnabled[feature] == 0.0 or candidateEnabled[feature] == 1.0;
                if candidateEnabled[feature] == 1.0 then
                  candidateValid := measurement.enabled[feature] and feature <= measurement.count;
                  if candidateValid then
                    expectedWorld := measurement.bodyRotation*(measurement.opticalToBody*measurement.opticalPoint[feature,:]
                      +measurement.cameraOriginBody)+measurement.bodyPosition;
                    for axis in 1:dimension loop
                      candidateValid := candidateValid and abs(expectedWorld[axis]) <= coordinateLimit
                        and abs(candidatePoint[feature,axis]-expectedWorld[axis]) <= consistencyTolerance;
                    end for;
                  end if;
                end if;
                valid := valid and candidateValid;
              end for;
            end if;
            if valid then
              proposed := previous; diagnostics := EmptyDiagnostics();
              nodePosition := previousNodePosition; nodeRotation := previousNodeRotation;
              nodePosition[slot,:] := measurement.bodyPosition; nodeRotation[slot,:,:] := measurement.bodyRotation;
              (proposed.point,proposed.occupied,proposed.confidence,proposed.lastSeen,proposed.lastFrame,confirmed,
                diagnostics.mapAccepted,diagnostics.catalogUpdateRejectionReason,proposed.imageTime,proposed.frame,proposed.worldFrame,
                diagnostics.occupiedCount,diagnostics.confirmedCount,diagnostics.tentativeCount,
                diagnostics.insertedCount,diagnostics.mergedCount,diagnostics.prunedCount,diagnostics.droppedCount,diagnostics.invalidCandidateCount,
                proposed.localPoint,proposed.anchorId,proposed.anchorSlot,proposed.generation,proposed.catalogRevision,
                diagnostics.catalogRejectionReason,diagnostics.correctionReason,diagnostics.updateRejectionReason,
                diagnostics.mapRejectionReason,diagnostics.anchorRejectionReason,diagnostics.projectedCount,
                diagnostics.evictedCount,diagnostics.assignedCount,diagnostics.retainedCount,diagnostics.clearedCount) := UpdateCatalogLandmarkMap(
                  previous.point,previous.occupied,previous.confidence,previous.lastSeen,previous.lastFrame,
                  candidatePoint,candidateEnabled,measurement.count,measurement.bodyPosition,poseAccepted,
                  previous.imageTime,measurement.imageTime,previous.frame,previous.frame+1,previous.worldFrame,previous.worldFrame,0.0,
                  coordinateLimit,voxelWidth,mergeRadius,maximumDistance,tentativeLifetime,confirmedLifetime,
                  confirmationObservations,maximumConfidence,maximumTentative,
                  previous.localPoint,previous.anchorId,previous.anchorSlot,previous.generation,previous.catalogRevision,
                  previousCatalog.occupied,previousCatalog.ids,previousNodePosition,previousNodeRotation,
                  visual.catalog.occupied,visual.catalog.ids,nodePosition,nodeRotation,
                  visual.catalog.generation,previous.catalogRevision+1,measurement.id,slot,true,true,consistencyTolerance,
                  previousPoseRevision,previousPoseRevision+1);
              result.diagnostics := diagnostics; result.rejectionReason := 5;
              if diagnostics.mapAccepted == 1.0 then
                proposed.imageEpoch := measurement.epoch;
                result.catalog := visual.catalog; result.graph := visual.graph; result.map := proposed;
                result.problem := visual.problem; result.accepted := true; result.rejectionReason := 0;
              end if;
            end if;
          end if;
        end if;
      end if;
    end if;
  end Capture;
end RGBDCatalogMapping;