function

SLAM.Inertial.SchmidtGraphPoseCorrection.InverseJacobian

Public declarations

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

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

Modelica declarations and implementation

Show parameters, interfaces and source

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

function InverseJacobian
    input Real vector[3]; input Boolean right; output Real jacobian[3,3];
  protected
    Real square; Real angle; Real coefficient; Real skew[3,3];
  algorithm
    square := sum(vector.^2); angle := sqrt(square);
    coefficient := if square < 1e-8 then 1.0/12+square/720+square^2/30240
      else (1-angle*cos(angle/2)/(2*sin(angle/2)))/square;
    skew := RGBDUncertaintySkew(vector);
    jacobian := identity(3)+(if right then 0.5 else -0.5)*skew+coefficient*(skew*skew);
  end InverseJacobian;