/// <summary>
/// distill an individual grid particle
/// </summary>
/// <param name="hypothesis">the grid particle to be distilled</param>
public void Distill(particleGridCell hypothesis)
{
int z = hypothesis.z;
// create the distilled array
if (distilled == null)
{
distilled = new particleGridCellBase[Hypothesis.Length];
}
bool initialised = false;
if (distilled[z] == null)
{
// create a new distilled particle
distilled[z] = new particleGridCellBase();
distilled[z].colour = new byte[3];
initialised = true;
}
// update an existing distilled value
distilled[z].probabilityLogOdds += hypothesis.probabilityLogOdds;
// and update the distilled colour value
for (int col = 2; col >= 0; col--)
{
if (initialised)
{
distilled[z].colour[col] = hypothesis.colour[col];
}
else
{
distilled[z].colour[col] = (byte)((hypothesis.colour[col] +
distilled[z].colour[col]) / 2);
}
}
}
/// <summary>
/// distill a given probability value
/// </summary>
/// <param name="z">z coordinate</param>
/// <param name="probability">probability value in the range 0.0-1.0</param>
/// <param name="r">red colour component in the range 0-255</param>
/// <param name="g">green colour component in the range 0-255</param>
/// <param name="b">blue colour component in the range 0-255</param>
public void Distill(
int z,
float probability,
int r, int g, int b)
{
// create the distilled array
if (distilled == null)
{
distilled = new particleGridCellBase[Hypothesis.Length];
}
if (distilled[z] == null)
{
// create a new distilled particle
distilled[z] = new particleGridCellBase();
distilled[z].colour = new byte[3];
}
// update an existing distilled value
distilled[z].probabilityLogOdds = probabilities.LogOdds(probability);
distilled[z].colour[0] = (byte)r;
distilled[z].colour[1] = (byte)g;
distilled[z].colour[2] = (byte)b;
}
public void LoadTile(
byte[] data)
{
// read the bounding box
int array_index = 0;
const int int32_bytes = 4;
int tx = BitConverter.ToInt32(data, 0);
int ty = BitConverter.ToInt32(data, int32_bytes);
int bx = BitConverter.ToInt32(data, int32_bytes * 2);
int by = BitConverter.ToInt32(data, int32_bytes * 3);
array_index = int32_bytes * 4;
// dimensions of the box
int w1 = bx - tx;
int w2 = by - ty;
//Read binary index as a byte array
int no_of_bits = w1 * w2;
int no_of_bytes = no_of_bits / 8;
if (no_of_bytes * 8 < no_of_bits) no_of_bytes++;
byte[] indexData = new byte[no_of_bytes];
for (int i = 0; i < no_of_bytes; i++)
indexData[i] = data[array_index + i];
bool[] binary_index = ArrayConversions.ToBooleanArray(indexData);
array_index += no_of_bytes;
int n = 0;
int occupied_cells = 0;
for (int y = ty; y < by; y++)
{
for (int x = tx; x < bx; x++)
{
if (binary_index[n])
{
cell[x][y] = new occupancygridCellMultiHypothesis(dimension_cells_vertical);
occupied_cells++;
}
n++;
}
}
if (occupied_cells > 0)
{
const int float_bytes = 4;
// read occupancy values
no_of_bytes = occupied_cells * dimension_cells_vertical * float_bytes;
byte[] occupancyData = new byte[no_of_bytes];
for (int i = 0; i < no_of_bytes; i++)
occupancyData[i] = data[array_index + i];
array_index += no_of_bytes;
float[] occupancy = ArrayConversions.ToFloatArray(occupancyData);
// read colour values
no_of_bytes = occupied_cells * dimension_cells_vertical * 3;
byte[] colourData = new byte[no_of_bytes];
for (int i = 0; i < no_of_bytes; i++)
colourData[i] = data[array_index + i];
array_index += no_of_bytes;
// insert the data into the grid
n = 0;
for (int y = ty; y < by; y++)
{
for (int x = tx; x < bx; x++)
{
if (cell[x][y] != null)
{
particleGridCellBase[] distilled = new particleGridCellBase[dimension_cells_vertical];
for (int z = 0; z < dimension_cells_vertical; z++)
{
// set the probability value
int index = (n * dimension_cells_vertical) + z;
float probLogOdds = occupancy[index];
if (probLogOdds != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
{
// create a distilled grid particle
distilled[z] = new particleGridCellBase();
distilled[z].probabilityLogOdds = probLogOdds;
// set the colour
distilled[z].colour = new byte[3];
for (int col = 0; col < 3; col++)
distilled[z].colour[col] = colourData[(index * 3) + col];
}
}
// insert the distilled particles into the grid cell
cell[x][y].SetDistilledValues(distilled);
// update the navigable space
updateNavigableSpace(null, x, y);
n++;
}
}
}
}
}
/// <summary>
/// sets distilled probability values
/// This is used when loading a grid from file
/// </summary>
/// <param name="distilled"></param>
public void SetDistilledValues(particleGridCellBase[] distilled)
{
this.distilled = distilled;
}
/// <summary>
/// distill an individual grid particle
/// </summary>
/// <param name="hypothesis">the grid particle to be distilled</param>
public void Distill(particleGridCell hypothesis)
{
int z = hypothesis.z;
// create the distilled array
if (distilled == null)
distilled = new particleGridCellBase[Hypothesis.Length];
bool initialised = false;
if (distilled[z] == null)
{
// create a new distilled particle
distilled[z] = new particleGridCellBase();
distilled[z].colour = new byte[3];
initialised = true;
}
// update an existing distilled value
distilled[z].probabilityLogOdds += hypothesis.probabilityLogOdds;
// and update the distilled colour value
for (int col = 2; col >= 0; col--)
{
if (initialised)
distilled[z].colour[col] = hypothesis.colour[col];
else
distilled[z].colour[col] = (byte)((hypothesis.colour[col] +
distilled[z].colour[col]) / 2);
}
}
/// <summary>
/// distill a given probability value
/// </summary>
/// <param name="z">z coordinate</param>
/// <param name="probability">probability value in the range 0.0-1.0</param>
/// <param name="r">red colour component in the range 0-255</param>
/// <param name="g">green colour component in the range 0-255</param>
/// <param name="b">blue colour component in the range 0-255</param>
public void Distill(
int z,
float probability,
int r, int g, int b)
{
// create the distilled array
if (distilled == null)
distilled = new particleGridCellBase[Hypothesis.Length];
if (distilled[z] == null)
{
// create a new distilled particle
distilled[z] = new particleGridCellBase();
distilled[z].colour = new byte[3];
}
// update an existing distilled value
distilled[z].probabilityLogOdds = probabilities.LogOdds(probability);
distilled[z].colour[0] = (byte)r;
distilled[z].colour[1] = (byte)g;
distilled[z].colour[2] = (byte)b;
}
