SLAM.Mapping.RGBDMapAnchors.Reproject

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

function Reproject
    input Real previousPoint[:,dimension];
    input Real previousLocalPoint[size(previousPoint,1),dimension];
    input Real previousOccupied[size(previousPoint,1)];
    input Integer previousAnchorId[size(previousPoint,1)];
    input Integer previousAnchorSlot[size(previousPoint,1)];
    input Integer mapGeneration; input Integer previousRevision;
    input Boolean nodeEnabled[:];
    input Integer nodeId[size(nodeEnabled,1)];
    input Real nodePosition[size(nodeEnabled,1),dimension];
    input Real nodeRotation[size(nodeEnabled,1),dimension,dimension];
    input Integer graphGeneration; input Integer graphRevision;
    input Boolean graphAccepted; input Boolean requested;
    input Real coordinateLimit;
    output Real point[size(previousPoint,1),dimension];
    output Real localPoint[size(previousPoint,1),dimension];
    output Real occupied[size(previousPoint,1)];
    output Integer anchorId[size(previousPoint,1)];
    output Integer anchorSlot[size(previousPoint,1)];
    output Integer nextRevision;
    output Boolean accepted;
    output Integer rejectionReason;
    output Integer projectedCount; output Integer prunedCount;
  protected
    Boolean configuration; Boolean stateValid; Boolean slotValid;
    Boolean coordinatesValid; Boolean retained;
    Integer owner; Real candidate[dimension];
  algorithm
    point := previousPoint; localPoint := previousLocalPoint;
    occupied := previousOccupied; anchorId := previousAnchorId; anchorSlot := previousAnchorSlot;
    nextRevision := previousRevision; accepted := false; rejectionReason := 1;
    projectedCount := 0; prunedCount := 0;
    if requested then
      configuration := size(previousPoint,1) >= 1 and size(previousPoint,1) <= mapCapacity
        and size(nodeEnabled,1) >= 1 and size(nodeEnabled,1) <= keyframeCapacity
        and coordinateLimit > 0.0 and coordinateLimit <= 1e6
        and mapGeneration >= 1 and mapGeneration <= identifierLimit and graphGeneration == mapGeneration
        and previousRevision >= 0 and previousRevision < identifierLimit and graphRevision == previousRevision+1;
      rejectionReason := 2;
      if configuration then
        rejectionReason := 3;
        if graphAccepted then
          rejectionReason := 4;
          if ValidNodes(nodeEnabled,nodeId,nodePosition,nodeRotation,coordinateLimit) then
            stateValid := true; coordinatesValid := true;
            for slot in 1:size(previousPoint,1) loop
              if previousOccupied[slot] == 0.0 then
                point[slot,:] := zeros(dimension); localPoint[slot,:] := zeros(dimension);
                occupied[slot] := 0.0; anchorId[slot] := 0; anchorSlot[slot] := 0;
              elseif previousOccupied[slot] == 1.0 then
                owner := previousAnchorSlot[slot];
                slotValid := previousAnchorId[slot] >= 1 and previousAnchorId[slot] <= identifierLimit
                  and owner >= 1 and owner <= size(nodeEnabled,1);
                for axis in 1:dimension loop
                  slotValid := slotValid and abs(previousPoint[slot,axis]) <= coordinateLimit
                    and abs(previousLocalPoint[slot,axis]) <= coordinateLimit;
                end for;
                stateValid := stateValid and slotValid;
                if slotValid then
                  retained := nodeEnabled[owner] and nodeId[owner] == previousAnchorId[slot];
                  if retained then
                    candidate := nodeRotation[owner,:,:]*previousLocalPoint[slot,:]+nodePosition[owner,:];
                    for axis in 1:dimension loop
                      coordinatesValid := coordinatesValid and abs(candidate[axis]) <= coordinateLimit;
                    end for;
                    point[slot,:] := candidate; projectedCount := projectedCount+1;
                  else
                    // Eviction or slot reuse cannot transfer a landmark to a
                    // new keyframe that happens to occupy the same array slot.
                    point[slot,:] := zeros(dimension); localPoint[slot,:] := zeros(dimension);
                    occupied[slot] := 0.0; anchorId[slot] := 0; anchorSlot[slot] := 0;
                    prunedCount := prunedCount+1;
                  end if;
                end if;
              else
                stateValid := false;
              end if;
            end for;
            accepted := stateValid and coordinatesValid;
            rejectionReason := if not stateValid then 5 else if not coordinatesValid then 6 else 0;
            if accepted then nextRevision := graphRevision; end if;
          end if;
        end if;
      end if;
    end if;
    // Late failures preserve all previous fields, including poisoned disabled
    // payload. Canonicalization, pruning and version advancement commit together.
    if not accepted then
      point := previousPoint; localPoint := previousLocalPoint; occupied := previousOccupied;
      anchorId := previousAnchorId; anchorSlot := previousAnchorSlot;
      nextRevision := previousRevision; projectedCount := 0; prunedCount := 0;
    end if;
  end Reproject;