/// <summary>
/// Writes additional value to stored grid positions,
/// using either the originally added positions
/// or the positions calculated in <see cref="Process"/>.
/// </summary>
/// <param name="buffer"><see cref="GridBuffer"/> used by the sensor</param>
/// <param name="channel">Grid channel index</param>
/// <param name="value">The value to write</param>
public virtual void Write(GridBuffer buffer, int channel, float value)
{
foreach (Vector2Int pos in GridPositions)
{
buffer.Write(channel, pos, value);
}
}
/// <inheritdoc/>
public override void Process(GridBuffer buffer, int channel)
{
// Side length of squares around points required for filling
// Width x Height, IF positions were distributed evenly.
float sparseness = Mathf.Sqrt(Width * Height / (float)GridPositions.Count);
if (sparseness > 1)
{
sparseness *= c_Multiplier;
foreach (Vector2Int pGrid in GridPositions)
{
int xGrid = pGrid.x;
int yGrid = pGrid.y;
float proximity = m_AuxGrid.Read(0, xGrid, yGrid);
// Get matching kernel size for current proximity and point sparseness.
var kernel = m_Kernels[Mathf.Min(Mathf.RoundToInt(sparseness * proximity), c_MaxRadius - 1)];
float bufferValue = buffer.Read(channel, xGrid, yGrid);
foreach (var pKernel in kernel)
{
int xDilate = xGrid + pKernel.x;
int yDilate = yGrid + pKernel.y;
// TryRead -> Dilation area might go beyond grid size.
if (m_AuxGrid.TryRead(1, xDilate, yDilate, out float kernelValue))
{
// Occlusion, 3D specific:
// Write maximum kernel value if already set.
m_AuxGrid.Write(1, xDilate, yDilate, Mathf.Max(pKernel.value, kernelValue));
// Write maximum buffer value if already set.
m_AuxGrid.Write(2, xDilate, yDilate, Mathf.Max(m_AuxGrid.Read(2, xDilate, yDilate), bufferValue));
Expand(xDilate, yDilate);
}
}
}
// Expanded area.
for (int x = m_xMin; x <= m_xMax; x++)
{
for (int y = m_yMin; y <= m_yMax; y++)
{
if (m_AuxGrid.Read(1, x, y) > c_Threshold)
{
// Copy back dilated buffer values.
buffer.Write(channel, x, y, m_AuxGrid.Read(2, x, y));
// Store for later Write(buffer, channel, value) call.
GridPositions.Add(new Vector2Int(x, y));
}
}
}
}
// else: keep original positions, they're dense enough.
}
/// <inheritdoc/>
public override void Process(GridBuffer buffer, int channel)
{
int xCenter = (m_xMin + m_xMax) / 2;
int yCenter = (m_yMin + m_yMax) / 2;
float tmpValue = 0;
for (int x = m_xMin; x <= xCenter; x++)
{
int xLeft = Mathf.Max(m_xMin, x - 1);
for (int y = m_yMin; y <= yCenter; y++)
{
float bufferValue = buffer.Read(channel, x, y);
if (bufferValue == 0)
{
int yTop = Mathf.Max(m_yMin, y - 1);
bufferValue = Mathf.Max(
buffer.Read(channel, xLeft, y),
buffer.Read(channel, x, yTop),
buffer.Read(channel, xLeft, yTop));
if (bufferValue > 0)
{
buffer.Write(channel, x, y, bufferValue);
tmpValue = bufferValue;
// Store for later Write(buffer, channel, value) call.
GridPositions.Add(new Vector2Int(x, y));
}
}
else
{
// Occlusion, 3D specific:
// Write maximum buffer value if already set.
buffer.Write(channel, x, y, Mathf.Max(tmpValue, bufferValue));
}
}
}
tmpValue = 0;
for (int x = m_xMax; x > xCenter; x--)
{
int xRight = Mathf.Min(m_xMax, x + 1);
for (int y = m_yMin; y <= yCenter; y++)
{
float bufferValue = buffer.Read(channel, x, y);
if (bufferValue == 0)
{
int yTop = Mathf.Max(m_yMin, y - 1);
bufferValue = Mathf.Max(
buffer.Read(channel, xRight, y),
buffer.Read(channel, x, yTop),
buffer.Read(channel, xRight, yTop));
if (bufferValue > 0)
{
buffer.Write(channel, x, y, bufferValue);
tmpValue = bufferValue;
// Store for later Write(buffer, channel, value) call.
GridPositions.Add(new Vector2Int(x, y));
}
}
else
{
// Occlusion, 3D specific:
// Write maximum buffer value if already set.
buffer.Write(channel, x, y, Mathf.Max(tmpValue, bufferValue));
}
}
}
tmpValue = 0;
for (int x = m_xMin; x <= xCenter; x++)
{
int xLeft = Mathf.Max(m_xMin, x - 1);
for (int y = m_yMax; y > yCenter; y--)
{
float bufferValue = buffer.Read(channel, x, y);
if (bufferValue == 0)
{
int yBottom = Mathf.Min(m_yMax, y + 1);
bufferValue = Mathf.Max(
buffer.Read(channel, xLeft, y),
buffer.Read(channel, x, yBottom),
buffer.Read(channel, xLeft, yBottom));
if (bufferValue > 0)
{
buffer.Write(channel, x, y, bufferValue);
tmpValue = bufferValue;
// Store for later Write(buffer, channel, value) call.
GridPositions.Add(new Vector2Int(x, y));
}
}
else
{
// Occlusion, 3D specific:
// Write maximum buffer value if already set.
buffer.Write(channel, x, y, Mathf.Max(tmpValue, bufferValue));
}
}
}
tmpValue = 0;
for (int x = m_xMax; x > xCenter; x--)
{
int xRight = Mathf.Min(m_xMax, x + 1);
for (int y = m_yMax; y > yCenter; y--)
{
float bufferValue = buffer.Read(channel, x, y);
if (bufferValue == 0)
{
int yBottom = Mathf.Min(m_yMax, y + 1);
bufferValue = Mathf.Max(
buffer.Read(channel, xRight, y),
buffer.Read(channel, x, yBottom),
buffer.Read(channel, xRight, yBottom));
if (bufferValue > 0)
{
buffer.Write(channel, x, y, bufferValue);
tmpValue = bufferValue;
// Store for later Write(buffer, channel, value) call.
GridPositions.Add(new Vector2Int(x, y));
}
}
else
{
// Occlusion, 3D specific:
// Write maximum buffer value if already set.
buffer.Write(channel, x, y, Mathf.Max(tmpValue, bufferValue));
}
}
}
}
/// <inheritdoc/>
public void Encode(DetectionResult result)
{
m_GridBuffer.Clear();
if (m_Debug_IsEnabled)
{
m_Debug_ChannelData.ClearGridPositions();
}
int firstTagChannel = 0;
// TODO Too many nested loops.
foreach (var tag in result.DetectableTags)
{
// Observables for current tag.
var tagObs = m_Settings.GetObservables(tag);
if (result.TryGetItems(tag, out IList <DetectionResult.Item> tagItems))
{
// Has detection result for current tag -> get matching modifier.
var modifier = m_PointModifiersByTag[tag];
foreach (var item in tagItems)
{
modifier.Reset();
int channel = firstTagChannel;
// Iterate observables for current result item.
foreach (var obs in tagObs)
{
// We evaluate the observable here and write obsValue
// to all grid positions below, UNLESS it's distance.
bool encodeDistance = obs.Evaluate(
item.Detectable, out float obsValue)
== ObservableType.Distance;
if (!modifier.HasGridPositions)
{
if (item.HasPoints)
{
// Is first observable we have any points for.
foreach (var point in item.NormPoints)
{
// Normalized item point -> grid position.
Vector2Int pos = m_GridBuffer.NormalizedToGridPos(point);
if (encodeDistance)
{
// Ignore obsValue (is 0 for distance).
// (Weighted) inverse distance, 1 (near) - 0 (far).
float proximity = m_Normalization.Evaluate(point.z);
// Override if closer.
if (proximity > m_GridBuffer.Read(channel, pos))
{
// Write unmodified position to buffer.
m_GridBuffer.Write(channel, pos, proximity);
// Write unmodified position to modifier.
modifier.AddPosition(pos, 1 - point.z); // unweighted
}
// else: ignore occluded.
}
else
{
// Write unmodified position to buffer.
m_GridBuffer.Write(channel, pos, obsValue);
// Write unmodified position to modifier.
// NOTE Need some proximity value for PointModDilation.
// Passing 0.5 to get a visible result, 0 does nothing.
modifier.AddPosition(pos, 0.5f);
}
}
// Will write additional positions to buffer.
// NOTE So far, all point modifiers are only ADDING positions,
// but that might not be the case for future modifiers.
modifier.Process(m_GridBuffer, channel);
if (m_Debug_IsEnabled)
{
// Store modified grid positions.
m_Debug_ChannelData.AddGridPositions(channel, modifier.GridPositions);
}
}
}
else
{
// Is additional observable.
//
// The modifier already contains the required grid positions,
// even if we haven't applied any modifications (PointModNone).
//
// The reason for requiring distance as the first observable,
// is that we write obsValue to ALL stored grid positions here.
// Distance is the only observable with individual position values.
//
// Will write all positions to buffer.
modifier.Write(m_GridBuffer, channel, obsValue);
if (m_Debug_IsEnabled)
{
// Store modified grid positions.
m_Debug_ChannelData.AddGridPositions(channel, modifier.GridPositions);
}
}
// Next observable...
channel++;
}
}
}
firstTagChannel += tagObs.Count();
}
}
/// <inheritdoc/>
public override void AddPosition(Vector2Int point, float proximity)
{
base.AddPosition(point, proximity);
m_AuxGrid.Write(0, point, proximity);
}
/// <inheritdoc/>
public override void Process(GridBuffer buffer, int channel)
{
// Side length of squares around points required for filling
// Width x Height, IF positions were distributed evenly.
float sparseness = Mathf.Sqrt(Width * Height / (float)GridPositions.Count);
if (sparseness > 1)
{
// Make downsample area a bit larger than
// sparseness value in order to prevent gaps.
int size = Mathf.RoundToInt(sparseness) + 2; // TBD pad
// Bottom/left offset.
int xOffset = m_xMin - (size - Width % size) / 2;
int yOffset = m_yMin - (size - Height % size) / 2;
int nx = 0, ny = 0;
foreach (Vector2Int point in GridPositions)
{
// Downsampling: grid pos -> sample pos.
int xSample = (point.x - xOffset) / size;
int ySample = (point.y - yOffset) / size;
nx = Mathf.Max(nx, xSample);
ny = Mathf.Max(ny, ySample);
m_Samples[xSample, ySample] = Mathf.Max(
m_Samples[xSample, ySample], buffer.Read(channel, point));
}
// Replace grid points with squares (size x size).
GridPositions.Clear();
for (int xSample = 0; xSample <= nx; xSample++)
{
for (int ySample = 0; ySample <= ny; ySample++)
{
float sampleValue = m_Samples[xSample, ySample];
if (sampleValue > 0)
{
// Upscaling: sample pos -> grid pos.
int xGrid = xOffset + xSample * size;
int yGrid = yOffset + ySample * size;
for (int x = 0; x < size; x++)
{
for (int y = 0; y < size; y++)
{
Vector2Int gridPos = new Vector2Int(xGrid + x, yGrid + y);
// TryRead -> Square might go beyond grid size.
if (buffer.TryRead(channel, gridPos, out float bufferValue))
{
// Occlusion, 3D specific:
// Write maximum buffer value if already set.
buffer.Write(channel, gridPos, Mathf.Max(bufferValue, sampleValue));
// Store for later Write(buffer, channel, value) call.
GridPositions.Add(gridPos);
}
}
}
}
}
}
}
// else: keep original positions, they're dense enough.
}