function

SLAM.Localization.InitializeFastRGBDLocalization

Public declarations

Explicit source declarations; dimensions, modifications and defaults are shown as written, without evaluation.

Role	Name	Type	Dimensions / attributes / default	Description
input	`rgb`	`Real`	`[:,:,:]`
input	`depth`	`Real`	`[size(rgb,1),size(rgb,2)]`
input	`rgbCalibration`	`Real`	`[4]`
input	`depthCalibration`	`Real`	`[4]`
input	`disparityNoise`	`Real`
input	`noiseReferenceFx`	`Real`
input	`baseline`	`Real`
input	`opticalToBody`	`Real`	`[3,3]`
input	`cameraOriginBody`	`Real`	`[3]`
input	`frameEnabled`	`Real`
input	`imageCaptureRequested`	`Real`
input	`position`	`Real`	`[3]`
input	`velocity`	`Real`	`[3]`
input	`rotation`	`Real`	`[3,3]`
input	`accelBias`	`Real`	`[3]`
input	`gyroBias`	`Real`	`[3]`
input	`covariance`	`Real`	`[15,15]`
input	`crossCovariance`	`Real`	`[15,6]`
input	`referenceCovariance`	`Real`	`[6,6]`
input	`referencePosition`	`Real`	`[3]`
input	`referenceRotation`	`Real`	`[3,3]`
input	`referenceAvailable`	`Real`
input	`referenceEpoch`	`Real`
input	`currentEpoch`	`Real`
input	`referenceUsed`	`Real`
input	`lastUsedEpoch`	`Real`
input	`referenceDescriptor`	`Real`	`[featureCapacity,descriptorSize]`
input	`referencePoint`	`Real`	`[featureCapacity,3]`
input	`referenceEnabled`	`Real`	`[featureCapacity]`
input	`referencePixels`	`Real`	`[featureCapacity,2]`
input	`referenceCount`	`Real`
input	`referenceRgbCalibration`	`Real`	`[4]`
input	`referenceDepthCalibration`	`Real`	`[4]`
input	`referenceNoiseReferenceFx`	`Real`
input	`referenceDisparityNoise`	`Real`
input	`referenceBaseline`	`Real`
input	`referenceOpticalToBody`	`Real`	`[3,3]`
input	`referenceCameraOriginBody`	`Real`	`[3]`
input	`accel`	`Real`	`[3]`
input	`gyro`	`Real`	`[3]`
input	`gravity`	`Real`	`[3]`
input	`h`	`Real`
input	`density`	`Real`	`[12]`
input	`imageTime`	`Real`
input	`initializationRequested`	`Real`
input	`selectedFeatureLimit`	`Real`	`= 350.0`
input	`absoluteThreshold`	`Real`	`= 18.0`
output	`nextPosition`	`Real`	`[3]`
output	`nextVelocity`	`Real`	`[3]`
output	`nextRotation`	`Real`	`[3,3]`
output	`nextAccelBias`	`Real`	`[3]`
output	`nextGyroBias`	`Real`	`[3]`
output	`nextCovariance`	`Real`	`[15,15]`
output	`nextCrossCovariance`	`Real`	`[15,6]`
output	`nextReferenceCovariance`	`Real`	`[6,6]`
output	`nextReferencePosition`	`Real`	`[3]`
output	`nextReferenceRotation`	`Real`	`[3,3]`
output	`nextReferenceAvailable`	`Real`
output	`nextReferenceEpoch`	`Real`
output	`nextReferenceUsed`	`Real`
output	`nextLastUsedEpoch`	`Real`
output	`nextReferenceDescriptor`	`Real`	`[featureCapacity,descriptorSize]`
output	`nextReferencePoint`	`Real`	`[featureCapacity,3]`
output	`nextReferenceEnabled`	`Real`	`[featureCapacity]`
output	`nextReferencePixels`	`Real`	`[featureCapacity,2]`
output	`nextReferenceCount`	`Real`
output	`nextReferenceRgbCalibration`	`Real`	`[4]`
output	`nextReferenceDepthCalibration`	`Real`	`[4]`
output	`nextReferenceNoiseReferenceFx`	`Real`
output	`nextReferenceDisparityNoise`	`Real`
output	`nextReferenceBaseline`	`Real`
output	`nextReferenceOpticalToBody`	`Real`	`[3,3]`
output	`nextReferenceCameraOriginBody`	`Real`	`[3]`
output	`predictionAccepted`	`Real`
output	`observationAccepted`	`Real`
output	`observationRejected`	`Real`
output	`captureAccepted`	`Real`
output	`captureRejected`	`Real`
output	`imageReuseRejected`	`Real`
output	`imagePairEligible`	`Real`
output	`frameValid`	`Real`
output	`referenceGeometryCompatible`	`Real`
output	`visualValid`	`Real`
output	`matchCount`	`Real`
output	`uncertaintyRejectionReason`	`Real`
output	`currentDescriptor`	`Real`	`[featureCapacity,descriptorSize]`
output	`currentPoint`	`Real`	`[featureCapacity,3]`
output	`currentEnabled`	`Real`	`[featureCapacity]`
output	`currentCount`	`Real`
output	`currentFromReference`	`Real`	`[3,3]`
output	`currentFromReferenceTranslation`	`Real`	`[3]`
output	`relativeCovariance`	`Real`	`[6,6]`
output	`mapCandidatePoint`	`Real`	`[featureCapacity,3]`
output	`mapCandidateEnabled`	`Real`	`[featureCapacity]`
output	`mapCandidateCount`	`Real`
output	`nextQuaternion`	`Real`	`[4]`
output	`positionCovariance`	`Real`	`[3,3]`
output	`attitudeCovariance`	`Real`	`[3,3]`
output	`confidence`	`Real`
output	`features`	`Real`	`[featureCapacity,3]`
output	`featureEnabled`	`Real`	`[featureCapacity]`
output	`trackingCurrentPixel`	`Real`	`[featureCapacity,2]`
output	`trackingReferencePixel`	`Real`	`[featureCapacity,2]`
output	`trackingEnabled`	`Real`	`[featureCapacity]`
output	`initializationAccepted`	`Real`
output	`initializationRejected`	`Real`
output	`selectionValid`	`Real`
input	`depthQualifiedSelection`	`Boolean`	`= false`	Reserve the RGB-D budget for valid calibrated depth
input	`depthUnits`	`Real`	`= 1.0`	Meters per depth sample; 1 for metric depth, SDK scale for Z16

