SLAM.Mapping.RGBDSpatialIndex.Build

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

function Build
    input Real point[:,3];
    input Real occupied[size(point,1)];
    input Real voxelWidth;
    input Integer bucketCount = defaultBucketCount;
    output Integer head[bucketCount];
    output Integer link[size(point,1)];
    output Integer cell[size(point,1),3];
    output Integer freeNext[size(point,1)];
    output Integer firstFree;
    output Boolean valid;
  protected
    Integer slot;
    Integer bucket;
    Boolean pointValid;
  algorithm
    head := fill(0,bucketCount);
    link := fill(0,size(point,1));
    cell := fill(0,size(point,1),3);
    freeNext := fill(0,size(point,1));
    firstFree := 0;
    valid := size(point,1) > 0 and size(point,1) <= maximumSlotCount
      and bucketCount >= 1 and bucketCount <= maximumBucketCount
      and voxelWidth >= 0.001 and voxelWidth <= 10.0;
    if valid then
      // Reverse construction makes both chains and the free list ascending.
      // Storage uses array indices, with no lossy collision replacement.
      // Allocation and buffer reuse are compiler/runtime responsibilities.
      for reverse in 1:size(point,1) loop
        slot := size(point,1)+1-reverse;
        if occupied[slot] == 1.0 then
          (cell[slot,:],pointValid) := Cell(point[slot,:],voxelWidth);
          valid := valid and pointValid;
          if pointValid then
            bucket := Bucket(cell[slot,:],bucketCount);
            link[slot] := head[bucket];
            head[bucket] := slot;
          end if;
        elseif occupied[slot] == 0.0 then
          freeNext[slot] := firstFree;
          firstFree := slot;
        else
          valid := false;
        end if;
      end for;
    end if;
    if not valid then
      head := fill(0,bucketCount);
      link := fill(0,size(point,1));
      cell := fill(0,size(point,1),3);
      freeNext := fill(0,size(point,1));
      firstFree := 0;
    end if;
  end Build;