function

SLAM.LoopClosure.RGBDCatalogLoopVerification.PoseConsistent

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
input	`referencePosition`	`Real`	`[3]`
input	`currentPosition`	`Real`	`[3]`
input	`referenceRotation`	`Real`	`[3,3]`
input	`referenceCovariance`	`Real`	`[6,6]`
input	`currentCovariance`	`Real`	`[6,6]`
input	`measuredTranslation`	`Real`	`[3]`
input	`measuredCovariance`	`Real`	`[3,3]`
input	`maximumSquared`	`Real`	`= 9.0`
input	`minimumPivot`	`Real`	`= 1e-10`
input	`maximumTranslationInnovation`	`Real`	`= 2.0`	Motion disagreement limit; explicit relocalization policy may enlarge it
output	`accepted`	`Boolean`
output	`squared`	`Real`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function PoseConsistent
    input Real referencePosition[3]; input Real currentPosition[3];
    input Real referenceRotation[3,3];
    input Real referenceCovariance[6,6]; input Real currentCovariance[6,6];
    input Real measuredTranslation[3]; input Real measuredCovariance[3,3];
    input Real maximumSquared = 9.0; input Real minimumPivot = 1e-10;
    input Real maximumTranslationInnovation = 2.0 "Motion disagreement limit; explicit relocalization policy may enlarge it";
    output Boolean accepted; output Real squared;
  protected
    Real residual[3]; Real arm[3,3]; Real covariance[3,3]; Boolean valid;
  algorithm
    residual := currentPosition-referencePosition-referenceRotation*measuredTranslation;
    arm := referenceRotation*RGBDUncertaintySkew(measuredTranslation);
    // A conservative screening bound, not an independent measurement update.
    // Four times the marginal sum bounds arbitrary correlations among the
    // two positions, retained-reference angle and measured translation.
    covariance := 4.0*(referenceCovariance[1:3,1:3]+currentCovariance[1:3,1:3]
      +referenceRotation*measuredCovariance*transpose(referenceRotation)
      +arm*referenceCovariance[4:6,4:6]*transpose(arm));
    (valid,squared) := RegistrationWhitenedResidual3(residual,covariance,minimumPivot);
    accepted := valid and maximumSquared > 0.0 and squared <= maximumSquared
      and maximumTranslationInnovation > 0.0
      and residual*residual <= maximumTranslationInnovation*maximumTranslationInnovation;
  end PoseConsistent;