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");
}
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>
/// 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;
}
}
}
/// <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);
}
/// <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>
/// create a path planner for the given grid
/// </summary>
/// <param name="grid">occupancy grid</param>
private void createPlanner(occupancygridMultiHypothesis grid)
{
if (planner == null)
planner = new sentience.pathplanner.pathplanner(grid.navigable_space, (int)LocalGridCellSize_mm, grid.x, grid.y);
else
planner.navigable_space = grid.navigable_space;
}
/// <summary>
/// recreate the local grid
/// </summary>
/// <param name="index">index number of the occupancy grid being considered</param>
private void createLocalGrid(int index)
{
// create the local grid
LocalGrid[index] = new occupancygridMultiHypothesis(LocalGridDimension, LocalGridDimensionVertical, (int)LocalGridCellSize_mm, (int)LocalGridLocalisationRadius_mm, (int)LocalGridMappingRange_mm, LocalGridVacancyWeighting);
//createPlanner(LocalGrid[index]);
}
/// <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>
/// 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);
}
