function

SLAM.Inertial.SchmidtGraphPoseCorrection.Exp

Public declarations

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

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

Modelica declarations and implementation

Show parameters, interfaces and source

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

function Exp
    input Real vector[3]; output Real rotation[3,3];
  protected
    Real square; Real angle; Real a; Real b; Real skew[3,3];
  algorithm
    square := sum(vector.^2); angle := sqrt(square);
    a := if square < 1e-8 then 1-square/6+square^2/120 else sin(angle)/angle;
    b := if square < 1e-8 then 0.5-square/24+square^2/720 else (1-cos(angle))/square;
    skew := RGBDUncertaintySkew(vector);
    rotation := identity(3)+a*skew+b*(skew*skew);
  end Exp;