/// <summary> /// remove the mapping particles associated with this path /// </summary> public bool Remove(dpslam 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> /// remove all poses along the path until a branch point is located /// </summary> /// <param name="pose"></param> /// <returns></returns> private particlePose CollapseBranch( particlePose pose, dpslam 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> /// distills all grid particles associated with this pose /// </summary> /// <param name="grid">grid to be updated</param> public void Distill(dpslam grid) { for (int i = observed_grid_cells.Count - 1; i >= 0; i--) { particleGridCell hypothesis = observed_grid_cells[i]; grid.Distill(hypothesis); } }
/// <summary> /// distill all grid particles within this path /// </summary> /// <param name="grid"></param> public void Distill(dpslam grid) { particlePose pose = current_pose; while (pose != null) { pose.Distill(grid); pose = pose.parent; } map_cache = null; Enabled = false; }
public motionModel( robot rob, dpslam LocalGrid, int random_seed) { // seed the random number generator //rnd = new Random(random_seed); rnd = new 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.z, rob.pan, rob.tilt, rob.roll); }
/// <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, dpslam 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].tilt, camera_centre_location[cam].roll, camera_centre_location[cam].x, camera_centre_location[cam].y, camera_centre_location[cam].z); //Console.WriteLine("ray.vert[0] " + (trial_ray.vertices[0] == null).ToString()); //Console.WriteLine("ray.vert[1] " + (trial_ray.vertices[1] == null).ToString()); // 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); }
public void ProbeView() { int map_dim = 14; byte[] map = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; dpslam sim = CreateSimulation(map_dim, map, 50); int image_width = 320; int image_height = 240; float FOV_degrees = 90; float max_range_mm = 2000; pos3D camPose = new pos3D(0, 0, 0); float x0_mm = 0; float y0_mm = 0; float z0_mm = 100; float x1_mm = 0; float y1_mm = 1500; float z1_mm = -300; bool use_ground_plane = true; float range = sim.ProbeRange( null, x0_mm, y0_mm, z0_mm, x1_mm, y1_mm, z1_mm, use_ground_plane); Assert.IsTrue(range > -1, "Ground plane was not detected"); Assert.IsTrue(range > 550); Assert.IsTrue(range < 650); int step_size = 10; float[] range_buffer = new float[(image_width / step_size) * (image_height / step_size)]; sim.ProbeView( null, camPose.x, camPose.y, camPose.z, camPose.pan, camPose.tilt, camPose.roll, FOV_degrees, image_width, image_height, step_size, max_range_mm, false, range_buffer); int ctr = 0; for (int i = 0; i < range_buffer.Length; i++) { if (range_buffer[i] > -1) { ctr++; } //Console.WriteLine("Range: " + range_buffer[i].ToString()); } Assert.IsTrue(ctr > 0, "No objects were ranged within the simulation"); }
//[Test()] public static void CreateSim() { int dimension_cells = 100; int dimension_cells_vertical = 20; int cellSize_mm = 50; int localisationRadius_mm = 2000; int maxMappingRange_mm = 2000; float vacancyWeighting = 0.8f; int map_dim = 14; byte[] map = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; dpslam sim = CreateSimulation(map_dim, map, 50); particlePose pose = null; int img_width = 640; byte[] img = new byte[img_width * img_width * 3]; sim.Show(0, img, img_width, img_width, pose, true, true); Bitmap bmp = new Bitmap(img_width, img_width, System.Drawing.Imaging.PixelFormat.Format24bppRgb); BitmapArrayConversions.updatebitmap_unsafe(img, bmp); bmp.Save("dpslam_tests_CreateSimulation1.bmp", System.Drawing.Imaging.ImageFormat.Bmp); robot rob = new robot(1); rob.WheelBase_mm = 90; rob.WheelDiameter_mm = 30; rob.x = 0; rob.y = 0; rob.pan = 90 * (float)Math.PI / 180.0f; rob.head.cameraFOVdegrees[0] = 90; rob.head.cameraSensorSizeMm[0] = 4.17f; rob.head.cameraFocalLengthMm[0] = 3.6f; rob.head.cameraImageWidth[0] = 320; rob.head.cameraImageHeight[0] = 240; rayModelLookup sensor_model = new rayModelLookup(10, 10); sensor_model.InitSurveyorSVS(); rob.head.sensormodel[0] = sensor_model; float time_elapsed_sec = 1; float forward_velocity = 50; float angular_velocity_pan = 0; float angular_velocity_tilt = 0; float angular_velocity_roll = 0; float min_x_mm = -((dimension_cells / 2) * cellSize_mm) / 3; float min_y_mm = -((dimension_cells / 2) * cellSize_mm) / 3; float max_x_mm = -min_x_mm; float max_y_mm = -min_y_mm; for (float t = 0.0f; t < 4; t += time_elapsed_sec) { rob.updateFromVelocities(sim, forward_velocity, angular_velocity_pan, angular_velocity_tilt, angular_velocity_roll, time_elapsed_sec); Console.WriteLine("xy: " + rob.x.ToString() + " " + rob.y.ToString()); rob.motion.Show(img, img_width, img_width, min_x_mm, min_y_mm, max_x_mm, max_y_mm, true, false, t == 0.0f); } rob.SaveGridImage("dpslam_tests_CreateSimulation2.bmp"); BitmapArrayConversions.updatebitmap_unsafe(img, bmp); bmp.Save("dpslam_tests_CreateSimulation3.bmp", System.Drawing.Imaging.ImageFormat.Bmp); }
private static dpslam CreateSimulation( int map_dim, byte[] map, int cellSize_mm) { int dimension_cells = 100; int dimension_cells_vertical = 20; int localisationRadius_mm = 2000; int maxMappingRange_mm = 2000; float vacancyWeighting = 0.8f; dpslam sim = new dpslam( dimension_cells, dimension_cells_vertical, cellSize_mm, localisationRadius_mm, maxMappingRange_mm, vacancyWeighting); Assert.IsNotNull(sim); int world_tx = -cellSize_mm * dimension_cells / 2; int world_ty = -cellSize_mm * dimension_cells / 2; //int world_bx = cellSize_mm * dimension_cells / 2; //int world_by = cellSize_mm * dimension_cells / 2; for (int y = 0; y < map_dim; y++) { int start_x = 0; bool wall = false; for (int x = 0; x < map_dim; x++) { if ((!wall) && (map[y * map_dim + x] == 1) && (map[y * map_dim + x + 1] == 1)) { start_x = x; wall = true; } if ((wall) && (map[y * map_dim + x] == 0)) { int tx = (int)(world_tx + ((start_x + 0.5f) * dimension_cells / map_dim * cellSize_mm)); int ty = (int)(world_ty + ((y + 0.5f) * dimension_cells / map_dim * cellSize_mm)); int bx = (int)(world_tx + ((x - 0.5f) * dimension_cells / map_dim * cellSize_mm)); int by = ty; sim.InsertWall(tx, ty, bx, by, (int)cellSize_mm * 10, (int)cellSize_mm, 0.2f, 0, 0, 0); wall = false; } } } for (int x = 0; x < map_dim; x++) { int start_y = 0; bool wall = false; for (int y = 0; y < map_dim; y++) { if ((!wall) && (map[y * map_dim + x] == 1) && (map[(y + 1) * map_dim + x] == 1)) { start_y = y; wall = true; } if ((wall) && (map[y * map_dim + x] == 0)) { int tx = (int)(world_tx + ((x + 0.5f) * dimension_cells / map_dim * cellSize_mm)); int ty = (int)(world_ty + ((start_y + 0.5f) * dimension_cells / map_dim * cellSize_mm)); int bx = tx; int by = (int)(world_ty + ((y - 0.5f) * dimension_cells / map_dim * cellSize_mm)); sim.InsertWall(tx, ty, bx, by, cellSize_mm * 10, cellSize_mm, 0.2f, 0, 0, 0); wall = false; } } } return(sim); }