public XmlElement getXmlSensorModels(XmlDocument doc, XmlElement parent)
{
XmlElement nodeModels = doc.CreateElement("StereoCameraSensorModels");
parent.AppendChild(nodeModels);
int no_of_stereo_cameras = image_width.Length;
int no_of_grid_levels = sensormodel[0].Length;
xml.AddTextElement(doc, nodeModels, "NoOfStereoCameras", Convert.ToString(no_of_stereo_cameras));
xml.AddTextElement(doc, nodeModels, "NoOfGridLevels", Convert.ToString(no_of_grid_levels));
for (int stereo_cam = 0; stereo_cam < no_of_stereo_cameras; stereo_cam++)
{
for (int size = 0; size < no_of_grid_levels; size++)
{
XmlElement nodeCamera = doc.CreateElement("Model");
nodeModels.AppendChild(nodeCamera);
rayModelLookup ray_model = sensormodel[stereo_cam][size].ray_model;
nodeCamera.AppendChild(
ray_model.getXml(doc, nodeCamera));
}
}
return(nodeModels);
}
/// <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>
/// insert a stereo ray into the grid
/// </summary>
/// <param name="ray">stereo ray</param>
/// <param name="origin">pose from which this observation was made</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,
particlePose origin,
stereoModel[] sensormodel,
pos3D left_camera_location,
pos3D right_camera_location,
bool localiseOnly)
{
float matchingScore = 0;
switch (grid_type)
{
case TYPE_MULTI_HYPOTHESIS:
{
for (int grid_level = 0; grid_level < grid.Length; grid_level++)
{
rayModelLookup sensormodel_lookup = sensormodel[grid_level].ray_model;
occupancygridMultiHypothesis grd = (occupancygridMultiHypothesis)grid[grid_level];
matchingScore += grid_weight[grid_level] * grd.Insert(ray, origin, sensormodel_lookup, left_camera_location, right_camera_location, localiseOnly);
}
break;
}
}
return(matchingScore);
}
/// <summary>
/// creates a lookup table for sensor models
/// </summary>
/// <param name="grid_layer">the grid upon which the rays will be drawn</param>
/// <param name="grid_dimension">size of the grid</param>
/// <param name="img">image within which to display the results</param>
/// <param name="img_width">width of the image</param>
/// <param name="img_height">height of the image</param>
/// <param name="divisor">a dividing factor used to make the width of the grid smaller than its length</param>
/// <param name="apply_smoothing">try to smooth the data to remove pixelation effects</param>
/// <param name="gridCellSize_mm">Size of each occupancy grid cell in millimetres</param>
/// <param name="mirror">apply mirroring</param>
private void updateRayModel(
float[, ,] grid_layer, int grid_dimension,
byte[] img, int img_width, int img_height,
int divisor, bool apply_smoothing,
int gridCellSize_mm, bool mirror)
{
// half a pixel of horizontal uncertainty
sigma = 1.0f / (image_width * 1) * FOV_horizontal;
//sigma *= image_width / 320;
this.divisor = divisor;
float max_disparity = (10 * image_width / 100);
ray_model_max_length = grid_dimension;
ray_model = new rayModelLookup((int)(max_disparity) + 1, ray_model_max_length);
int min_dist = (int)baseline;
int max_dist = grid_dimension;
int x = (image_width)*499/1000;
for (float disparity_pixels = 1; disparity_pixels < max_disparity; disparity_pixels += 0.5f)
{
float xx = x + disparity_pixels;
// clear the grid
max_prob = 0.0f;
for (int gx = 0; gx < grid_dimension / divisor; gx++)
for (int gy = 0; gy < grid_dimension; gy++)
{
grid_layer[gx, gy, 0] = 0;
grid_layer[gx, gy, 1] = 0;
grid_layer[gx, gy, 2] = 0;
}
float x_start = 0, y_start = 0;
float x_end = 0, y_end = 0;
float x_left = 0, y_left = 0;
float x_right = 0, y_right = 0;
float confidence = 1;
float distance = 100;
raysIntersection(xx, x, grid_dimension, confidence, distance,
ref x_start, ref y_start, ref x_end, ref y_end,
ref x_left, ref y_left, ref x_right, ref y_right);
if (x_right > x_left)
{
if (y_start < -1) y_start = -y_start;
if (y_end < -1) y_end = -y_end;
if (y_start > 0)
min_dist = (int)y_start;
else
min_dist = 100;
max_dist = grid_dimension;
if ((y_end > 0) && (y_end < grid_dimension))
max_dist = (int)y_end;
throwRay(-baseline / 2, xx, vergence_radians, min_dist, max_dist, grid_layer, grid_dimension, 1, mirror);
throwRay(baseline / 2, x, -vergence_radians, min_dist, max_dist, grid_layer, grid_dimension, 2, mirror);
int start = -1;
float min_prob = 0.0f;
int max_length = 0;
int x2 = (grid_layer.GetLength(0) / 2);
int winner = x2;
float total_probability = 0;
for (int xx2 = x2 - (grid_dimension / (divisor * 4)); xx2 <= x2 + (grid_dimension / (divisor * 4)); xx2++)
{
int length = 0;
float tot = 0;
for (int l = 0; l < ray_model_max_length; l++)
if ((grid_layer[xx2, l, 2] == 2) &&
(grid_layer[xx2, l, 0] > 1))
{
float cellval = grid_layer[xx2, l, 1];
if (cellval > min_prob)
{
length++;
tot += cellval;
}
}
if (length > max_length)
{
max_length = length;
winner = xx2;
total_probability = tot;
}
}
float scaling_factor = 1;
if (total_probability > 0)
{
if (max_length > 0) scaling_factor = 1.0f / total_probability;
// record the length of the ray model
ray_model.length[(int)(disparity_pixels * 2)] = max_length+1;
float max = 0;
int max_index = 0;
for (int l = 0; l < ray_model_max_length; l++)
{
int y2 = ray_model_max_length - 1 - l;
if ((y2 > -1) && (y2 < grid_dimension))
{
if ((grid_layer[winner, y2, 2] == 2) &&
(grid_layer[winner, y2, 1] != 0) &&
(grid_layer[winner, y2, 0] > 1))
{
float cellval = grid_layer[winner, y2, 1] * scaling_factor;
if (cellval > min_prob)
{
if (start == -1) start = l;
ray_model.probability[(int)(disparity_pixels * 2)][l - start] = cellval;
if (cellval > max)
{
max = cellval;
max_index = l - start;
}
}
}
}
}
if (apply_smoothing)
{
float[] probvalues = new float[ray_model_max_length];
int radius = 20;
for (int itt = 0; itt < 10; itt++)
{
for (int i = 0; i < ray_model_max_length; i++)
{
float value = 0;
int hits = 0;
for (int j = i - radius; j < i + radius; j++)
{
if ((j >= 0) && (j < ray_model_max_length))
{
value += ((j - (i - radius)) * ray_model.probability[(int)(disparity_pixels * 2)][j]);
hits++;
}
}
if (hits > 0) value /= hits;
probvalues[i] = value;
}
for (int i = 0; i < ray_model_max_length; i++)
if (ray_model.probability[(int)(disparity_pixels * 2)][i] > max / 200.0f)
ray_model.probability[(int)(disparity_pixels * 2)][i] = probvalues[i];
}
}
}
}
}
// compress the array down to a more managable size
compressRayModels(gridCellSize_mm);
ray_model_to_graph_image(img, img_width, img_height);
this.divisor = 1;
}
/// <summary>
/// Ok, so we've calculated the ray models in stupifying detail
/// now let's compress them down to some more compact and less memory-hogging form
/// so that each array element corresponds to a single occupancy grid cell, which
/// makes updating the grid a simple process
/// </summary>
/// <param name="gridCellSize_mm">Size of each occupancy grid cell in millimetres</param>
private void compressRayModels(int gridCellSize_mm)
{
if (ray_model != null)
{
ray_model_normal_length = ray_model.dimension_probability / gridCellSize_mm;
rayModelLookup new_ray_model = new rayModelLookup(ray_model.dimension_disparity, ray_model_normal_length);
for (int d = 1; d < ray_model.dimension_disparity; d++)
{
int prev_index = 0;
for (int i = 0; i < ray_model_normal_length; i++)
{
int next_index = (i + 1) * ray_model.dimension_probability / new_ray_model.dimension_probability;
// sum the probabilities
float total_probability = 0;
for (int j = prev_index; j < next_index; j++)
{
// is there a DJ in the house ?
total_probability += ray_model.probability[d][j];
}
if (total_probability > 0)
new_ray_model.probability[d][i] = total_probability;
prev_index = next_index;
}
new_ray_model.length[d] = ray_model.length[d] * new_ray_model.dimension_probability / ray_model.dimension_probability;
// if there's only one grid cell give it the full probability
// (it's gotta be in there somewhere!)
if (new_ray_model.length[d] == 1) new_ray_model.probability[d][0] = 1.0f;
}
// finally swap the arrays
ray_model = new_ray_model;
//ray_model_length = new_ray_model_length;
}
}
/// <summary>
/// inserts the given ray into the grid
/// There are three components to the sensor model used here:
/// two areas determining the probably vacant area and one for
/// the probably occupied space
/// </summary>
/// <param name="ray">ray object to be inserted into the grid</param>
/// <param name="origin">the pose of the robot</param>
/// <param name="sensormodel">the sensor model to be used</param>
/// <param name="leftcam_x">x position of the left camera in millimetres</param>
/// <param name="leftcam_y">y position of the left camera in millimetres</param>
/// <param name="rightcam_x">x position of the right camera in millimetres</param>
/// <param name="rightcam_y">y position of the right camera in millimetres</param>
/// <param name="localiseOnly">if true does not add any mapping particles (pure localisation)</param>
/// <returns>matching probability, expressed as log odds</returns>
public float Insert(evidenceRay ray,
particlePose origin,
rayModelLookup sensormodel_lookup,
pos3D left_camera_location,
pos3D right_camera_location,
bool localiseOnly)
{
// some constants to aid readability
const int OCCUPIED_SENSORMODEL = 0;
const int VACANT_SENSORMODEL_LEFT_CAMERA = 1;
const int VACANT_SENSORMODEL_RIGHT_CAMERA = 2;
const int X_AXIS = 0;
const int Y_AXIS = 1;
// which disparity index in the lookup table to use
// we multiply by 2 because the lookup is in half pixel steps
int sensormodel_index = (int)Math.Round(ray.disparity * 2);
// the initial models are blank, so just default to the one disparity pixel model
bool small_disparity_value = false;
if (sensormodel_index < 2)
{
sensormodel_index = 2;
small_disparity_value = true;
}
// beyond a certain disparity the ray model for occupied space
// is always only going to be only a single grid cell
if (sensormodel_index >= sensormodel_lookup.probability.GetLength(0))
sensormodel_index = sensormodel_lookup.probability.GetLength(0) - 1;
float xdist_mm=0, ydist_mm=0, zdist_mm=0, xx_mm=0, yy_mm=0, zz_mm=0;
float occupied_dx = 0, occupied_dy = 0, occupied_dz = 0;
float intersect_x = 0, intersect_y = 0, intersect_z = 0;
float centre_prob = 0, prob = 0, prob_localisation = 0; // probability values at the centre axis and outside
float matchingScore = occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE; // total localisation matching score
int rayWidth = 0; // widest point in the ray in cells
int widest_point; // widest point index
int step_size = 1;
particleGridCell hypothesis;
// ray width at the fattest point in cells
rayWidth = (int)Math.Round(ray.width / (cellSize_mm * 2));
// calculate the centre position of the grid in millimetres
int half_grid_width_mm = dimension_cells * cellSize_mm / 2;
//int half_grid_width_vertical_mm = dimension_cells_vertical * cellSize_mm / 2;
int grid_centre_x_mm = (int)(x - half_grid_width_mm);
int grid_centre_y_mm = (int)(y - half_grid_width_mm);
int grid_centre_z_mm = (int)z;
int max_dimension_cells = dimension_cells - rayWidth;
// in turbo mode only use a single vacancy ray
int max_modelcomponent = VACANT_SENSORMODEL_RIGHT_CAMERA;
if (TurboMode) max_modelcomponent = VACANT_SENSORMODEL_LEFT_CAMERA;
float[][] sensormodel_lookup_probability = sensormodel_lookup.probability;
// consider each of the three parts of the sensor model
for (int modelcomponent = OCCUPIED_SENSORMODEL; modelcomponent <= max_modelcomponent; modelcomponent++)
{
// the range from the cameras from which insertion of data begins
// for vacancy rays this will be zero, but will be non-zero for the occupancy area
int starting_range_cells = 0;
switch (modelcomponent)
{
case OCCUPIED_SENSORMODEL:
{
// distance between the beginning and end of the probably
// occupied area
occupied_dx = ray.vertices[1].x - ray.vertices[0].x;
occupied_dy = ray.vertices[1].y - ray.vertices[0].y;
occupied_dz = ray.vertices[1].z - ray.vertices[0].z;
intersect_x = ray.vertices[0].x + (occupied_dx * ray.fattestPoint);
intersect_y = ray.vertices[0].y + (occupied_dy * ray.fattestPoint);
intersect_z = ray.vertices[0].z + (occupied_dz * ray.fattestPoint);
xdist_mm = occupied_dx;
ydist_mm = occupied_dy;
zdist_mm = occupied_dz;
// begin insertion at the beginning of the
// probably occupied area
xx_mm = ray.vertices[0].x;
yy_mm = ray.vertices[0].y;
zz_mm = ray.vertices[0].z;
break;
}
case VACANT_SENSORMODEL_LEFT_CAMERA:
{
// distance between the left camera and the left side of
// the probably occupied area of the sensor model
xdist_mm = intersect_x - left_camera_location.x;
ydist_mm = intersect_y - left_camera_location.y;
zdist_mm = intersect_z - left_camera_location.z;
// begin insertion from the left camera position
xx_mm = left_camera_location.x;
yy_mm = left_camera_location.y;
zz_mm = left_camera_location.z;
step_size = 2;
break;
}
case VACANT_SENSORMODEL_RIGHT_CAMERA:
{
// distance between the right camera and the right side of
// the probably occupied area of the sensor model
xdist_mm = intersect_x - right_camera_location.x;
ydist_mm = intersect_y - right_camera_location.y;
zdist_mm = intersect_z - right_camera_location.z;
// begin insertion from the right camera position
xx_mm = right_camera_location.x;
yy_mm = right_camera_location.y;
zz_mm = right_camera_location.z;
step_size = 2;
break;
}
}
// which is the longest axis ?
int longest_axis = X_AXIS;
float longest = Math.Abs(xdist_mm);
if (Math.Abs(ydist_mm) > longest)
{
// y has the longest length
longest = Math.Abs(ydist_mm);
longest_axis = Y_AXIS;
}
// ensure that the vacancy model does not overlap
// the probably occupied area
// This is crude and could potentially leave a gap
if (modelcomponent != OCCUPIED_SENSORMODEL)
longest -= ray.width;
int steps = (int)(longest / cellSize_mm);
if (steps < 1) steps = 1;
// calculate the range from the cameras to the start of the ray in grid cells
if (modelcomponent == OCCUPIED_SENSORMODEL)
{
if (longest_axis == Y_AXIS)
starting_range_cells = (int)Math.Abs((ray.vertices[0].y - ray.observedFrom.y) / cellSize_mm);
else
starting_range_cells = (int)Math.Abs((ray.vertices[0].x - ray.observedFrom.x) / cellSize_mm);
}
// what is the widest point of the ray in cells
if (modelcomponent == OCCUPIED_SENSORMODEL)
widest_point = (int)(ray.fattestPoint * steps / ray.length);
else
widest_point = steps;
// calculate increment values in millimetres
float x_incr_mm = xdist_mm / steps;
float y_incr_mm = ydist_mm / steps;
float z_incr_mm = zdist_mm / steps;
// step through the ray, one grid cell at a time
int grid_step = 0;
while (grid_step < steps)
{
// is this position inside the maximum mapping range
bool withinMappingRange = true;
if (grid_step + starting_range_cells > max_mapping_range_cells)
{
withinMappingRange = false;
if ((grid_step==0) && (modelcomponent == OCCUPIED_SENSORMODEL))
{
grid_step = steps;
modelcomponent = 9999;
}
}
// calculate the width of the ray in cells at this point
// using a diamond shape ray model
int ray_wdth = 0;
if (rayWidth > 0)
{
if (grid_step < widest_point)
ray_wdth = grid_step * rayWidth / widest_point;
else
{
if (!small_disparity_value)
// most disparity values tail off to some point in the distance
ray_wdth = (steps - grid_step + widest_point) * rayWidth / (steps - widest_point);
else
// for very small disparity values the ray model has an infinite tail
// and maintains its width after the widest point
ray_wdth = rayWidth;
}
}
// localisation rays are wider, to enable a more effective matching score
// which is not too narrowly focussed and brittle
int ray_wdth_localisation = ray_wdth + 1; //localisation_search_cells;
xx_mm += x_incr_mm*step_size;
yy_mm += y_incr_mm*step_size;
zz_mm += z_incr_mm*step_size;
// convert the x millimetre position into a grid cell position
int x_cell = (int)Math.Round((xx_mm - grid_centre_x_mm) / (float)cellSize_mm);
if ((x_cell > ray_wdth_localisation) && (x_cell < dimension_cells - ray_wdth_localisation))
{
// convert the y millimetre position into a grid cell position
int y_cell = (int)Math.Round((yy_mm - grid_centre_y_mm) / (float)cellSize_mm);
if ((y_cell > ray_wdth_localisation) && (y_cell < dimension_cells - ray_wdth_localisation))
{
// convert the z millimetre position into a grid cell position
int z_cell = (int)Math.Round((zz_mm - grid_centre_z_mm) / (float)cellSize_mm);
if ((z_cell >= 0) && (z_cell < dimension_cells_vertical))
{
int x_cell2 = x_cell;
int y_cell2 = y_cell;
// get the probability at this point
// for the central axis of the ray using the inverse sensor model
if (modelcomponent == OCCUPIED_SENSORMODEL)
centre_prob = 0.5f + (sensormodel_lookup_probability[sensormodel_index][grid_step] * 0.5f);
else
// calculate the probability from the vacancy model
centre_prob = vacancyFunction(grid_step / (float)steps, steps);
// width of the localisation ray
for (int width = -ray_wdth_localisation; width <= ray_wdth_localisation; width++)
{
// is the width currently inside the mapping area of the ray ?
bool isInsideMappingRayWidth = false;
if ((width >= -ray_wdth) && (width <= ray_wdth))
isInsideMappingRayWidth = true;
// adjust the x or y cell position depending upon the
// deviation from the main axis of the ray
if (longest_axis == Y_AXIS)
x_cell2 = x_cell + width;
else
y_cell2 = y_cell + width;
// probability at the central axis
prob = centre_prob;
prob_localisation = centre_prob;
// probabilities are symmetrical about the axis of the ray
// this multiplier implements a gaussian distribution around the centre
if (width != 0) // don't bother if this is the centre of the ray
{
// the probability used for wide localisation rays
prob_localisation *= gaussianLookup[Math.Abs(width) * 9 / ray_wdth_localisation];
// the probability used for narrower mapping rays
if (isInsideMappingRayWidth)
prob *= gaussianLookup[Math.Abs(width) * 9 / ray_wdth];
}
if ((cell[x_cell2][y_cell2] != null) && (withinMappingRange))
{
// only localise using occupancy, not vacancy
if (modelcomponent == OCCUPIED_SENSORMODEL)
{
// update the matching score, by combining the probability
// of the grid cell with the probability from the localisation ray
float score = occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE;
if (longest_axis == X_AXIS)
score = matchingProbability(x_cell2, y_cell2, z_cell, origin, prob_localisation, ray.colour);
if (longest_axis == Y_AXIS)
score = matchingProbability(x_cell2, y_cell2, z_cell, origin, prob_localisation, ray.colour);
if (score != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
{
if (matchingScore != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
matchingScore += score;
else
matchingScore = score;
}
}
}
if ((isInsideMappingRayWidth) &&
(withinMappingRange) &&
(!localiseOnly))
{
// add a new hypothesis to this grid coordinate
// note that this is also added to the original pose
hypothesis = new particleGridCell(x_cell2, y_cell2, z_cell,
prob, origin,
ray.colour);
if (origin.AddHypothesis(hypothesis, max_mapping_range_cells, dimension_cells, dimension_cells_vertical))
{
// generate a grid cell if necessary
if (cell[x_cell2][y_cell2] == null)
cell[x_cell2][y_cell2] = new occupancygridCellMultiHypothesis(dimension_cells_vertical);
cell[x_cell2][y_cell2].AddHypothesis(hypothesis);
total_valid_hypotheses++;
}
}
}
}
else grid_step = steps; // its the end of the ray, break out of the loop
}
else grid_step = steps; // its the end of the ray, break out of the loop
}
else grid_step = steps; // time to bail out chaps!
grid_step += step_size;
}
}
return (matchingScore);
}
