SLAM.PoseGraph.GraphGaugeUncertainty.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 relativePositions[2,3]; input Real relativeRotations[2,3,3];
    input Real relativeJointBound[12,12]; input Real beta; input Boolean requested;
    output Estimate result; output Boolean accepted; output Integer rejectionReason;
  protected
    Boolean valid; Boolean endpoint;
    Real B[12,6]; Real C[12,12]; Real candidate[12,12]; Real lever[3,3];
    Real candidatePositions[2,3]; Real candidateRotations[2,3,3];
    Integer offset;
  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; end for;
        for node in 1:2 loop
          valid := valid and RGBDUncertaintyProper(relativeRotations[node,:,:]);
          for i in 1:3 loop valid := valid and abs(relativePositions[node,i]) <= 1e6; end for;
        end for;
      end if;
      if valid then
        rejectionReason := 5;
        valid := SLAMCovariancePSDCheck(anchorBound,1e-12) == 1
          and SLAMCovariancePSDCheck(relativeJointBound,1e-12) == 1;
      end if;
      if valid then
        rejectionReason := 6;
        for node in 1:2 loop
          endpoint := (if node == 1 then binding.currentId else binding.referenceId) == binding.anchorId;
          offset := 6*(node-1);
          if endpoint then
            for i in 1:3 loop
              valid := valid and relativePositions[node,i] == 0;
              for j in 1:3 loop valid := valid and relativeRotations[node,i,j] == (if i == j then 1 else 0); end for;
            end for;
            for i in 1:6 loop for j in 1:12 loop
              valid := valid and relativeJointBound[offset+i,j] == 0 and relativeJointBound[j,offset+i] == 0;
            end for; end for;
          end if;
        end for;
      end if;
      if valid and SameCapture(binding) then
        rejectionReason := 6;
        for i in 1:3 loop
          valid := valid and relativePositions[1,i] == relativePositions[2,i];
          for j in 1:3 loop valid := valid and relativeRotations[1,i,j] == relativeRotations[2,i,j]; end for;
        end for;
        for i in 1:6 loop for j in 1:6 loop
          valid := valid and relativeJointBound[i,j] == relativeJointBound[i,j+6]
            and relativeJointBound[i,j] == relativeJointBound[i+6,j]
            and relativeJointBound[i,j] == relativeJointBound[i+6,j+6];
        end for; end for;
      end if;
      if valid then
        B := zeros(12,6); C := zeros(12,12);
        for node in 1:2 loop
          offset := 6*(node-1); lever := -anchorRotation*RGBDUncertaintySkew(relativePositions[node,:]);
          for i in 1:3 loop for j in 1:3 loop
            B[offset+i,j] := if i == j then 1 else 0; B[offset+i,j+3] := lever[i,j];
            B[offset+i+3,j+3] := relativeRotations[node,j,i];
            C[offset+i,offset+j] := anchorRotation[i,j]; C[offset+i+3,offset+j+3] := if i == j then 1 else 0;
          end for; end for;
        end for;
        candidate := B*anchorBound*transpose(B)/beta+C*relativeJointBound*transpose(C)/(1-beta);
        rejectionReason := 7;
        valid := SLAMCovariancePSDCheck(candidate,1e-12) == 1;
        if valid then
          rejectionReason := 8;
          for node in 1:2 loop
            candidatePositions[node,:] := anchorPosition+anchorRotation*relativePositions[node,:];
            candidateRotations[node,:,:] := anchorRotation*relativeRotations[node,:,:];
            valid := valid and RGBDUncertaintyProper(candidateRotations[node,:,:]);
            for i in 1:3 loop valid := valid and abs(candidatePositions[node,i]) <= 1e6; end for;
          end for;
          if valid then
            result.binding := binding; result.covariance := candidate;
            result.positions := candidatePositions; result.rotations := candidateRotations;
            accepted := true; rejectionReason := 0;
          end if;
        end if;
      end if;
    end if;
  end Transport;