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