SLAM.PoseGraph.ModelicaPoseGraphCovariance.BuildTree

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

function BuildTree
    input Real nodeMask[nodeCapacity]; input Real edgeMask[edgeCapacity];
    input Integer source[edgeCapacity]; input Integer target[edgeCapacity];
    input Real Ji[edgeCapacity,poseDimension,poseDimension];
    input Real Jj[edgeCapacity,poseDimension,poseDimension];
    input Real information[edgeCapacity,poseDimension,poseDimension];
    output Tree tree;
  protected
    Boolean reached[nodeCapacity]; Boolean layer[nodeCapacity]; Boolean inverseValid; Boolean weightValid;
    Integer parent; Integer child; Real childJacobian[poseDimension,poseDimension];
    Real parentJacobian[poseDimension,poseDimension]; Real inverse[poseDimension,poseDimension];
    Real weight[poseDimension,poseDimension];
  algorithm
    tree.parent:=fill(0,nodeCapacity); tree.order:=fill(1,nodeCapacity); tree.count:=0;
    tree.childInverse:=zeros(nodeCapacity,poseDimension,poseDimension);
    tree.parentJacobian:=zeros(nodeCapacity,poseDimension,poseDimension);
    tree.weightFactor:=zeros(nodeCapacity,poseDimension,poseDimension); tree.valid:=true;
    reached:=fill(false,nodeCapacity); reached[1]:=true; layer:=reached; parent:=1; child:=1;
    childJacobian:=zeros(poseDimension,poseDimension); parentJacobian:=childJacobian;
    inverse:=childJacobian; weight:=childJacobian; inverseValid:=false; weightValid:=false;
    for pass in 1:nodeCapacity loop
      // Only vertices reached before this pass can parent new vertices. Keeping
      // the discovery layer fixed avoids selecting an entire long chain before
      // any shortcut edge is examined. Original edge order breaks equal-depth
      // ties; every factor remains in the full undamped information operator.
      layer:=reached;
      for edge in 1:edgeCapacity loop
        if tree.valid and edgeMask[edge]==1.0 then
          parent:=1; child:=1;
          if layer[source[edge]] and not reached[target[edge]] then
            parent:=source[edge]; child:=target[edge];
            childJacobian:=Jj[edge,:,:]; parentJacobian:=Ji[edge,:,:];
          elseif layer[target[edge]] and not reached[source[edge]] then
            parent:=target[edge]; child:=source[edge];
            childJacobian:=Ji[edge,:,:]; parentJacobian:=Jj[edge,:,:];
          end if;
          if child>1 then
            (inverse,inverseValid):=Inverse6(childJacobian);
            (weight,weightValid):=PGCholesky(information[edge,:,:],1e-10);
            tree.valid:=inverseValid and weightValid and tree.count<nodeCapacity-1;
            if tree.valid then
              reached[child]:=true; tree.count:=tree.count+1;
              tree.order[tree.count]:=child; tree.parent[child]:=parent;
              tree.childInverse[child,:,:]:=inverse;
              tree.parentJacobian[child,:,:]:=parentJacobian;
              tree.weightFactor[child,:,:]:=weight;
            end if;
          end if;
        end if;
      end for;
    end for;
    for node in 1:nodeCapacity loop tree.valid:=tree.valid and (nodeMask[node]==0.0 or reached[node]); end for;
  end BuildTree;