function

SLAM.PoseGraph.PGGraphCost

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`p`	`Real`	`[:,3]`
input	`R`	`Real`	`[size(p,1),3,3]`
input	`edgeMask`	`Real`	`[:]`
input	`source`	`Integer`	`[size(edgeMask,1)]`
input	`target`	`Integer`	`[size(edgeMask,1)]`
input	`translation`	`Real`	`[size(edgeMask,1),3]`
input	`measuredRotation`	`Real`	`[size(edgeMask,1),3,3]`
input	`information`	`Real`	`[size(edgeMask,1),6,6]`
output	`cost`	`Real`
output	`valid`	`Boolean`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function PGGraphCost
  import PGEdge = SLAM.PoseGraph.PGEdge;

  input Real p[:,3]; input Real R[size(p,1),3,3]; input Real edgeMask[:];
  input Integer source[size(edgeMask,1)]; input Integer target[size(edgeMask,1)];
  input Real translation[size(edgeMask,1),3]; input Real measuredRotation[size(edgeMask,1),3,3];
  input Real information[size(edgeMask,1),6,6];
  output Real cost; output Boolean valid;
protected Real residual[6]; Real Ji[6,6]; Real Jj[6,6]; Boolean edgeValid;
algorithm
  cost := 0.0; valid := true; edgeValid := false; residual := zeros(6); Ji := zeros(6,6); Jj := zeros(6,6);
  for edge in 1:size(edgeMask,1) loop
    if edgeMask[edge] == 1.0 then
      (residual,Ji,Jj,edgeValid) := PGEdge(p[source[edge],:],R[source[edge],:,:],
        p[target[edge],:],R[target[edge],:,:],translation[edge,:],measuredRotation[edge,:,:]);
      valid := valid and edgeValid; cost := cost+0.5*(residual*(information[edge,:,:]*residual));
    end if;
  end for;
  valid := valid and cost >= 0.0 and cost <= 1e100;
end PGGraphCost;