Modelica declarations and implementation

Show parameters, interfaces and source

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

function InitializeFastRGBDLocalization
  import FastFrameScores = Vision.Features.FastFrameScores;
  import FastSelectionScoreFloor = Vision.Features.FastSelectionScoreFloor;
  import InitializeRGBDLocalization = SLAM.Localization.InitializeRGBDLocalization;
  import RGBDDepthQualifiedScores = Vision.Matching.RGBDDepthQualifiedScores;
  import SLAMExactRealEqual = SLAM.Inertial.SLAMExactRealEqual;
  import SelectRasterFeatures = Vision.Features.SelectRasterFeatures;

  input Real rgb[:,:,:];
  input Real depth[size(rgb,1),size(rgb,2)];
  input Real rgbCalibration[4];
  input Real depthCalibration[4];
  input Real disparityNoise;
  input Real noiseReferenceFx;
  input Real baseline;
  input Real opticalToBody[3,3];
  input Real cameraOriginBody[3];
  input Real frameEnabled;
  input Real imageCaptureRequested;
  input Real position[3];
  input Real velocity[3];
  input Real rotation[3,3];
  input Real accelBias[3];
  input Real gyroBias[3];
  input Real covariance[15,15];
  input Real crossCovariance[15,6];
  input Real referenceCovariance[6,6];
  input Real referencePosition[3];
  input Real referenceRotation[3,3];
  input Real referenceAvailable;
  input Real referenceEpoch;
  input Real currentEpoch;
  input Real referenceUsed;
  input Real lastUsedEpoch;
  input Real referenceDescriptor[featureCapacity,descriptorSize];
  input Real referencePoint[featureCapacity,3];
  input Real referenceEnabled[featureCapacity];
  input Real referencePixels[featureCapacity,2];
  input Real referenceCount;
  input Real referenceRgbCalibration[4];
  input Real referenceDepthCalibration[4];
  input Real referenceNoiseReferenceFx;
  input Real referenceDisparityNoise;
  input Real referenceBaseline;
  input Real referenceOpticalToBody[3,3];
  input Real referenceCameraOriginBody[3];
  input Real accel[3];
  input Real gyro[3];
  input Real gravity[3];
  input Real h;
  input Real density[12];
  input Real imageTime;
  input Real initializationRequested;
  input Real selectedFeatureLimit = 350.0;
  input Real absoluteThreshold = 18.0;
  output Real nextPosition[3];
  output Real nextVelocity[3];
  output Real nextRotation[3,3];
  output Real nextAccelBias[3];
  output Real nextGyroBias[3];
  output Real nextCovariance[15,15];
  output Real nextCrossCovariance[15,6];
  output Real nextReferenceCovariance[6,6];
  output Real nextReferencePosition[3];
  output Real nextReferenceRotation[3,3];
  output Real nextReferenceAvailable;
  output Real nextReferenceEpoch;
  output Real nextReferenceUsed;
  output Real nextLastUsedEpoch;
  output Real nextReferenceDescriptor[featureCapacity,descriptorSize];
  output Real nextReferencePoint[featureCapacity,3];
  output Real nextReferenceEnabled[featureCapacity];
  output Real nextReferencePixels[featureCapacity,2];
  output Real nextReferenceCount;
  output Real nextReferenceRgbCalibration[4];
  output Real nextReferenceDepthCalibration[4];
  output Real nextReferenceNoiseReferenceFx;
  output Real nextReferenceDisparityNoise;
  output Real nextReferenceBaseline;
  output Real nextReferenceOpticalToBody[3,3];
  output Real nextReferenceCameraOriginBody[3];
  output Real predictionAccepted;
  output Real observationAccepted;
  output Real observationRejected;
  output Real captureAccepted;
  output Real captureRejected;
  output Real imageReuseRejected;
  output Real imagePairEligible;
  output Real frameValid;
  output Real referenceGeometryCompatible;
  output Real visualValid;
  output Real matchCount;
  output Real uncertaintyRejectionReason;
  output Real currentDescriptor[featureCapacity,descriptorSize];
  output Real currentPoint[featureCapacity,3];
  output Real currentEnabled[featureCapacity];
  output Real currentCount;
  output Real currentFromReference[3,3];
  output Real currentFromReferenceTranslation[3];
  output Real relativeCovariance[6,6];
  output Real mapCandidatePoint[featureCapacity,3];
  output Real mapCandidateEnabled[featureCapacity];
  output Real mapCandidateCount;
  output Real nextQuaternion[4];
  output Real positionCovariance[3,3];
  output Real attitudeCovariance[3,3];
  output Real confidence;
  output Real features[featureCapacity,3];
  output Real featureEnabled[featureCapacity];
  output Real trackingCurrentPixel[featureCapacity,2];
  output Real trackingReferencePixel[featureCapacity,2];
  output Real trackingEnabled[featureCapacity];
  output Real initializationAccepted;
  output Real initializationRejected;
  output Real selectionValid;
  input Boolean depthQualifiedSelection = false "Reserve the RGB-D budget for valid calibrated depth";
  input Real depthUnits = 1.0 "Meters per depth sample; 1 for metric depth, SDK scale for Z16";
