public steersman( int body_width_mm, int body_length_mm, int body_height_mm, int centre_of_rotation_x, int centre_of_rotation_y, int centre_of_rotation_z, int head_centroid_x, int head_centroid_y, int head_centroid_z, string sensormodels_filename, int no_of_stereo_cameras, float baseline_mm, float dist_from_centre_of_tilt_mm, int image_width, int image_height, float FOV_degrees, float head_diameter_mm, float default_head_orientation_degrees, int no_of_grid_levels, int dimension_mm, int dimension_vertical_mm, int cellSize_mm) { rnd = new Random(0); int grid_type = metagrid.TYPE_SIMPLE; int localisationRadius_mm = dimension_mm * 50 / 100; int maxMappingRange_mm = dimension_mm * 50 / 100; float vacancyWeighting = 0.5f; buffer = new metagridBuffer( no_of_grid_levels, grid_type, dimension_mm, dimension_vertical_mm, cellSize_mm, localisationRadius_mm, maxMappingRange_mm, vacancyWeighting); robot_geometry = new robotGeometry(); robot_geometry.SetBodyDimensions(body_width_mm, body_length_mm, body_height_mm); robot_geometry.SetCentreOfRotation(centre_of_rotation_x, centre_of_rotation_y, centre_of_rotation_z); robot_geometry.SetHeadPosition(head_centroid_x, head_centroid_y, head_centroid_z); robot_geometry.CreateStereoCameras( no_of_stereo_cameras, baseline_mm, dist_from_centre_of_tilt_mm, image_width, image_height, FOV_degrees, head_diameter_mm, default_head_orientation_degrees); robot_geometry.CreateSensorModels(buffer); }
public void SaveAndLoad() { string filename = "tests_robotGeometry_SaveAndLoad.xml"; int no_of_stereo_cameras = 2; robotGeometry geom1 = new robotGeometry(); geom1.CreateStereoCameras(no_of_stereo_cameras, 120, 0, 320, 240, 78, 100, 0); geom1.SetBodyDimensions(800,700,600); geom1.SetCentreOfRotation(400, 350, 10); geom1.SetHeadPosition(400,350, 1000); geom1.Save(filename); robotGeometry geom2 = new robotGeometry(); geom2.Load(filename); Assert.AreEqual(geom1.body_width_mm, geom2.body_width_mm); Assert.AreEqual(geom1.body_length_mm, geom2.body_length_mm); Assert.AreEqual(geom1.body_height_mm, geom2.body_height_mm); Assert.AreEqual(geom1.body_centre_of_rotation_x, geom2.body_centre_of_rotation_x); Assert.AreEqual(geom1.body_centre_of_rotation_y, geom2.body_centre_of_rotation_y); Assert.AreEqual(geom1.body_centre_of_rotation_z, geom2.body_centre_of_rotation_z); Assert.AreEqual(geom1.head_centroid_x, geom2.head_centroid_x); Assert.AreEqual(geom1.head_centroid_y, geom2.head_centroid_y); Assert.AreEqual(geom1.head_centroid_z, geom2.head_centroid_z); for (int cam = 0; cam < no_of_stereo_cameras; cam++) { Assert.AreEqual(geom1.baseline_mm[cam], geom2.baseline_mm[cam]); Assert.AreEqual(geom1.image_width[cam], geom2.image_width[cam]); Assert.AreEqual(geom1.image_height[cam], geom2.image_height[cam]); Assert.AreEqual(geom1.FOV_degrees[cam], geom2.FOV_degrees[cam]); Assert.AreEqual(geom1.stereo_camera_position_x[cam], geom2.stereo_camera_position_x[cam]); Assert.AreEqual(geom1.stereo_camera_position_y[cam], geom2.stereo_camera_position_y[cam]); Assert.AreEqual(geom1.stereo_camera_position_z[cam], geom2.stereo_camera_position_z[cam]); Assert.AreEqual(geom1.stereo_camera_pan[cam], geom2.stereo_camera_pan[cam]); Assert.AreEqual(geom1.stereo_camera_tilt[cam], geom2.stereo_camera_tilt[cam]); Assert.AreEqual(geom1.stereo_camera_roll[cam], geom2.stereo_camera_roll[cam]); } }
public float Localise( robotGeometry geom, float[][] stereo_features, byte[][,] stereo_features_colour, float[][] stereo_features_uncertainties, Random rnd, ref pos3D pose_offset, ref bool buffer_transition, string debug_mapping_filename, float known_best_pose_x_mm, float known_best_pose_y_mm, 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) { return(Localise( geom.body_width_mm, geom.body_length_mm, geom.body_centre_of_rotation_x, geom.body_centre_of_rotation_y, geom.body_centre_of_rotation_z, geom.head_centroid_x, geom.head_centroid_y, geom.head_centroid_z, geom.head_pan, geom.head_tilt, geom.head_roll, geom.baseline_mm, geom.stereo_camera_position_x, geom.stereo_camera_position_y, geom.stereo_camera_position_z, geom.stereo_camera_pan, geom.stereo_camera_tilt, geom.stereo_camera_roll, geom.image_width, geom.image_height, geom.FOV_degrees, stereo_features, stereo_features_colour, stereo_features_uncertainties, geom.sensormodel, ref geom.left_camera_location, ref geom.right_camera_location, geom.no_of_sample_poses, geom.sampling_radius_major_mm, geom.sampling_radius_minor_mm, geom.pose, geom.max_orientation_variance, geom.max_tilt_variance, geom.max_roll_variance, geom.poses, geom.pose_probability, rnd, ref pose_offset, ref buffer_transition, debug_mapping_filename, known_best_pose_x_mm, known_best_pose_y_mm, 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 )); }
public steersman( int body_width_mm, int body_length_mm, int body_height_mm, int centre_of_rotation_x, int centre_of_rotation_y, int centre_of_rotation_z, int head_centroid_x, int head_centroid_y, int head_centroid_z, string sensormodels_filename, int no_of_stereo_cameras, float baseline_mm, float dist_from_centre_of_tilt_mm, int image_width, int image_height, float FOV_degrees, float head_diameter_mm, float default_head_orientation_degrees, int no_of_grid_levels, int dimension_mm, int dimension_vertical_mm, int cellSize_mm) { rnd = new Random(0); int grid_type = metagrid.TYPE_SIMPLE; int localisationRadius_mm = dimension_mm * 50/100; int maxMappingRange_mm = dimension_mm * 50/100; float vacancyWeighting = 0.5f; buffer = new metagridBuffer( no_of_grid_levels, grid_type, dimension_mm, dimension_vertical_mm, cellSize_mm, localisationRadius_mm, maxMappingRange_mm, vacancyWeighting); robot_geometry = new robotGeometry(); robot_geometry.SetBodyDimensions(body_width_mm, body_length_mm, body_height_mm); robot_geometry.SetCentreOfRotation(centre_of_rotation_x, centre_of_rotation_y, centre_of_rotation_z); robot_geometry.SetHeadPosition(head_centroid_x, head_centroid_y, head_centroid_z); robot_geometry.CreateStereoCameras( no_of_stereo_cameras, baseline_mm, dist_from_centre_of_tilt_mm, image_width, image_height, FOV_degrees, head_diameter_mm, default_head_orientation_degrees); robot_geometry.CreateSensorModels(buffer); }
/// <summary> /// parse an xml node /// </summary> /// <param name="xnod"></param> /// <param name="level"></param> public void LoadFromXml( XmlNode xnod, int level) { XmlNode xnodWorking; if (xnod.Name == "SteersmanGeometry") { int cameraIndex = -1; if (robot_geometry == null) robot_geometry = new robotGeometry(); robot_geometry.LoadFromXml(xnod, level + 1, ref cameraIndex); } if (xnod.Name == "SteersmanBuffer") { if (buffer == null) buffer = new metagridBuffer(); int no_of_grid_levels=0; int grid_type=0; int dimension_mm=0; int dimension_vertical_mm=0; int cellSize_mm=0; int localisationRadius_mm=0; int maxMappingRange_mm=0; float vacancyWeighting=0; buffer.LoadFromXml(xnod, level + 1, ref no_of_grid_levels, ref grid_type, ref dimension_mm, ref dimension_vertical_mm, ref cellSize_mm, ref localisationRadius_mm, ref maxMappingRange_mm, ref vacancyWeighting ); buffer.Initialise( no_of_grid_levels, grid_type, dimension_mm, dimension_vertical_mm, cellSize_mm, localisationRadius_mm, maxMappingRange_mm, vacancyWeighting); } if (xnod.Name == "SteersmanSensorModels") { if (robot_geometry == null) robot_geometry = new robotGeometry(); int no_of_stereo_cameras = 0; int no_of_grid_levels = 0; int camera_index = 0; int grid_level = 0; robot_geometry.LoadFromXmlSensorModels( xnod, level + 1, ref no_of_stereo_cameras, ref no_of_grid_levels, ref camera_index, ref grid_level); } // call recursively on all children of the current node if (xnod.HasChildNodes) { xnodWorking = xnod.FirstChild; while (xnodWorking != null) { LoadFromXml(xnodWorking, level + 1); xnodWorking = xnodWorking.NextSibling; } } }
public void LocaliseAlongPath() { // systematic bias float bias_x_mm = -200; float bias_y_mm = 0; // number of stereo cameras on the robot's head int no_of_stereo_cameras = 2; // diameter of the robot's head float head_diameter_mm = 100; // number of stereo features per step during mapping int no_of_mapping_stereo_features = 300; // number of stereo features observed per localisation step int no_of_localisation_stereo_features = 100; string filename = "localise_along_path.dat"; float path_length_mm = 20000; float start_orientation = 0; float end_orientation = 0; // 90 * (float)Math.PI / 180.0f; float distance_between_poses_mm = 100; float disparity = 15; string overall_map_filename = "overall_map.jpg"; byte[] overall_map_img = null; int overall_img_width = 640; int overall_img_height = 480; int overall_map_dimension_mm = 0; int overall_map_centre_x_mm = 0; int overall_map_centre_y_mm = 0; string[] str = filename.Split('.'); List<OdometryData> path = null; SavePath( filename, path_length_mm, start_orientation, end_orientation, distance_between_poses_mm, disparity, no_of_mapping_stereo_features, no_of_stereo_cameras, ref path); Assert.AreEqual(true, File.Exists(filename)); Assert.AreEqual(true, File.Exists(str[0] + "_disparities_index.dat")); Assert.AreEqual(true, File.Exists(str[0] + "_disparities.dat")); int no_of_grids = 1; int grid_type = metagrid.TYPE_SIMPLE; int dimension_mm = 8000; int dimension_vertical_mm = 2000; int cellSize_mm = 50; int localisationRadius_mm = 8000; int maxMappingRange_mm = 10000; float vacancyWeighting = 0.5f; metagridBuffer buffer = new metagridBuffer( no_of_grids, grid_type, dimension_mm, dimension_vertical_mm, cellSize_mm, localisationRadius_mm, maxMappingRange_mm, vacancyWeighting); buffer.LoadPath( filename, str[0] + "_disparities_index.dat", str[0] + "_disparities.dat", ref overall_map_dimension_mm, ref overall_map_centre_x_mm, ref overall_map_centre_y_mm); int img_width = 640; int img_height = 640; byte[] img = new byte[img_width * img_height * 3]; Bitmap bmp = new Bitmap(img_width, img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb); buffer.ShowPath(img, img_width, img_height, true, true); BitmapArrayConversions.updatebitmap_unsafe(img, bmp); bmp.Save("localise_along_path.jpg", System.Drawing.Imaging.ImageFormat.Jpeg); robotGeometry geom = new robotGeometry(); geom.CreateStereoCameras( no_of_stereo_cameras, 120, 0, 320, 240, 65, head_diameter_mm, 0); geom.CreateSensorModels(buffer); Random rnd = new Random(0); pos3D pose_offset = null; bool buffer_transition = false; float[][] stereo_features = new float[no_of_stereo_cameras][]; byte[][,] stereo_features_colour = new byte[no_of_stereo_cameras][,]; float[][] stereo_features_uncertainties = new float[no_of_stereo_cameras][]; for (int i = 0; i < no_of_stereo_cameras; i++) { stereo_features_uncertainties[i] = new float[no_of_localisation_stereo_features]; for (int j = 0; j < no_of_localisation_stereo_features; j++) stereo_features_uncertainties[i][j] = 1; } float average_offset_x_mm = 0; float average_offset_y_mm = 0; List<OdometryData> estimated_path = new List<OdometryData>(); int no_of_localisation_failures = 0; for (int i = 0; i < path.Count-1; i += 5) { string debug_mapping_filename = "localise_along_path_map_" + i.ToString() + ".jpg"; OdometryData p0 = path[i]; OdometryData p1 = path[i + 1]; // create an intermediate pose for (int cam = 0; cam < no_of_stereo_cameras; cam++) { geom.pose[cam].x = p0.x + ((p1.x - p0.x)/2) + bias_x_mm; geom.pose[cam].y = p0.y + ((p1.y - p0.y)/2) + bias_y_mm; geom.pose[cam].z = 0; geom.pose[cam].pan = p0.orientation + ((p1.orientation - p0.orientation)/2); // create stereo features int ctr = 0; stereo_features[cam] = new float[no_of_localisation_stereo_features * 3]; stereo_features_colour[cam] = new byte[no_of_localisation_stereo_features, 3]; for (int f = 0; f < no_of_localisation_stereo_features; f += 5) { if (f < no_of_localisation_stereo_features/2) { stereo_features[cam][ctr++] = 20; stereo_features[cam][ctr++] = rnd.Next(239); } else { stereo_features[cam][ctr++] = geom.image_width[cam] - 20; stereo_features[cam][ctr++] = rnd.Next(239); } stereo_features[cam][ctr++] = disparity; } } float matching_score = buffer.Localise( geom, stereo_features, stereo_features_colour, stereo_features_uncertainties, rnd, ref pose_offset, ref buffer_transition, debug_mapping_filename, bias_x_mm, bias_y_mm, 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); if (matching_score != occupancygridBase.NO_OCCUPANCY_EVIDENCE) { Console.WriteLine("pose_offset (mm): " + pose_offset.x.ToString() + ", " + pose_offset.y.ToString() + ", " + pose_offset.pan.ToString()); OdometryData estimated_pose = new OdometryData(); estimated_pose.x = geom.pose[0].x + pose_offset.x; estimated_pose.y = geom.pose[0].y + pose_offset.y; estimated_pose.orientation = geom.pose[0].pan + pose_offset.pan; estimated_path.Add(estimated_pose); average_offset_x_mm += pose_offset.x; average_offset_y_mm += pose_offset.y; } else { // fail! no_of_localisation_failures++; Console.WriteLine("Localisation failure"); } } buffer.ShowPath(img, img_width, img_height, true, true); BitmapArrayConversions.updatebitmap_unsafe(img, bmp); bmp.Save("localisations_along_path.jpg", System.Drawing.Imaging.ImageFormat.Jpeg); average_offset_x_mm /= estimated_path.Count; average_offset_y_mm /= estimated_path.Count; Console.WriteLine("Average offsets: " + average_offset_x_mm.ToString() + ", " + average_offset_y_mm.ToString()); float diff_x_mm = Math.Abs(average_offset_x_mm - bias_x_mm); float diff_y_mm = Math.Abs(average_offset_y_mm - bias_y_mm); Assert.Less(diff_x_mm, cellSize_mm*3/2, "x bias not detected"); Assert.Less(diff_y_mm, cellSize_mm*3/2, "y bias not detected"); if (no_of_localisation_failures > 0) Console.WriteLine("Localisation failures: " + no_of_localisation_failures.ToString()); else Console.WriteLine("No localisation failures!"); Assert.Less(no_of_localisation_failures, 4, "Too many localisation failures"); }
/// <summary> /// parse an xml node /// </summary> /// <param name="xnod"></param> /// <param name="level"></param> public void LoadFromXml( XmlNode xnod, int level) { XmlNode xnodWorking; if (xnod.Name == "SteersmanGeometry") { int cameraIndex = -1; if (robot_geometry == null) { robot_geometry = new robotGeometry(); } robot_geometry.LoadFromXml(xnod, level + 1, ref cameraIndex); } if (xnod.Name == "SteersmanBuffer") { if (buffer == null) { buffer = new metagridBuffer(); } int no_of_grid_levels = 0; int grid_type = 0; int dimension_mm = 0; int dimension_vertical_mm = 0; int cellSize_mm = 0; int localisationRadius_mm = 0; int maxMappingRange_mm = 0; float vacancyWeighting = 0; buffer.LoadFromXml(xnod, level + 1, ref no_of_grid_levels, ref grid_type, ref dimension_mm, ref dimension_vertical_mm, ref cellSize_mm, ref localisationRadius_mm, ref maxMappingRange_mm, ref vacancyWeighting ); buffer.Initialise( no_of_grid_levels, grid_type, dimension_mm, dimension_vertical_mm, cellSize_mm, localisationRadius_mm, maxMappingRange_mm, vacancyWeighting); } if (xnod.Name == "SteersmanSensorModels") { if (robot_geometry == null) { robot_geometry = new robotGeometry(); } int no_of_stereo_cameras = 0; int no_of_grid_levels = 0; int camera_index = 0; int grid_level = 0; robot_geometry.LoadFromXmlSensorModels( xnod, level + 1, ref no_of_stereo_cameras, ref no_of_grid_levels, ref camera_index, ref grid_level); } // call recursively on all children of the current node if (xnod.HasChildNodes) { xnodWorking = xnod.FirstChild; while (xnodWorking != null) { LoadFromXml(xnodWorking, level + 1); xnodWorking = xnodWorking.NextSibling; } } }