function

SLAM.PoseGraph.ModelicaPoseGraphCovariance.BalancedPairSolve

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`f`	`Real`	`[nodeCapacity,poseDimension]`
input	`pairs`	`Pairs`
input	`coarse`	`Coarse`
output	`x`	`Real`	`[nodeCapacity,poseDimension]`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function BalancedPairSolve
    input Real f[nodeCapacity,poseDimension]; input Pairs pairs; input Coarse coarse;
    output Real x[nodeCapacity,poseDimension];
  protected Real coefficients[poseDimension]; Real correction[poseDimension];
    Real pending[nodeCapacity,poseDimension]; Real work[nodeCapacity,poseDimension];
  algorithm
    coefficients := zeros(poseDimension); correction := zeros(poseDimension);
    pending := f; work := zeros(nodeCapacity,poseDimension); x := work;
    for column in 1:poseDimension loop coefficients[column] := PGDot(coarse.basis[:,:,column],f); end for;
    coefficients := PGSolveBlock(coarse.factor,coefficients);
    for column in 1:poseDimension loop pending := pending-coarse.product[:,:,column]*coefficients[column]; end for;
    work := PairSolve(pending,pairs);
    for column in 1:poseDimension loop correction[column] := PGDot(coarse.product[:,:,column],work); end for;
    correction := PGSolveBlock(coarse.factor,correction);
    x := work;
    for column in 1:poseDimension loop x := x+coarse.basis[:,:,column]*(coefficients[column]-correction[column]); end for;
  end BalancedPairSolve;