SLAM.Mapping.RGBDCatalogMapping.Capture

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

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;