function

SLAM.PoseGraph.PGProperRotation

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
input	`R`	`Real`	`[3,3]`
output	`valid`	`Boolean`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function PGProperRotation
  input Real R[3,3]; output Boolean valid;
protected Real value; Real determinant;
algorithm
  valid := true; value := 0.0;
  for i in 1:3 loop
    for j in 1:3 loop
      valid := valid and abs(R[i,j]) <= 1.000001;
      value := 0.0;
      for k in 1:3 loop value := value+R[k,i]*R[k,j]; end for;
      valid := valid and abs(value-(if i == j then 1.0 else 0.0)) <= 1e-7;
    end for;
  end for;
  determinant := R[1,1]*(R[2,2]*R[3,3]-R[2,3]*R[3,2])
    -R[1,2]*(R[2,1]*R[3,3]-R[2,3]*R[3,1])+R[1,3]*(R[2,1]*R[3,2]-R[2,2]*R[3,1]);
  valid := valid and abs(determinant-1.0) <= 1e-7;
end PGProperRotation;