function

SLAM.PoseGraph.ModelicaPoseGraphCovariance.FactorPair

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`A`	`Real`	`[pairDimension,pairDimension]`
output	`L`	`Real`	`[pairDimension,pairDimension]`
output	`valid`	`Boolean`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function FactorPair
    input Real A[pairDimension,pairDimension];
    output Real L[pairDimension,pairDimension]; output Boolean valid;
  protected Real scale; Real value;
  algorithm
    L := zeros(pairDimension,pairDimension); scale := 0.0; value := 0.0; valid := true;
    for i in 1:pairDimension loop scale := max(scale,abs(A[i,i])); end for;
    valid := scale > 1e-12 and scale <= 1e18;
    for i in 1:pairDimension loop
      for j in 1:pairDimension loop
        valid := valid and abs(A[i,j]) <= 1e18 and abs(A[i,j]-A[j,i]) <= 1e-10*max(1.0,scale);
        value := A[i,j];
        for k in 1:pairDimension loop
          if j <= i and k < j then value := value-L[i,k]*L[j,k]; end if;
        end for;
        if j <= i then
          if i == j then
            valid := valid and value > 1e-12*scale and value <= 1e18;
            L[i,j] := sqrt(if value > 1e-12*scale and value <= 1e18 then value else 1.0);
          else L[i,j] := value/L[j,j]; end if;
        end if;
      end for;
    end for;
    if not valid then L := zeros(pairDimension,pairDimension); end if;
  end FactorPair;