function

Vision.Registration.RegistrationWeighted.WeightedWhiten

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`covariance`	`Real`	`[3,3]`
input	`residual`	`Real`	`[3]`
input	`jacobian`	`Real`	`[3,6]`
input	`minimumPivot`	`Real`
output	`valid`	`Boolean`
output	`wr`	`Real`	`[3]`
output	`wj`	`Real`	`[3,6]`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function WeightedWhiten
    input Real covariance[3,3]; input Real residual[3]; input Real jacobian[3,6];
    input Real minimumPivot;
    output Boolean valid; output Real wr[3]; output Real wj[3,6];
  protected
    Real scale[3]; Real L[3,3]; Real v;
  algorithm
    valid := minimumPivot > 0 and minimumPivot < 1;
    scale := ones(3); L := zeros(3,3); wr := zeros(3); wj := zeros(3,6);
    for row in 1:3 loop
      valid := valid and covariance[row,row] > 0 and covariance[row,row] < 1e100;
      if valid then scale[row] := sqrt(covariance[row,row]); end if;
    end for;
    for row in 1:3 loop
      for column in 1:row loop
        v := covariance[row,column]/scale[row]/scale[column];
        valid := valid and abs(v) < 1e100
          and abs(covariance[row,column]-covariance[column,row]) <= 1e-10*max(1.0,abs(covariance[row,column]));
        for k in 1:column-1 loop v := v-L[row,k]*L[column,k]; end for;
        if row == column then
          valid := valid and v > minimumPivot and v < 1e100;
          if valid then L[row,column] := sqrt(v); end if;
        elseif valid then L[row,column] := v/L[column,column]; end if;
      end for;
      v := if valid then residual[row]/scale[row] else 0.0;
      for k in 1:row-1 loop v := v-L[row,k]*wr[k]; end for;
      if valid then wr[row] := v/L[row,row]; end if;
      for column in 1:6 loop
        v := if valid then jacobian[row,column]/scale[row] else 0.0;
        for k in 1:row-1 loop v := v-L[row,k]*wj[k,column]; end for;
        if valid then wj[row,column] := v/L[row,row]; end if;
      end for;
    end for;
  end WeightedWhiten;