function

SLAM.Mapping.RGBDMapAnchors.ProperRotation

Public declarations

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

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

Modelica declarations and implementation

Show parameters, interfaces and source

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

function ProperRotation
    input Real rotation[dimension,dimension];
    output Boolean valid;
  protected
    Real gram[dimension,dimension]; Real determinant;
  algorithm
    valid := true;
    for row in 1:dimension loop
      for column in 1:dimension loop
        valid := valid and abs(rotation[row,column]) <= 1.000001;
      end for;
    end for;
    if valid then
      gram := transpose(rotation)*rotation;
      for row in 1:dimension loop
        for column in 1:dimension loop
          valid := valid and abs(gram[row,column]-(if row == column then 1.0 else 0.0)) <= 1e-6;
        end for;
      end for;
      determinant := rotation[1,1]*(rotation[2,2]*rotation[3,3]-rotation[2,3]*rotation[3,2])
        -rotation[1,2]*(rotation[2,1]*rotation[3,3]-rotation[2,3]*rotation[3,1])
        +rotation[1,3]*(rotation[2,1]*rotation[3,2]-rotation[2,2]*rotation[3,1]);
      valid := valid and abs(determinant-1.0) <= 1e-6;
    end if;
  end ProperRotation;