function

SLAM.PoseGraph.PGRun

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`p`	`Real`	`[:,3]`
input	`R`	`Real`	`[size(p,1),3,3]`
input	`nodeMask`	`Real`	`[size(p,1)]`
input	`edgeMask`	`Real`	`[:]`
input	`source`	`Integer`	`[size(edgeMask,1)]`
input	`target`	`Integer`	`[size(edgeMask,1)]`
input	`translation`	`Real`	`[size(edgeMask,1),3]`
input	`measuredRotation`	`Real`	`[size(edgeMask,1),3,3]`
input	`information`	`Real`	`[size(edgeMask,1),6,6]`
input	`initialCost`	`Real`
input	`initialDamping`	`Real`
input	`maximumPositionStep`	`Real`
input	`maximumAngleStep`	`Real`
input	`pcgTolerance`	`Real`
input	`maximumIterations`	`Integer`
input	`maximumPCG`	`Integer`
input	`maximumBacktracks`	`Integer`
output	`nextP`	`Real`	`[size(p,1),3]`
output	`nextR`	`Real`	`[size(p,1),3,3]`
output	`cost`	`Real`
output	`acceptedIterations`	`Real`
output	`pcgIterations`	`Real`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function PGRun
  import PGStep = SLAM.PoseGraph.PGStep;

  input Real p[:,3]; input Real R[size(p,1),3,3]; input Real nodeMask[size(p,1)]; input Real edgeMask[:];
  input Integer source[size(edgeMask,1)]; input Integer target[size(edgeMask,1)];
  input Real translation[size(edgeMask,1),3]; input Real measuredRotation[size(edgeMask,1),3,3]; input Real information[size(edgeMask,1),6,6];
  input Real initialCost; input Real initialDamping; input Real maximumPositionStep; input Real maximumAngleStep; input Real pcgTolerance;
  input Integer maximumIterations; input Integer maximumPCG; input Integer maximumBacktracks;
  output Real nextP[size(p,1),3]; output Real nextR[size(p,1),3,3]; output Real cost; output Real acceptedIterations; output Real pcgIterations;
protected Real damping; Real accepted; Real iterations; Boolean running;
algorithm
  nextP := p; nextR := R; cost := initialCost; acceptedIterations := 0.0; pcgIterations := 0.0;
  damping := initialDamping; accepted := 0.0; iterations := 0.0; running := true;
  for iteration in 1:16 loop
    if running and iteration <= maximumIterations then
      (nextP,nextR,cost,damping,accepted,iterations,running) := PGStep(nextP,nextR,nodeMask,edgeMask,source,target,
        translation,measuredRotation,information,cost,damping,maximumPositionStep,maximumAngleStep,pcgTolerance,maximumPCG,maximumBacktracks);
      acceptedIterations := acceptedIterations+accepted; pcgIterations := pcgIterations+iterations;
    end if;
  end for;
end PGRun;