function

SLAM.PoseGraph.PGPrecondition

Public declarations

Explicit source declarations; dimensions, modifications and defaults are shown as written, without evaluation.

Role	Name	Type	Dimensions / attributes / default
input	`rhs`	`Real`	`[:,6]`
input	`factors`	`Real`	`[size(rhs,1),6,6]`
input	`nodeMask`	`Real`	`[size(rhs,1)]`
output	`z`	`Real`	`[size(rhs,1),6]`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function PGPrecondition
  import PGSolveBlock = SLAM.PoseGraph.PGSolveBlock;

  input Real rhs[:,6]; input Real factors[size(rhs,1),6,6]; input Real nodeMask[size(rhs,1)];
  output Real z[size(rhs,1),6];
algorithm
  z := zeros(size(rhs,1),6);
  for node in 1:size(rhs,1) loop
    if node > 1 and nodeMask[node] == 1.0 then z[node,:] := PGSolveBlock(factors[node,:,:],rhs[node,:]); end if;
  end for;
end PGPrecondition;