/// <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; }