function

SLAM.PoseGraph.RGBDGraphEstimatorCommit.EstimatorState

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
input	`localization`	`RGBDLocalizationCatalog.State`
output	`result`	`SchmidtGraphPoseCorrection.State`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function EstimatorState
    input RGBDLocalizationCatalog.State localization;
    output SchmidtGraphPoseCorrection.State result;
  protected
    Integer slot;
  algorithm
    // Caller validates headers, exact Real integer domains and reference birth
    // before this conversion; no projection, registration or host algebra.
    result.position := localization.estimator.position; result.velocity := localization.estimator.velocity;
    result.rotation := localization.estimator.rotation;
    result.accelBias := localization.estimator.accelBias; result.gyroBias := localization.estimator.gyroBias;
    result.covariance := localization.estimator.covariance;
    result.crossCovariance := localization.estimator.crossCovariance;
    result.referenceCovariance := localization.estimator.referenceCovariance;
    result.referencePosition := localization.estimator.referencePosition;
    result.referenceRotation := localization.estimator.referenceRotation;
    result.referenceAvailable := localization.estimator.referenceAvailable == 1;
    result.referenceUsed := localization.estimator.referenceUsed == 1;
    result.generation := localization.generation; result.sourceRevision := localization.sourceRevision;
    result.currentId := localization.catalog.nextId-1;
    result.currentEpoch := localization.lastProcessedImageEpoch;
    result.currentCaptureSequence := localization.steps;
    result.referenceId := localization.referenceBirth.catalogId;
    result.referenceEpoch := localization.referenceBirth.epoch;
    result.referenceCaptureSequence := localization.referenceBirth.sequence;
    result.lastUsedEpoch := integer(localization.estimator.lastUsedEpoch);
    result.predictionTime := localization.predictionTime; result.referenceTime := 0;
    if result.referenceAvailable then
      slot := mod(result.referenceId-1,nodeCapacity)+1;
      result.referenceTime := localization.catalog.imageTimes[slot];
    end if;
  end EstimatorState;