/// <summary>
/// remove all poses along the path until a branch point is located
/// </summary>
/// <param name="pose"></param>
/// <returns></returns>
private particlePose CollapseBranch(particlePose pose,
occupancygridMultiHypothesis grid)
{
int children = 0;
while ((pose != branch_pose) &&
(pose.path == this) &&
(children == 0))
{
if (pose != current_pose)
{
children = pose.no_of_children;
}
if (children == 0)
{
if (pose.path == this)
{
pose.Remove(grid);
pose = pose.parent;
}
}
}
return(pose);
}
/// <summary>
/// remove the mapping particles associated with this path
/// </summary>
public bool Remove(occupancygridMultiHypothesis grid)
{
Enabled = false;
particlePose pose = current_pose;
if (current_pose != null)
{
// collapse this path down to the next branching point
pose = CollapseBranch(pose, grid);
if (pose != null)
{
if (pose != current_pose)
{
if (pose.no_of_children > 0)
{
// reduce the number of children at the branch point
pose.no_of_children--;
}
}
// keep on truckin' down the line...
incrementPathChildren(pose.path, -1);
if (pose.path.total_children == 0)
{
pose.path.Remove(grid);
}
}
}
return(!Enabled);
}
public motionModel(robot rob,
occupancygridMultiHypothesis LocalGrid,
int random_seed)
{
// seed the random number generator
rnd = new Random(random_seed); // MersenneTwister(random_seed);
this.rob = rob;
this.LocalGrid = LocalGrid;
Poses = new List <particlePath>();
ActivePoses = new List <particlePath>();
motion_noise = new float[6];
// some default noise values
int i = 0;
while (i < 2)
{
motion_noise[i] = default_motion_noise_1;
i++;
}
while (i < 4)
{
motion_noise[i] = default_motion_noise_2;
i++;
}
while (i < motion_noise.Length)
{
motion_noise[i] = default_motion_noise_3;
i++;
}
// create some initial poses
createNewPoses(rob.x, rob.y, rob.pan);
}
/// <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="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>
/// probes the grid using the given 3D line and returns the distance to the nearest occupied grid cell
/// </summary>
/// <param name="grid_index">index number of the grid to be probed</param>
/// <param name="x0_mm">start x coordinate</param>
/// <param name="y0_mm">start y coordinate</param>
/// <param name="z0_mm">start z coordinate</param>
/// <param name="x1_mm">end x coordinate</param>
/// <param name="y1_mm">end y coordinate</param>
/// <param name="z1_mm">end z coordinate</param>
/// <returns>range to the nearest occupied grid cell in millimetres</returns>
public float ProbeRange(
int grid_index,
float x0_mm,
float y0_mm,
float z0_mm,
float x1_mm,
float y1_mm,
float z1_mm)
{
float range_mm = -1;
switch (grid_type)
{
case TYPE_SIMPLE:
{
range_mm = grid[grid_index].ProbeRange(x0_mm, y0_mm, z0_mm, x1_mm, y1_mm, z1_mm);
break;
}
case TYPE_MULTI_HYPOTHESIS:
{
occupancygridMultiHypothesis grd = (occupancygridMultiHypothesis)grid[grid_index];
// TODO get the best pose estimate
particlePose pose = null;
range_mm = grd.ProbeRange(x0_mm, y0_mm, z0_mm, x1_mm, y1_mm, z1_mm);
break;
}
}
return(range_mm);
}
/// <summary>
/// distills all grid particles associated with this pose
/// </summary>
/// <param name="grid">grid to be updated</param>
public void Distill(occupancygridMultiHypothesis grid)
{
for (int i = observed_grid_cells.Count - 1; i >= 0; i--)
{
particleGridCell hypothesis = observed_grid_cells[i];
grid.Distill(hypothesis);
}
}
/// <summary>
/// displays the occupancy grid
/// </summary>
private void showOccupancyGrid()
{
occupancygridMultiHypothesis grid = sim.rob.GetBestGrid();
picGridMap.Image = new Bitmap(grid.dimension_cells, grid.dimension_cells,
System.Drawing.Imaging.PixelFormat.Format24bppRgb);
Byte[] grid_img = new Byte[grid.dimension_cells * grid.dimension_cells * 3];
sim.ShowGrid(occupancygridMultiHypothesis.VIEW_ABOVE, grid_img, grid.dimension_cells, grid.dimension_cells, true);
BitmapArrayConversions.updatebitmap_unsafe(grid_img, (Bitmap)(picGridMap.Image));
}
/// <summary>
/// distill all grid particles within this path
/// </summary>
/// <param name="grid"></param>
public void Distill(occupancygridMultiHypothesis grid)
{
particlePose pose = current_pose;
while (pose != null)
{
pose.Distill(grid);
pose = pose.parent;
}
map_cache = null;
Enabled = false;
}
/// <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;
occupancygridMultiHypothesis grid =
new occupancygridMultiHypothesis(dimension_cells,
dimension_cells_vertical,
cellSize_mm,
localisationRadius_mm,
maxMappingRange_mm,
vacancyWeighting);
Assert.AreNotEqual(grid, null, "object occupancygridMultiHypothesis was not created");
}
/// <summary>
/// add an observation taken from this pose
/// </summary>
/// <param name="stereo_rays">list of ray objects in this observation</param>
/// <param name="rob">robot object</param>
/// <param name="LocalGrid">occupancy grid into which to insert the observation</param>
/// <param name="localiseOnly">if true does not add any mapping particles (pure localisation)</param>
/// <returns>localisation matching score</returns>
public float AddObservation(List <evidenceRay>[] stereo_rays,
robot rob,
occupancygridMultiHypothesis LocalGrid,
bool localiseOnly)
{
// clear the localisation score
float localisation_score = occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE;
// get the positions of the head and cameras for this pose
pos3D head_location = new pos3D(0, 0, 0);
pos3D[] camera_centre_location = new pos3D[rob.head.no_of_stereo_cameras];
pos3D[] left_camera_location = new pos3D[rob.head.no_of_stereo_cameras];
pos3D[] right_camera_location = new pos3D[rob.head.no_of_stereo_cameras];
calculateCameraPositions(rob, ref head_location,
ref camera_centre_location,
ref left_camera_location,
ref right_camera_location);
// itterate for each stereo camera
int cam = stereo_rays.Length - 1;
while (cam >= 0)
{
// itterate through each ray
int r = stereo_rays[cam].Count - 1;
while (r >= 0)
{
// observed ray. Note that this is in an egocentric
// coordinate frame relative to the head of the robot
evidenceRay ray = stereo_rays[cam][r];
// translate and rotate this ray appropriately for the pose
evidenceRay trial_ray = ray.trialPose(camera_centre_location[cam].pan,
camera_centre_location[cam].x,
camera_centre_location[cam].y);
// update the grid cells for this ray and update the
// localisation score accordingly
float score =
LocalGrid.Insert(trial_ray, this,
rob.head.sensormodel[cam],
left_camera_location[cam],
right_camera_location[cam],
localiseOnly);
if (score != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
{
if (localisation_score != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
{
localisation_score += score;
}
else
{
localisation_score = score;
}
}
r--;
}
cam--;
}
return(localisation_score);
}
