SLAM.Mapping.RGBDMapAnchorUncertainty.Transport

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

function Transport
    input Estimate previous; input Binding binding; input Binding expectedBinding;
    input Real anchorPosition[3]; input Real anchorRotation[3,3]; input Real anchorBound[6,6];
    input Real localPoint[3]; input Real localBound[3,3]; input Real beta; input Boolean requested;
    output Estimate result; output Boolean accepted; output Integer rejectionReason;
  protected
    Boolean valid; Real J[3,6]; Real lever[3,3]; Real point[3]; Real covariance[3,3];
  algorithm
    result := previous; accepted := false; rejectionReason := 1;
    if requested then
      rejectionReason := 2; valid := ValidBinding(binding) and SameBinding(binding,expectedBinding);
      if valid then rejectionReason := 3; valid := beta > 0 and beta < 1; end if;
      if valid then
        rejectionReason := 4; valid := RGBDUncertaintyProper(anchorRotation);
        for i in 1:3 loop valid := valid and abs(anchorPosition[i]) <= 1e6 and abs(localPoint[i]) <= 1e6; end for;
      end if;
      if valid then
        rejectionReason := 5;
        valid := SLAMCovariancePSDCheck(anchorBound,1e-12) == 1 and SLAMCovariancePSDCheck(localBound,1e-12) == 1;
      end if;
      if valid then
        J := zeros(3,6); J[:,1:3] := identity(3); lever := -anchorRotation*RGBDUncertaintySkew(localPoint); J[:,4:6] := lever;
        point := anchorPosition+anchorRotation*localPoint;
        covariance := J*anchorBound*transpose(J)/beta+anchorRotation*localBound*transpose(anchorRotation)/(1-beta);
        rejectionReason := 6;
        valid := SLAMCovariancePSDCheck(covariance,1e-12) == 1;
        for i in 1:3 loop valid := valid and abs(point[i]) <= 1e6; end for;
        if valid then
          result.binding := binding; result.worldPoint := point; result.covariance := covariance;
          accepted := true; rejectionReason := 0;
        end if;
      end if;
    end if;
  end Transport;