public void CreateSimulatedEnvironment() { int no_of_grids = 2; int grid_type = metagrid.TYPE_SIMPLE; int dimension_mm = 8000; int room_height_mm = 2500; int cellSize_mm = 32; int localisationRadius_mm = 8000; int maxMappingRange_mm = 16000; float vacancyWeighting = 0.5f; metagrid grid = new metagrid( no_of_grids, grid_type, dimension_mm, room_height_mm, cellSize_mm, localisationRadius_mm, maxMappingRange_mm, vacancyWeighting); int room_tx_mm = -(1050/2); int room_ty_mm = -(dimension_mm/2) * 80/100; int room_bx_mm = 1050/2; int room_by_mm = room_ty_mm + 6000; int doorway_height_mm = 2000; int doorway_width_mm = 740; int wall_thickness_mm = 100; List<int> left_wall_doorways = new List<int>(); List<int> top_wall_doorways = new List<int>(); List<int> right_wall_doorways = new List<int>(); List<int> bottom_wall_doorways = new List<int>(); int floor_r = 0; int floor_g = 0; int floor_b = 0; int walls_r = 0; int walls_g = 0; int walls_b = 255; float probability_variance = 0.1f; left_wall_doorways.Add(900); left_wall_doorways.Add(4000); left_wall_doorways.Add(5200); right_wall_doorways.Add(450); right_wall_doorways.Add(4000); grid.InsertRoom( room_tx_mm, room_ty_mm, room_bx_mm, room_by_mm, room_height_mm, wall_thickness_mm, probability_variance, floor_r, floor_g, floor_b, walls_r, walls_g, walls_b, left_wall_doorways, top_wall_doorways, right_wall_doorways, bottom_wall_doorways, doorway_width_mm, doorway_height_mm); int debug_img_width = 640; int debug_img_height = 480; byte[] debug_img = new byte[debug_img_width * debug_img_height * 3]; Bitmap bmp = new Bitmap(debug_img_width, debug_img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb); bool show_localisation = false; for (int i = 0; i < no_of_grids; i++) { grid.Show(i, debug_img, debug_img_width, debug_img_height, false, show_localisation); BitmapArrayConversions.updatebitmap_unsafe(debug_img, bmp); bmp.Save("tests_occupancygrid_meta_CreateSimulatedEnvironment_grid" + i.ToString() + ".bmp", System.Drawing.Imaging.ImageFormat.Bmp); } grid.ExportToIFrIT("tests_occupancygrid_meta_CreateSimulatedEnvironment.txt"); }
/// <summary> /// initialise the buffer /// </summary> /// <param name="no_of_grid_levels">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 void Initialise( int no_of_grid_levels, int grid_type, int dimension_mm, int dimension_vertical_mm, int cellSize_mm, int localisationRadius_mm, int maxMappingRange_mm, float vacancyWeighting) { this.dimension_mm = dimension_mm; this.dimension_vertical_mm = dimension_vertical_mm; this.no_of_grid_levels = no_of_grid_levels; this.grid_type = grid_type; this.cellSize_mm = cellSize_mm; this.localisationRadius_mm = localisationRadius_mm; this.maxMappingRange_mm = maxMappingRange_mm; this.vacancyWeighting = vacancyWeighting; // create the buffer buffer = new metagrid[2]; for (int i = 0; i < 2; i++) { buffer[i] = new metagrid( no_of_grid_levels, grid_type, dimension_mm, dimension_vertical_mm, cellSize_mm, localisationRadius_mm, maxMappingRange_mm, vacancyWeighting); } current_buffer_index = 0; current_grid_index = 0; grid_centres = new List<float>(); localisations = new List<float>(); disparities_index = new List<int>(); }
/// <summary> /// moves to the next grid in the sequence, if necessary /// </summary> /// <param name="current_grid_index">index of the current local grid</param> /// <param name="current_disparity_index">index of teh current disparities set within the disparities file</param> /// <param name="robot_pose">the current robot pose</param> /// <param name="buffer">buffer containing two metagrids</param> /// <param name="current_buffer_index">index of the currently active grid within the buffer (0 or 1)</param> /// <param name="grid_centres">list of grid centre positions (x,y,z)</param> /// <param name="update_map">returns whether the map should be updated</param> /// <param name="debug_mapping_filename">filename to save debugging images</param> /// <param name="overall_map_filename">filename to save an overall map of the path</param> /// <param name="overall_map_img">overall map image data</param> /// <param name="overall_map_dimension_mm">dimension of the overall map in millimetres</param> /// <param name="overall_map_centre_x_mm">centre x position of the overall map in millimetres</param> /// <param name="overall_map_centre_y_mm">centre x position of the overall map in millimetres</param> /// <returns>true if we have transitioned from one grid to the next</returns> public static bool MoveToNextLocalGrid( ref int current_grid_index, ref int current_disparity_index, pos3D robot_pose, metagrid[] buffer, ref int current_buffer_index, List<float> grid_centres, ref bool update_map, string debug_mapping_filename, string overall_map_filename, ref byte[] overall_map_img, int overall_img_width, int overall_img_height, int overall_map_dimension_mm, int overall_map_centre_x_mm, int overall_map_centre_y_mm) { bool buffer_transition = false; update_map = false; // if this is the first time that localisation // has been called since loading the path // then update the map if ((current_grid_index == 0) && (current_disparity_index == 0)) { update_map = true; float grid_centre_x_mm = grid_centres[current_grid_index * 3]; float grid_centre_y_mm = grid_centres[(current_grid_index * 3) + 1]; float grid_centre_z_mm = grid_centres[(current_grid_index * 3) + 2]; buffer[current_buffer_index].SetPosition(grid_centre_x_mm, grid_centre_y_mm, grid_centre_z_mm, 0); int next_grid_index = current_grid_index + 1; if (next_grid_index >= grid_centres.Count / 3) next_grid_index = current_grid_index; grid_centre_x_mm = grid_centres[next_grid_index * 3]; grid_centre_y_mm = grid_centres[(next_grid_index * 3) + 1]; grid_centre_z_mm = grid_centres[(next_grid_index * 3) + 2]; buffer[1 - current_buffer_index].SetPosition(grid_centre_x_mm, grid_centre_y_mm, grid_centre_z_mm, 0); } // distance to the centre of the currently active grid float dx = robot_pose.x - buffer[current_buffer_index].x; float dy = robot_pose.y - buffer[current_buffer_index].y; float dz = robot_pose.z - buffer[current_buffer_index].z; float dist_to_grid_centre_sqr_0 = dx*dx + dy*dy + dz*dz; dx = robot_pose.x - buffer[1 - current_buffer_index].x; dy = robot_pose.y - buffer[1 - current_buffer_index].y; dz = robot_pose.z - buffer[1 - current_buffer_index].z; float dist_to_grid_centre_sqr_1 = dx*dx + dy*dy + dz*dz; // if we are closer to the next grid than the current one // then swap the currently active grid //if (dist_to_grid_centre_sqr_0 > dimension_mm/2) if (dist_to_grid_centre_sqr_1 < dist_to_grid_centre_sqr_0) { if ((debug_mapping_filename != null) && (debug_mapping_filename != "")) { bool show_localisation = true; int debug_img_width = 640; int debug_img_height = 480; byte[] debug_img = new byte[debug_img_width * debug_img_height * 3]; Bitmap debug_bmp = new Bitmap(debug_img_width, debug_img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb); buffer[current_buffer_index].Show(0, debug_img, debug_img_width, debug_img_height, false, show_localisation); BitmapArrayConversions.updatebitmap_unsafe(debug_img, debug_bmp); if (debug_mapping_filename.ToLower().EndsWith("png")) debug_bmp.Save(debug_mapping_filename, System.Drawing.Imaging.ImageFormat.Png); if (debug_mapping_filename.ToLower().EndsWith("gif")) debug_bmp.Save(debug_mapping_filename, System.Drawing.Imaging.ImageFormat.Gif); if (debug_mapping_filename.ToLower().EndsWith("jpg")) debug_bmp.Save(debug_mapping_filename, System.Drawing.Imaging.ImageFormat.Jpeg); if (debug_mapping_filename.ToLower().EndsWith("bmp")) debug_bmp.Save(debug_mapping_filename, System.Drawing.Imaging.ImageFormat.Bmp); /* string[] str = debug_mapping_filename.Split('.'); string debug_mapping_filename2 = str[0] + "b." + str[1]; buffer[1 - current_buffer_index].Show(0, debug_img, debug_img_width, debug_img_height, false); BitmapArrayConversions.updatebitmap_unsafe(debug_img, debug_bmp); if (debug_mapping_filename2.ToLower().EndsWith("png")) debug_bmp.Save(debug_mapping_filename2, System.Drawing.Imaging.ImageFormat.Png); if (debug_mapping_filename2.ToLower().EndsWith("gif")) debug_bmp.Save(debug_mapping_filename2, System.Drawing.Imaging.ImageFormat.Gif); if (debug_mapping_filename2.ToLower().EndsWith("jpg")) debug_bmp.Save(debug_mapping_filename2, System.Drawing.Imaging.ImageFormat.Jpeg); if (debug_mapping_filename2.ToLower().EndsWith("bmp")) debug_bmp.Save(debug_mapping_filename2, System.Drawing.Imaging.ImageFormat.Bmp); */ } UpdateOverallMap( buffer, current_buffer_index, overall_map_filename, ref overall_map_img, overall_img_width, overall_img_height, overall_map_dimension_mm, overall_map_centre_x_mm, overall_map_centre_y_mm); // swap the two metagrids SwapBuffers( grid_centres, ref current_grid_index, buffer, ref current_buffer_index, ref update_map); buffer_transition = true; } return(buffer_transition); }
/// <summary> /// updates the image of the overall map /// </summary> /// <param name="overall_map_filename">filename to save an overall map of the path</param> /// <param name="overall_map_img">overall map image data</param> /// <param name="overall_map_dimension_mm">dimension of the overall map in millimetres</param> /// <param name="overall_map_centre_x_mm">centre x position of the overall map in millimetres</param> /// <param name="overall_map_centre_y_mm">centre x position of the overall map in millimetres</param> public static void UpdateOverallMap( metagrid[] buffer, int current_buffer_index, string overall_map_filename, ref byte[] overall_map_img, int overall_img_width, int overall_img_height, int overall_map_dimension_mm, int overall_map_centre_x_mm, int overall_map_centre_y_mm) { if ((overall_map_filename != null) && (overall_map_filename != "")) { bool show_localisation = false; int overall_map_dimension2_mm = overall_map_dimension_mm * overall_img_height / overall_img_width; bool clear_image = false; if (overall_map_img == null) { overall_map_img = new byte[overall_img_width * overall_img_height * 3]; clear_image = true; } Bitmap overall_bmp = new Bitmap(overall_img_width, overall_img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb); buffer[current_buffer_index].Show(0, overall_map_img, overall_img_width, overall_img_height, false, overall_map_dimension_mm, overall_map_dimension2_mm, overall_map_centre_x_mm, overall_map_centre_y_mm, clear_image, show_localisation); BitmapArrayConversions.updatebitmap_unsafe(overall_map_img, overall_bmp); if (overall_map_filename.ToLower().EndsWith("png")) overall_bmp.Save(overall_map_filename, System.Drawing.Imaging.ImageFormat.Png); if (overall_map_filename.ToLower().EndsWith("gif")) overall_bmp.Save(overall_map_filename, System.Drawing.Imaging.ImageFormat.Gif); if (overall_map_filename.ToLower().EndsWith("jpg")) overall_bmp.Save(overall_map_filename, System.Drawing.Imaging.ImageFormat.Jpeg); if (overall_map_filename.ToLower().EndsWith("bmp")) overall_bmp.Save(overall_map_filename, System.Drawing.Imaging.ImageFormat.Bmp); } }
/// <summary> /// swaps the two metagrids, clears the previous grid and assigns it the next centre position /// </summary> /// <param name="grid_centres">list of grid centre positions (x,y,z)</param> /// <param name="current_grid_index">index of the current grid which we are in</param> /// <param name="buffer">grid buffer, consisting of two metagrids</param> /// <param name="current_buffer_index">index of the currently active buffer - i.e. which of the two we are currently in</param> /// <param name="update_map">returns whether or not to update with new mapping rays</param> public static void SwapBuffers( List<float> grid_centres, ref int current_grid_index, metagrid[] buffer, ref int current_buffer_index, ref bool update_map) { int curr_buffer = 1 - current_buffer_index; int next_buffer = current_buffer_index; int no_of_grids = grid_centres.Count / 3; current_buffer_index = curr_buffer; if (current_grid_index < no_of_grids) { // move into the next grid current_grid_index++; if (current_grid_index < no_of_grids - 1) { // clear the grid which we have just passed through buffer[next_buffer].Clear(); // update the next map update_map = true; // retrieve the next grid centre position int next_grid_index = current_grid_index + 1; float next_grid_centre_x = grid_centres[(next_grid_index * 3)]; float next_grid_centre_y = grid_centres[(next_grid_index * 3) + 1]; float next_grid_centre_z = grid_centres[(next_grid_index * 3) + 2]; // set the next grid centre buffer[next_buffer].SetPosition(next_grid_centre_x, next_grid_centre_y, next_grid_centre_z, 0.0f); } } }
public void MoveToNextLocalGrid() { int no_of_grids = 2; int grid_type = metagrid.TYPE_SIMPLE; int dimension_mm = 3000; int dimension_vertical_mm = 2000; int cellSize_mm = 32; int localisationRadius_mm = 2000; int maxMappingRange_mm = 2000; float vacancyWeighting = 0.5f; int overall_img_width = 640; int overall_img_height = 480; int current_grid_index = 0; int current_disparity_index = 0; pos3D robot_pose = new pos3D(0,0,0); metagrid[] buffer = new metagrid[2]; int current_buffer_index = 0; List<float> grid_centres = new List<float>(); bool update_map = false; float grid_centre_x_mm; float grid_centre_y_mm; float grid_centre_z_mm; // create some grid centres along a straight line path float path_length_mm = 10000; int steps = (int)(path_length_mm / (dimension_mm/2)); for (int i = 0; i < steps; i++) { grid_centre_x_mm = 0; grid_centre_y_mm = i * path_length_mm / steps; grid_centre_z_mm = 0; grid_centres.Add(grid_centre_x_mm); grid_centres.Add(grid_centre_y_mm); grid_centres.Add(grid_centre_z_mm); } // create the buffer for (int i = 0; i < 2; i++) { buffer[i] = new metagrid( no_of_grids, grid_type, dimension_mm, dimension_vertical_mm, cellSize_mm, localisationRadius_mm, maxMappingRange_mm, vacancyWeighting); } // move along a straight line path int transitions = 0; for (int y = 0; y < path_length_mm; y += 100) { robot_pose.y = y; byte[] overall_map_img = null; if (metagridBuffer.MoveToNextLocalGrid( ref current_grid_index, ref current_disparity_index, robot_pose, buffer, ref current_buffer_index, grid_centres, ref update_map, null, null, ref overall_map_img, overall_img_width, overall_img_height, 0,0,0)); { transitions++; } current_disparity_index = 1; } Assert.AreEqual(steps - 1, transitions, "Incorrect number of local grid transitions"); Assert.AreEqual(steps - 1, current_grid_index, "Did not reach the final grid"); }