function

SLAM.PoseGraph.ModelicaPoseGraphCovariance.BalancedSolve

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	`tree`	`Tree`
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 BalancedSolve
    input Real f[nodeCapacity,poseDimension]; input Tree tree; 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:=TreeSolve(pending,tree);
    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 BalancedSolve;