SLAM/Localization/RGBDFastInertialLocalizationStep.mo

Download original .moGitHub source

Documentation: SLAM.Localization.RGBDFastInertialLocalizationStep

within SLAM.Localization;
model RGBDFastInertialLocalizationStep
  import AdvanceFastRGBDLocalization = SLAM.Localization.AdvanceFastRGBDLocalization;
  import RGBDInertialLocalizationInterface = SLAM.Localization.RGBDInertialLocalizationInterface;

  extends RGBDInertialLocalizationInterface;
  parameter Real selectedFeatureLimit = 350.0;
  parameter Real absoluteThreshold = 18.0;
  output Real selectionValid;
algorithm
  (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,
    selectionValid) := AdvanceFastRGBDLocalization(
    rgb=rgb,
    depth=depth,
    rgbCalibration=rgbCalibration,
    depthCalibration=depthCalibration,
    disparityNoise=disparityNoise,
    noiseReferenceFx=noiseReferenceFx,
    baseline=baseline,
    opticalToBody=opticalToBody,
    cameraOriginBody=cameraOriginBody,
    frameEnabled=frameEnabled,
    imageCaptureRequested=imageCaptureRequested,
    position=position,
    velocity=velocity,
    rotation=rotation,
    accelBias=accelBias,
    gyroBias=gyroBias,
    covariance=covariance,
    crossCovariance=crossCovariance,
    referenceCovariance=referenceCovariance,
    referencePosition=referencePosition,
    referenceRotation=referenceRotation,
    referenceAvailable=referenceAvailable,
    referenceEpoch=referenceEpoch,
    currentEpoch=currentEpoch,
    referenceUsed=referenceUsed,
    lastUsedEpoch=lastUsedEpoch,
    referenceDescriptor=referenceDescriptor,
    referencePoint=referencePoint,
    referenceEnabled=referenceEnabled,
    referencePixels=referencePixels,
    referenceCount=referenceCount,
    referenceRgbCalibration=referenceRgbCalibration,
    referenceDepthCalibration=referenceDepthCalibration,
    referenceNoiseReferenceFx=referenceNoiseReferenceFx,
    referenceDisparityNoise=referenceDisparityNoise,
    referenceBaseline=referenceBaseline,
    referenceOpticalToBody=referenceOpticalToBody,
    referenceCameraOriginBody=referenceCameraOriginBody,
    accel=accel,
    gyro=gyro,
    gravity=gravity,
    h=h,
    density=density,
    selectedFeatureLimit=selectedFeatureLimit,absoluteThreshold=absoluteThreshold,depthUnits=depthUnits);
end RGBDFastInertialLocalizationStep;