function

SLAM.LoopClosure.RGBDLoopVerification.ScorePairs

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`sourcePoint`	`Real`	`[:,dimension]`
input	`targetPoint`	`Real`	`[size(sourcePoint,1),dimension]`
input	`pairEnabled`	`Real`	`[size(sourcePoint,1)]`
input	`rotation`	`Real`	`[dimension,dimension]`
input	`translation`	`Real`	`[dimension]`
input	`distanceLimit`	`Real`
output	`mask`	`Real`	`[size(sourcePoint,1)]`
output	`count`	`Integer`
output	`cost`	`Real`
input	`calibratedResiduals`	`Boolean`	`= false`
input	`sourceCovariance`	`Real`	`[size(sourcePoint,1),dimension,dimension] = zeros(size(sourcePoint,1),dimension,dimension)`
input	`targetCovariance`	`Real`	`[size(sourcePoint,1),dimension,dimension] = zeros(size(sourcePoint,1),dimension,dimension)`
input	`maximumNormalizedSquared`	`Real`	`= 9.0`
input	`minimumPivot`	`Real`	`= 1e-10`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function ScorePairs
    input Real sourcePoint[:,dimension];
    input Real targetPoint[size(sourcePoint,1),dimension];
    input Real pairEnabled[size(sourcePoint,1)];
    input Real rotation[dimension,dimension]; input Real translation[dimension];
    input Real distanceLimit;
    output Real mask[size(sourcePoint,1)];
    output Integer count;
    output Real cost;
    input Boolean calibratedResiduals = false;
    input Real sourceCovariance[size(sourcePoint,1),dimension,dimension] = zeros(size(sourcePoint,1),dimension,dimension);
    input Real targetCovariance[size(sourcePoint,1),dimension,dimension] = zeros(size(sourcePoint,1),dimension,dimension);
    input Real maximumNormalizedSquared = 9.0;
    input Real minimumPivot = 1e-10;
  protected
    Real residual[dimension]; Real squared; Boolean residualValid;
  algorithm
    mask := zeros(size(sourcePoint,1)); count := 0; cost := 0.0;
    for slot in 1:size(sourcePoint,1) loop
      if pairEnabled[slot] == 1.0 then
        if calibratedResiduals then
          (residualValid,squared) := RegistrationPairResidual(sourcePoint[slot,:],targetPoint[slot,:],
            rotation,translation,true,sourceCovariance[slot,:,:],targetCovariance[slot,:,:],minimumPivot);
        else
          residual := rotation*sourcePoint[slot,:]+translation-targetPoint[slot,:];
          squared := residual*residual; residualValid := squared >= 0.0;
        end if;
        if residualValid and squared <= (if calibratedResiduals then maximumNormalizedSquared else distanceLimit^2) then
          mask[slot] := 1.0; count := count+1; cost := cost+squared;
        end if;
      end if;
    end for;
  end ScorePairs;