function

SLAM.Inertial.SchmidtGraphPoseCorrection.Log

Public declarations

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

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

Modelica declarations and implementation

Show parameters, interfaces and source

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

function Log
    input Real rotation[3,3]; output Real vector[3]; output Boolean valid;
  protected
    Real cosine; Real angle; Real square; Real factor;
  algorithm
    vector := zeros(3); valid := RGBDUncertaintyProper(rotation);
    if valid then
      cosine := min(1.0,max(-1.0,(sum(rotation[i,i] for i in 1:3)-1)/2));
      angle := acos(cosine); square := angle^2;
      valid := angle < 3.140592653589793;
      if valid then
        factor := if square < 1e-8 then 0.5+square/12+7*square^2/720 else angle/(2*sin(angle));
        vector := factor*{rotation[3,2]-rotation[2,3],rotation[1,3]-rotation[3,1],rotation[2,1]-rotation[1,2]};
      end if;
    end if;
  end Log;