function

SLAM.PoseGraph.ModelicaPoseGraphCovariance.Backward

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	`q`	`Real`	`[nodeCapacity,poseDimension]`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function Backward
    input Real f[nodeCapacity,poseDimension]; input Tree tree;
    output Real q[nodeCapacity,poseDimension];
  protected
    Real pending[nodeCapacity,poseDimension]; Integer node; Integer parent;
  algorithm
    q:=zeros(nodeCapacity,poseDimension); pending:=f; pending[1,:]:=zeros(poseDimension);
    node:=1; parent:=1;
    for reverseIndex in 1:nodeCapacity loop
      if reverseIndex<=tree.count then
        node:=tree.order[tree.count+1-reverseIndex]; parent:=tree.parent[node];
        q[node,:]:=transpose(tree.childInverse[node,:,:])*pending[node,:];
        if parent>1 then pending[parent,:]:=pending[parent,:]-transpose(tree.parentJacobian[node,:,:])*q[node,:]; end if;
      end if;
    end for;
  end Backward;