function

SLAM.PoseGraph.PGDampedFactor

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`localBlock`	`Real`	`[6,6]`
input	`damping`	`Real`
output	`L`	`Real`	`[6,6]`
output	`diagonal`	`Real`	`[6]`
output	`valid`	`Boolean`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function PGDampedFactor
  import PGCholesky = SLAM.PoseGraph.PGCholesky;

  input Real localBlock[6,6]; input Real damping;
  output Real L[6,6]; output Real diagonal[6]; output Boolean valid;
protected Real A[6,6];
algorithm
  A := localBlock; diagonal := zeros(6);
  for k in 1:6 loop diagonal[k] := max(A[k,k],1e-6); A[k,k] := A[k,k]+damping*diagonal[k]; end for;
  (L,valid) := PGCholesky(A,1e-12);
end PGDampedFactor;