function

SLAM.Localization.RGBDOpticalPointCovariance

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`p`	`Real`	`[3]`
input	`rgbFx`	`Real`
input	`rgbFy`	`Real`
input	`localizationSigma`	`Real`
input	`disparitySigma`	`Real`
input	`noiseReferenceFx`	`Real`
input	`baseline`	`Real`
input	`depthInflation`	`Real`
output	`covariance`	`Real`	`[3,3]`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function RGBDOpticalPointCovariance
  input Real p[3];
  input Real rgbFx; input Real rgbFy;
  input Real localizationSigma; input Real disparitySigma;
  input Real noiseReferenceFx; input Real baseline;
  input Real depthInflation;
  output Real covariance[3,3];
protected
  Real bearing[3]; Real depthSigma;
algorithm
  bearing := {p[1]/p[3],p[2]/p[3],1.0};
  // No reduction by interpolation neighbor count: correlated disparity bound.
  depthSigma := depthInflation*p[3]*p[3]*disparitySigma/noiseReferenceFx/baseline;
  for a in 1:3 loop
    for b in 1:3 loop
      covariance[a,b] := bearing[a]*bearing[b]*depthSigma*depthSigma;
    end for;
  end for;
  covariance[1,1] := covariance[1,1]+(p[3]*localizationSigma/rgbFx)^2;
  covariance[2,2] := covariance[2,2]+(p[3]*localizationSigma/rgbFy)^2;
end RGBDOpticalPointCovariance;