/// <summary>
/// show an overhead view of the grid map as an image
/// </summary>
/// <param name="grid_index">index number of the grid to be shown</param>
/// <param name="img">colour image data</param>
/// <param name="width">width in pixels</param>
/// <param name="height">height in pixels</param>
/// <param name="show_all_occupied_cells">show all occupied pixels</param>
/// <param name="map_width_mm">width of the larger map area within this will be displayed</param>
/// <param name="map_height_mm">height of the larger map area within this will be displayed</param>
/// <param name="map_centre_x_mm">centre x position of the larger map area within this will be displayed</param>
/// <param name="map_centre_y_mm">centre x position of the larger map area within this will be displayed</param>
/// <param name="clear_image">clear the image</param>
/// <param name="show_localisation">whether to show grid cells probed during localisation</param>
public void Show(
int grid_index,
byte[] img,
int width,
int height,
bool show_all_occupied_cells,
int map_width_mm,
int map_height_mm,
int map_centre_x_mm,
int map_centre_y_mm,
bool clear_image,
bool show_localisation)
{
switch (grid_type)
{
case TYPE_SIMPLE:
{
occupancygridSimple grd = (occupancygridSimple)grid[grid_index];
grd.Show(img, width, height, show_all_occupied_cells, map_width_mm, map_height_mm, map_centre_x_mm, map_centre_y_mm, clear_image, show_localisation);
break;
}
case TYPE_MULTI_HYPOTHESIS:
{
// TODO: get the best pose
particlePose pose = null;
occupancygridMultiHypothesis grd = (occupancygridMultiHypothesis)grid[grid_index];
grd.Show(
occupancygridMultiHypothesis.VIEW_ABOVE,
img, width, height, pose,
true, true);
break;
}
}
}
/// <summary>
/// insert a stereo ray into the grid
/// </summary>
/// <param name="ray">stereo ray</param>
/// <param name="sensormodel">sensor model for each grid level</param>
/// <param name="left_camera_location">left stereo camera position and orientation</param>
/// <param name="right_camera_location">right stereo camera position and orientation</param>
/// <param name="localiseOnly">whether we are mapping or localising</param>
/// <returns>localisation matching score</returns>
public float Insert(
evidenceRay ray,
stereoModel[] sensormodel,
pos3D left_camera_location,
pos3D right_camera_location,
bool localiseOnly)
{
float matchingScore = occupancygridBase.NO_OCCUPANCY_EVIDENCE;
switch (grid_type)
{
case TYPE_SIMPLE:
{
for (int grid_level = 0; grid_level < grid.Length; grid_level++)
{
rayModelLookup sensormodel_lookup = sensormodel[grid_level].ray_model;
occupancygridSimple grd = (occupancygridSimple)grid[grid_level];
float score = grd.Insert(ray, sensormodel_lookup, left_camera_location, right_camera_location, localiseOnly);
if (score != occupancygridBase.NO_OCCUPANCY_EVIDENCE)
{
if (matchingScore == occupancygridBase.NO_OCCUPANCY_EVIDENCE)
{
matchingScore = 0;
}
matchingScore += grid_weight[grid_level] * score;
}
}
break;
}
}
return(matchingScore);
}
/// <summary>
/// constructor
/// </summary>
/// <param name="no_of_grids">The number of sub grids</param>
/// <param name="grid_type">the type of sub grids</param>
/// <param name="dimension_mm">dimension of the smallest sub grid</param>
/// <param name="dimension_vertical_mm">vertical dimension of the smallest sub grid</param>
/// <param name="cellSize_mm">cell size of the smallest sub grid</param>
/// <param name="localisationRadius_mm">localisation radius within the smallest sub grid</param>
/// <param name="maxMappingRange_mm">maximum mapping radius within the smallest sub grid</param>
/// <param name="vacancyWeighting">vacancy model weighting, typically between 0.2 and 2</param>
public metagrid(
int no_of_grids,
int grid_type,
int dimension_mm,
int dimension_vertical_mm,
int cellSize_mm,
int localisationRadius_mm,
int maxMappingRange_mm,
float vacancyWeighting)
{
int dimension_cells = dimension_mm / cellSize_mm;
int dimension_cells_vertical = dimension_vertical_mm / cellSize_mm;
if (vacancyWeighting < 0.2f)
{
vacancyWeighting = 0.2f;
}
this.grid_type = grid_type;
grid = new occupancygridBase[no_of_grids];
// values used to weight the matching score for each sub grid
grid_weight = new float[no_of_grids];
grid_weight[0] = 1;
for (int i = 1; i < no_of_grids; i++)
{
grid_weight[i] = grid_weight[i] * 0.5f;
}
switch (grid_type)
{
case TYPE_SIMPLE:
{
for (int g = 0; g < no_of_grids; g++)
{
grid[g] = new occupancygridSimple(dimension_cells, dimension_cells_vertical, cellSize_mm * (g + 1), localisationRadius_mm * (g + 1), maxMappingRange_mm * (g + 1), vacancyWeighting);
}
break;
}
case TYPE_MULTI_HYPOTHESIS:
{
for (int g = 0; g < no_of_grids; g++)
{
grid[g] = new occupancygridMultiHypothesis(dimension_cells, dimension_cells_vertical, cellSize_mm * (g + 1), localisationRadius_mm * (g + 1), maxMappingRange_mm * (g + 1), vacancyWeighting);
}
break;
}
}
}
public void GridCreation()
{
int dimension_cells = 50;
int dimension_cells_vertical = 30;
int cellSize_mm = 50;
int localisationRadius_mm = 1000;
int maxMappingRange_mm = 5000;
float vacancyWeighting = 0;
occupancygridSimple grid =
new occupancygridSimple(
dimension_cells,
dimension_cells_vertical,
cellSize_mm,
localisationRadius_mm,
maxMappingRange_mm,
vacancyWeighting);
Assert.AreNotEqual(grid, null, "object occupancygridSimple was not created");
}
public void InsertRays()
{
int no_of_stereo_features = 2000;
int image_width = 640;
int image_height = 480;
int no_of_stereo_cameras = 1;
int localisationRadius_mm = 16000;
int maxMappingRange_mm = 16000;
int cellSize_mm = 32;
int dimension_cells = 16000 / cellSize_mm;
int dimension_cells_vertical = dimension_cells / 2;
float vacancyWeighting = 0.5f;
float FOV_horizontal = 78 * (float)Math.PI / 180.0f;
// create a grid
Console.WriteLine("Creating grid");
occupancygridSimple grid =
new occupancygridSimple(
dimension_cells,
dimension_cells_vertical,
cellSize_mm,
localisationRadius_mm,
maxMappingRange_mm,
vacancyWeighting);
Assert.AreNotEqual(grid, null, "object occupancygridSimple was not created");
Console.WriteLine("Creating sensor models");
stereoModel inverseSensorModel = new stereoModel();
inverseSensorModel.FOV_horizontal = FOV_horizontal;
inverseSensorModel.FOV_vertical = FOV_horizontal * image_height / image_width;
inverseSensorModel.createLookupTable(cellSize_mm, image_width, image_height);
//Assert.AreNotEqual(0, inverseSensorModel.ray_model.probability[1][5], "Ray model probabilities not updated");
// observer parameters
int pan_angle_degrees = 0;
pos3D observer = new pos3D(0, 0, 0);
observer.pan = pan_angle_degrees * (float)Math.PI / 180.0f;
float stereo_camera_baseline_mm = 100;
pos3D left_camera_location = new pos3D(stereo_camera_baseline_mm * 0.5f, 0, 0);
pos3D right_camera_location = new pos3D(-stereo_camera_baseline_mm * 0.5f, 0, 0);
left_camera_location = left_camera_location.rotate(observer.pan, observer.tilt, observer.roll);
right_camera_location = right_camera_location.rotate(observer.pan, observer.tilt, observer.roll);
left_camera_location = left_camera_location.translate(observer.x, observer.y, observer.z);
right_camera_location = right_camera_location.translate(observer.x, observer.y, observer.z);
float FOV_degrees = 78;
float[] stereo_features = new float[no_of_stereo_features * 3];
byte[,] stereo_features_colour = new byte[no_of_stereo_features, 3];
float[] stereo_features_uncertainties = new float[no_of_stereo_features];
// create some stereo disparities within the field of view
Console.WriteLine("Adding disparities");
//MersenneTwister rnd = new MersenneTwister(0);
Random rnd = new Random(0);
for (int correspondence = 0; correspondence < no_of_stereo_features; correspondence++)
{
float x = rnd.Next(image_width - 1);
float y = rnd.Next(image_height / 50) + (image_height / 2);
float disparity = 7;
if ((x < image_width / 5) || (x > image_width * 4 / 5))
{
disparity = 7; //15;
}
byte colour_red = (byte)rnd.Next(255);
byte colour_green = (byte)rnd.Next(255);
byte colour_blue = (byte)rnd.Next(255);
stereo_features[correspondence * 3] = x;
stereo_features[(correspondence * 3) + 1] = y;
stereo_features[(correspondence * 3) + 2] = disparity;
stereo_features_colour[correspondence, 0] = colour_red;
stereo_features_colour[correspondence, 1] = colour_green;
stereo_features_colour[correspondence, 2] = colour_blue;
stereo_features_uncertainties[correspondence] = 0;
}
// create an observation as a set of rays from the stereo correspondence results
List <evidenceRay>[] stereo_rays = new List <evidenceRay> [no_of_stereo_cameras];
for (int cam = 0; cam < no_of_stereo_cameras; cam++)
{
Console.WriteLine("Creating rays");
stereo_rays[cam] =
inverseSensorModel.createObservation(
observer,
stereo_camera_baseline_mm,
image_width,
image_height,
FOV_degrees,
stereo_features,
stereo_features_colour,
stereo_features_uncertainties,
true);
// insert rays into the grid
Console.WriteLine("Throwing rays");
for (int ray = 0; ray < stereo_rays[cam].Count; ray++)
{
grid.Insert(stereo_rays[cam][ray], inverseSensorModel.ray_model, left_camera_location, right_camera_location, false);
}
}
// save the result as an image
Console.WriteLine("Saving grid");
int debug_img_width = 640;
int debug_img_height = 480;
byte[] debug_img = new byte[debug_img_width * debug_img_height * 3];
Bitmap bmp = new Bitmap(debug_img_width, debug_img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
grid.Show(debug_img, debug_img_width, debug_img_height, false, false);
BitmapArrayConversions.updatebitmap_unsafe(debug_img, bmp);
bmp.Save("tests_occupancygrid_simple_InsertRays_overhead.jpg", System.Drawing.Imaging.ImageFormat.Jpeg);
grid.ShowFront(debug_img, debug_img_width, debug_img_height, true);
BitmapArrayConversions.updatebitmap_unsafe(debug_img, bmp);
bmp.Save("tests_occupancygrid_simple_InsertRays_front.jpg", System.Drawing.Imaging.ImageFormat.Jpeg);
// side view of the probabilities
float max_prob = -1;
float min_prob = 1;
float[] probs = new float[dimension_cells / 2];
float[] mean_colour = new float[3];
for (int y = dimension_cells / 2; y < dimension_cells; y++)
{
float p = grid.GetProbability(dimension_cells / 2, y, mean_colour);
probs[y - (dimension_cells / 2)] = p;
if (p != occupancygridSimple.NO_OCCUPANCY_EVIDENCE)
{
if (p < min_prob)
{
min_prob = p;
}
if (p > max_prob)
{
max_prob = p;
}
}
}
for (int i = 0; i < debug_img.Length; i++)
{
debug_img[i] = 255;
}
int prev_x = -1;
int prev_y = debug_img_height / 2;
for (int i = 0; i < probs.Length; i++)
{
if (probs[i] != occupancygridSimple.NO_OCCUPANCY_EVIDENCE)
{
int x = i * (debug_img_width - 1) / probs.Length;
int y = debug_img_height - 1 - (int)((probs[i] - min_prob) / (max_prob - min_prob) * (debug_img_height - 1));
int n = ((y * debug_img_width) + x) * 3;
if (prev_x > -1)
{
int r = 255;
int g = 0;
int b = 0;
if (probs[i] > 0.5f)
{
r = 0;
g = 255;
b = 0;
}
drawing.drawLine(debug_img, debug_img_width, debug_img_height, prev_x, prev_y, x, y, r, g, b, 0, false);
}
prev_x = x;
prev_y = y;
}
}
int y_zero = debug_img_height - 1 - (int)((0.5f - min_prob) / (max_prob - min_prob) * (debug_img_height - 1));
drawing.drawLine(debug_img, debug_img_width, debug_img_height, 0, y_zero, debug_img_width - 1, y_zero, 0, 0, 0, 0, false);
BitmapArrayConversions.updatebitmap_unsafe(debug_img, bmp);
bmp.Save("tests_occupancygrid_simple_InsertRays_probs.jpg", System.Drawing.Imaging.ImageFormat.Jpeg);
}
