function

SLAM.PoseGraph.ModelicaPoseGraphCovariance.PairSolve

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`
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 PairSolve
    input Real f[nodeCapacity,poseDimension]; input Pairs pairs;
    output Real x[nodeCapacity,poseDimension];
  protected Real z[pairDimension]; Real w[pairDimension]; Real value; Integer node; Integer axis; Integer row;
  algorithm
    x := zeros(nodeCapacity,poseDimension); z := zeros(pairDimension); w := z;
    value := 0.0; node := 1; axis := 1; row := 1;
    if pairs.valid then
      for pair in 1:pairCapacity loop
        z := zeros(pairDimension); w := z;
        for i in 1:pairDimension loop
          node := 2+2*(pair-1)+div(i-1,poseDimension); axis := mod(i-1,poseDimension)+1;
          value := 0.0;
          if node <= nodeCapacity then if pairs.active[node] then value := f[node,axis]; end if; end if;
          for k in 1:pairDimension loop if k < i then value := value-pairs.factor[pair,i,k]*z[k]; end if; end for;
          z[i] := value/pairs.factor[pair,i,i];
        end for;
        for i in 1:pairDimension loop
          row := pairDimension+1-i; value := z[row];
          for k in 1:pairDimension loop if k > row then value := value-pairs.factor[pair,k,row]*w[k]; end if; end for;
          w[row] := value/pairs.factor[pair,row,row];
        end for;
        for i in 1:pairDimension loop
          node := 2+2*(pair-1)+div(i-1,poseDimension); axis := mod(i-1,poseDimension)+1;
          if node <= nodeCapacity then if pairs.active[node] then x[node,axis] := w[i]; end if; end if;
        end for;
      end for;
    end if;
  end PairSolve;