function

SLAM.PoseGraph.ModelicaPoseGraphCovariance.TreeSolve

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`
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 TreeSolve
    input Real f[nodeCapacity,poseDimension]; input Tree tree;
    output Real x[nodeCapacity,poseDimension];
  protected
    Real q[nodeCapacity,poseDimension]; Real w[poseDimension]; Integer node;
  algorithm
    q:=Backward(f,tree); x:=zeros(nodeCapacity,poseDimension); w:=zeros(poseDimension); node:=1;
    for index in 1:nodeCapacity loop
      if index<=tree.count then
        node:=tree.order[index]; w:=PGSolveBlock(tree.weightFactor[node,:,:],q[node,:]);
        x[node,:]:=tree.childInverse[node,:,:]*(w-tree.parentJacobian[node,:,:]*x[tree.parent[node],:]);
      end if;
    end for;
  end TreeSolve;