protected
  constant Integer featureCapacity = 350; constant Integer descriptorSize = 49;
  Boolean imageOn; Boolean selectionConfiguration;
  Real scores[size(rgb,1)*size(rgb,2)];
  Real selectedFeatures[featureCapacity,3];
  Real selectionCount; Real selectionStatus; Real selectedCount;
algorithm
  imageOn := SLAMExactRealEqual(frameEnabled,1.0);
  scores := FastFrameScores(rgb,imageOn,FastSelectionScoreFloor(absoluteThreshold,1e8));
  if depthQualifiedSelection then
    scores := RGBDDepthQualifiedScores(depth,scores,rgbCalibration,depthCalibration,
      0.28,10.0,disparityNoise,noiseReferenceFx,baseline,imageOn,depthUnits=depthUnits);
  end if;
  (selectedFeatures,selectionCount,selectionStatus) := SelectRasterFeatures(scores,
    size(rgb,2),size(rgb,1),featureCapacity,3,
    {absoluteThreshold,0.0,1e8,3.0,selectedFeatureLimit,1.0,3.0,3.0},false,imageOn);
  selectionConfiguration := selectedFeatureLimit >= 1.0 and selectedFeatureLimit <= featureCapacity
    and SLAMExactRealEqual(selectedFeatureLimit,floor(selectedFeatureLimit));
  selectionValid := if selectionConfiguration and selectionStatus > 0.5
    and selectionCount >= 0.0 and selectionCount <= featureCapacity then 1.0 else 0.0;
  selectedCount := if selectionValid > 0.5 then selectionCount else 0.0;
  (nextPosition,nextVelocity,nextRotation,
    nextAccelBias,nextGyroBias,nextCovariance,
    nextCrossCovariance,nextReferenceCovariance,nextReferencePosition,
    nextReferenceRotation,nextReferenceAvailable,nextReferenceEpoch,
    nextReferenceUsed,nextLastUsedEpoch,nextReferenceDescriptor,
    nextReferencePoint,nextReferenceEnabled,nextReferencePixels,
    nextReferenceCount,nextReferenceRgbCalibration,nextReferenceDepthCalibration,
    nextReferenceNoiseReferenceFx,nextReferenceDisparityNoise,nextReferenceBaseline,
    nextReferenceOpticalToBody,nextReferenceCameraOriginBody,predictionAccepted,
    observationAccepted,observationRejected,captureAccepted,
    captureRejected,imageReuseRejected,imagePairEligible,
    frameValid,referenceGeometryCompatible,visualValid,
    matchCount,uncertaintyRejectionReason,currentDescriptor,
    currentPoint,currentEnabled,currentCount,
    currentFromReference,currentFromReferenceTranslation,relativeCovariance,
    mapCandidatePoint,mapCandidateEnabled,mapCandidateCount,
    nextQuaternion,positionCovariance,attitudeCovariance,
    confidence,features,featureEnabled,
    trackingCurrentPixel,trackingReferencePixel,trackingEnabled,
    initializationAccepted,initializationRejected) := InitializeRGBDLocalization(
    rgb,depth,rgbCalibration,
    depthCalibration,disparityNoise,noiseReferenceFx,
    baseline,opticalToBody,cameraOriginBody,
    frameEnabled,imageCaptureRequested,position,
    velocity,rotation,accelBias,
    gyroBias,covariance,crossCovariance,
    referenceCovariance,referencePosition,referenceRotation,
    referenceAvailable,referenceEpoch,currentEpoch,
    referenceUsed,lastUsedEpoch,referenceDescriptor,
    referencePoint,referenceEnabled,referencePixels,
    referenceCount,referenceRgbCalibration,referenceDepthCalibration,
    referenceNoiseReferenceFx,referenceDisparityNoise,referenceBaseline,
    referenceOpticalToBody,referenceCameraOriginBody,accel,
    gyro,gravity,h,
    density,selectedFeatures[1:featureCapacity,1:2],selectedCount,
    selectedFeatures[1:featureCapacity,3],imageTime,initializationRequested,depthUnits=depthUnits);
end InitializeFastRGBDLocalization;