function

SLAM.PoseGraph.RGBDGraphMeasurements.ValidUncertainty

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`covariance`	`Real`	`[poseDimension,poseDimension]`
input	`information`	`Real`	`[poseDimension,poseDimension]`
output	`valid`	`Boolean`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function ValidUncertainty
    input Real covariance[poseDimension,poseDimension];
    input Real information[poseDimension,poseDimension];
    output Boolean valid;
  protected
    Real inverseCovariance[poseDimension,poseDimension];
    Real inverseInformation[poseDimension,poseDimension];
    Real pivot; Boolean covarianceValid; Boolean informationValid;
  algorithm
    (inverseCovariance,covarianceValid,pivot) := RGBDUncertaintyInverse6(covariance,1e-10);
    (inverseInformation,informationValid,pivot) := RGBDUncertaintyInverse6(information,1e-10);
    valid := covarianceValid and informationValid;
    if valid then
      for row in 1:poseDimension loop
        for column in 1:poseDimension loop
          valid := valid and abs(inverseCovariance[row,column]-information[row,column])
            <= 1e-7*max(1.0,abs(inverseCovariance[row,column]))
            and abs(inverseInformation[row,column]-covariance[row,column])
            <= 1e-7*max(1.0,abs(inverseInformation[row,column]));
        end for;
      end for;
    end if;
  end ValidUncertainty;