function

SLAM.PoseGraph.PGNormalProduct

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`x`	`Real`	`[:,6]`
input	`nodeMask`	`Real`	`[size(x,1)]`
input	`edgeMask`	`Real`	`[:]`
input	`source`	`Integer`	`[size(edgeMask,1)]`
input	`target`	`Integer`	`[size(edgeMask,1)]`
input	`Ji`	`Real`	`[size(edgeMask,1),6,6]`
input	`Jj`	`Real`	`[size(edgeMask,1),6,6]`
input	`information`	`Real`	`[size(edgeMask,1),6,6]`
input	`dampingDiagonal`	`Real`	`[size(x,1),6]`
input	`damping`	`Real`
output	`y`	`Real`	`[size(x,1),6]`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function PGNormalProduct
  input Real x[:,6]; input Real nodeMask[size(x,1)];
  input Real edgeMask[:]; input Integer source[size(edgeMask,1)]; input Integer target[size(edgeMask,1)];
  input Real Ji[size(edgeMask,1),6,6]; input Real Jj[size(edgeMask,1),6,6];
  input Real information[size(edgeMask,1),6,6]; input Real dampingDiagonal[size(x,1),6]; input Real damping;
  output Real y[size(x,1),6];
protected Integer i; Integer j; Real row[6]; Real weighted[6];
algorithm
  y := zeros(size(x,1),6); i := 1; j := 1; row := zeros(6); weighted := zeros(6);
  for node in 1:size(x,1) loop
    for k in 1:6 loop
      y[node,k] := if node > 1 and nodeMask[node] == 1.0 then damping*dampingDiagonal[node,k]*x[node,k] else 0.0;
    end for;
  end for;
  for edge in 1:size(edgeMask,1) loop
    if edgeMask[edge] == 1.0 then
      i := source[edge]; j := target[edge];
      row := Ji[edge,:,:]*x[i,:]+Jj[edge,:,:]*x[j,:]; weighted := information[edge,:,:]*row;
      if i > 1 then y[i,:] := y[i,:]+transpose(Ji[edge,:,:])*weighted; end if;
      if j > 1 then y[j,:] := y[j,:]+transpose(Jj[edge,:,:])*weighted; end if;
    end if;
  end for;
  // Node1 is the exact gauge: callers keep x[1,:]=0; its operator row is zero.
end PGNormalProduct;