model

SLAM.Localization.RGBDRelativePose

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
constant	`dimension`	`Integer`	`= 3`
input	`referenceBodyRotation`	`Real`	`[dimension,dimension] = identity(dimension)`
input	`referenceBodyPosition`	`Real`	`[dimension] = zeros(dimension)`
input	`currentFromReference`	`Real`	`[dimension,dimension] = identity(dimension)`
input	`currentFromReferenceTranslation`	`Real`	`[dimension] = zeros(dimension)`
input	`opticalToBody`	`Real`	`[dimension,dimension] = [0.0,0.0,1.0;-1.0,0.0,0.0;0.0,-1.0,0.0]`
input	`cameraOriginBody`	`Real`	`[dimension] = {0.18,0.0,-0.04}`
input	`registrationAccepted`	`Real`	`= 0.0`
output	`valid`	`Real`
output	`observedBodyRotation`	`Real`	`[dimension,dimension]`
output	`observedBodyPosition`	`Real`	`[dimension]`

Modelica declarations and implementation

Show parameters, interfaces and source

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

model RGBDRelativePose
  import RGBDProperRotation = SLAM.Localization.RGBDProperRotation;

  constant Integer dimension = 3;
  input Real referenceBodyRotation[dimension,dimension] = identity(dimension);
  input Real referenceBodyPosition[dimension] = zeros(dimension);
  input Real currentFromReference[dimension,dimension] = identity(dimension);
  input Real currentFromReferenceTranslation[dimension] = zeros(dimension);
  input Real opticalToBody[dimension,dimension] = [0.0,0.0,1.0;-1.0,0.0,0.0;0.0,-1.0,0.0];
  input Real cameraOriginBody[dimension] = {0.18,0.0,-0.04};
  input Real registrationAccepted = 0.0;
  output Real valid;
  output Real observedBodyRotation[dimension,dimension];
  output Real observedBodyPosition[dimension];
protected
  parameter Real coordinateLimit = 1e6;
  RGBDProperRotation referenceCheck(rotation=referenceBodyRotation);
  RGBDProperRotation registrationCheck(rotation=currentFromReference);
  RGBDProperRotation extrinsicsCheck(rotation=opticalToBody);
  Real positionChecks[dimension];
  Real referenceCameraRotation[dimension,dimension];
  Real referenceCameraPosition[dimension];
  Real currentCameraRotation[dimension,dimension];
  Real currentCameraPosition[dimension];
  Real proposedBodyRotation[dimension,dimension];
  Real proposedBodyPosition[dimension];
equation
  referenceCameraRotation = referenceBodyRotation*opticalToBody;
  referenceCameraPosition = referenceBodyPosition+referenceBodyRotation*cameraOriginBody;
  currentCameraRotation = referenceCameraRotation*transpose(currentFromReference);
  currentCameraPosition = referenceCameraPosition-currentCameraRotation*currentFromReferenceTranslation;
  proposedBodyRotation = currentCameraRotation*transpose(opticalToBody);
  proposedBodyPosition = currentCameraPosition-proposedBodyRotation*cameraOriginBody;
  for i in 1:dimension loop
    positionChecks[i] = if noEvent(abs(referenceBodyPosition[i]) <= coordinateLimit
      and abs(currentFromReferenceTranslation[i]) <= coordinateLimit
      and abs(cameraOriginBody[i]) <= coordinateLimit
      and abs(proposedBodyPosition[i]) <= coordinateLimit) then 0.0 else 1.0;
  end for;
  valid = if noEvent(registrationAccepted >= 1.0 and registrationAccepted <= 1.0
    and referenceCheck.valid > 0.5 and registrationCheck.valid > 0.5
    and extrinsicsCheck.valid > 0.5 and sum(positionChecks) < 0.5) then 1.0 else 0.0;
  observedBodyRotation = if noEvent(valid > 0.5) then proposedBodyRotation else identity(dimension);
  observedBodyPosition = if noEvent(valid > 0.5) then proposedBodyPosition else zeros(dimension);
end RGBDRelativePose;