示例#1
0
        static void Main(string[] args)
        {
            // extract command line parameters
            ArrayList parameters = commandline.ParseCommandLineParameters(args, "-", GetValidParameters());

            string help_str = commandline.GetParameterValue("help", parameters);
            if (help_str != "")
            {
                ShowHelp();
            }
            else
            {
                Console.WriteLine("viewpath: creates images used to visualise occupancy grid data along a path");

                string steersman_filename = commandline.GetParameterValue("steer", parameters);
                if (steersman_filename == "")
                {
                    Console.WriteLine("Please provide a steersman filename");
                }
                else
                {
                    if (!File.Exists(steersman_filename))
                    {
                        Console.WriteLine("Cound't find steersman file");
                        Console.WriteLine(steersman_filename);
                    }
                    else
                    {
                        string path_filename = commandline.GetParameterValue("path", parameters);
                        if (path_filename == "")
                        {
                            Console.WriteLine("Please provide a path filename");
                        }
                        else
                        {
                            if (!File.Exists(path_filename))
                            {
                                Console.WriteLine("Cound't find path file");
                                Console.WriteLine(path_filename);
                            }
                            else
                            {

                                string disparities_filename = commandline.GetParameterValue("disp", parameters);
                                if (disparities_filename == "")
                                {
                                    Console.WriteLine("Please provide a disparities filename");
                                }
                                else
                                {

                                    if (!File.Exists(disparities_filename))
                                    {
                                        Console.WriteLine("Cound't find disparities file");
                                        Console.WriteLine(disparities_filename);
                                    }
                                    else
                                    {
                                        string disparities_index_filename = commandline.GetParameterValue("index", parameters);
                                        if (disparities_index_filename == "")
                                        {
                                            Console.WriteLine("Please provide a disparities index filename");
                                        }
                                        else
                                        {
                                            if (!File.Exists(disparities_index_filename))
                                            {
                                                Console.WriteLine("Cound't find disparities index file");
                                                Console.WriteLine(disparities_index_filename);
                                            }
                                            else
                                            {
                                                steersman visual_guidance = new steersman();
                                                if (visual_guidance.Load(steersman_filename))
                                                {
                                                    // load the path
                                                    visual_guidance.LoadPath(
                                                        path_filename,
                                                        disparities_index_filename,
                                                        disparities_filename);

                                                    string path_image_filename = "localise_along_path.jpg";
                                                    visual_guidance.ShowLocalisations(path_image_filename, 640, 480);
                                                    Console.WriteLine("Saved " + path_image_filename);
                                                }
                                                else
                                                {
                                                    Console.WriteLine("Couldn't load steersman file");
                                                }
                                            }
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
示例#2
0
		public void LocaliseAlongPath()
		{
            // systematic bias
            float bias_x_mm = -200;
            float bias_y_mm = 0;
			
			// 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 = 50;

		    string filename = "steersman_localise_along_path.dat";
		    float path_length_mm = 10000;
		    float start_orientation = 0;
		    float end_orientation = 0; //90 * (float)Math.PI / 180.0f;
		    float distance_between_poses_mm = 100;
            float disparity = 15;
			
			string steersman_filename = "tests_steersman_LocaliseAlongPath.xml";
		    int body_width_mm = 465;
		    int body_length_mm = 380;
		    int body_height_mm= 1660;
		    int centre_of_rotation_x = body_width_mm/2;
		    int centre_of_rotation_y = body_length_mm/2;
		    int centre_of_rotation_z = 10;
		    int head_centroid_x = body_width_mm/2;
		    int head_centroid_y = 65;
		    int head_centroid_z = 1600;
		    string sensormodels_filename = "";
		    int no_of_stereo_cameras = 2;
		    float baseline_mm = 120;
			float dist_to_centre_of_tilt_mm = 0;
		    int image_width = 320;
		    int image_height = 240;
		    float FOV_degrees = 65;
		    float head_diameter_mm = 160;
		    float default_head_orientation_degrees = 0;
            int no_of_grid_levels = 1;
		    int dimension_mm = 8000;
            int dimension_vertical_mm = 2000; 
            int cellSize_mm = 50;
						
			string[] str = filename.Split('.');
				
			List<OdometryData> path = null;
            tests_metagridbuffer.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"));
						
			steersman visual_guidance = null;
			if (File.Exists(steersman_filename))
			{
				visual_guidance = new steersman();
				visual_guidance.Load(steersman_filename);
			}
			else
			{
				visual_guidance = new steersman(
			        body_width_mm,
			        body_length_mm,
			        body_height_mm,
			        centre_of_rotation_x,
			        centre_of_rotation_y,
			        centre_of_rotation_z,
			        head_centroid_x,
			        head_centroid_y,
			        head_centroid_z,
			        sensormodels_filename,
			        no_of_stereo_cameras,
			        baseline_mm,
				    dist_to_centre_of_tilt_mm,
			        image_width,
			        image_height,
			        FOV_degrees,
			        head_diameter_mm,
			        default_head_orientation_degrees,
	                no_of_grid_levels,
			        dimension_mm,
	                dimension_vertical_mm,
	                cellSize_mm);
				
			    visual_guidance.Save(steersman_filename);
			}
			
            visual_guidance.LoadPath(filename, str[0] + "_disparities_index.dat", str[0] + "_disparities.dat");
            
			visual_guidance.ShowLocalisations("steersman_localise_along_path.jpg", 640, 480);
			
		    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;

			DateTime start_time = DateTime.Now;
			int no_of_localisations = 0;
			for (int i = 0; i < path.Count-1; i += 5, no_of_localisations++)
			{
                string debug_mapping_filename = "steersman_localise_along_path_map_" + i.ToString() + ".jpg";

				OdometryData p0 = path[i];
				OdometryData p1 = path[i + 1];
				
				float current_x_mm = p0.x + ((p1.x - p0.x)/2) + bias_x_mm; 
				float current_y_mm = p0.y + ((p1.y - p0.y)/2) + bias_y_mm;
				float current_pan = p0.orientation + ((p1.orientation - p0.orientation)/2);
				
				// create an intermediate pose
				for (int cam = 0; cam < no_of_stereo_cameras; cam++)
				{
					// set the current pose
					visual_guidance.SetCurrentPosition(
					    cam,
				        current_x_mm,
				        current_y_mm,
				        0,
				        current_pan,
					    0, 0);
								
					// 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++] = image_width - 20;
							stereo_features[cam][ctr++] = rnd.Next(239);
						}
					    stereo_features[cam][ctr++] = disparity;
					}
				}
				
				float offset_x_mm = 0;
				float offset_y_mm = 0;
				float offset_z_mm = 0;
				float offset_pan = 0;
				float offset_tilt = 0;
				float offset_roll = 0;
				
				bool valid_localisation = visual_guidance.Localise(
				    stereo_features,
				    stereo_features_colour,
				    stereo_features_uncertainties,
				    debug_mapping_filename,
				    bias_x_mm, bias_y_mm,
				    ref offset_x_mm,
				    ref offset_y_mm,
				    ref offset_z_mm,
				    ref offset_pan,
				    ref offset_tilt,
				    ref offset_roll);
				
				if (valid_localisation)
				{				
					Console.WriteLine("pose_offset (mm): " + offset_x_mm.ToString() + ", " + offset_y_mm.ToString() + ", " + offset_pan.ToString());
					OdometryData estimated_pose = new OdometryData();
					estimated_pose.x = current_x_mm + offset_x_mm;
					estimated_pose.y = current_y_mm + offset_y_mm;
					estimated_pose.orientation = current_pan + offset_pan;
					estimated_path.Add(estimated_pose);
	                average_offset_x_mm += offset_x_mm;
	                average_offset_y_mm += offset_y_mm;
				}
				else
				{
					// fail!
					no_of_localisation_failures++;
					Console.WriteLine("Localisation failure");
				}
			}
			
			TimeSpan diff = DateTime.Now.Subtract(start_time);
			float time_per_localisation_sec = (float)diff.TotalSeconds / no_of_localisations;
			Console.WriteLine("Time per localisation: " + time_per_localisation_sec.ToString() + " sec");

            visual_guidance.ShowLocalisations("steersman_localisations_along_path.jpg", 640, 480);

            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");
        }
示例#3
0
		public static void Main(string[] args)
		{
			//tests_metagridbuffer.LocaliseAlongPath();
			
            // extract command line parameters
            ArrayList parameters = commandline.ParseCommandLineParameters(args, "-", GetValidParameters());

            string help_str = commandline.GetParameterValue("help", parameters);
            if (help_str != "")
            {
                ShowHelp();
            }
            else
            {
                Console.WriteLine("steersman: creates sensor models to steer a robot along a path");					
				
				int body_width_mm = 465;
                string body_width_mm_str = commandline.GetParameterValue("bodywidth", parameters);
                if (body_width_mm_str != "")
					body_width_mm = Convert.ToInt32(body_width_mm_str);

				int body_length_mm = 375;
                string body_length_mm_str = commandline.GetParameterValue("bodylength", parameters);
                if (body_length_mm_str != "")
					body_length_mm = Convert.ToInt32(body_length_mm_str);

				int body_height_mm = 930;
                string body_height_mm_str = commandline.GetParameterValue("bodyheight", parameters);
                if (body_height_mm_str != "")
					body_height_mm = Convert.ToInt32(body_height_mm_str);

				int centre_of_rotation_x = body_width_mm/2;
                string centre_of_rotation_x_str = commandline.GetParameterValue("centrex", parameters);
                if (centre_of_rotation_x_str != "")
					centre_of_rotation_x = Convert.ToInt32(centre_of_rotation_x_str);

				int centre_of_rotation_y = 75; //body_length_mm/2;
                string centre_of_rotation_y_str = commandline.GetParameterValue("centrey", parameters);
                if (centre_of_rotation_y_str != "")
					centre_of_rotation_y = Convert.ToInt32(centre_of_rotation_y_str);

				int centre_of_rotation_z = 75;
                string centre_of_rotation_z_str = commandline.GetParameterValue("centrez", parameters);
                if (centre_of_rotation_z_str != "")
					centre_of_rotation_z = Convert.ToInt32(centre_of_rotation_z_str);

				int head_centroid_x = body_width_mm/2;
                string head_centroid_x_str = commandline.GetParameterValue("headx", parameters);
                if (head_centroid_x_str != "")
					head_centroid_x = Convert.ToInt32(head_centroid_x_str);

				int head_centroid_y = 65; //body_length_mm/2;
                string head_centroid_y_str = commandline.GetParameterValue("heady", parameters);
                if (head_centroid_y_str != "")
					head_centroid_y = Convert.ToInt32(head_centroid_y_str);

				int head_centroid_z = 1600; //body_height_mm;
                string head_centroid_z_str = commandline.GetParameterValue("headz", parameters);
                if (head_centroid_z_str != "")
					head_centroid_z = Convert.ToInt32(head_centroid_z_str);

                string sensormodels_filename = "";

				int no_of_stereo_cameras = 2;
                string no_of_stereo_cameras_str = commandline.GetParameterValue("cameras", parameters);
                if (no_of_stereo_cameras_str != "")
					no_of_stereo_cameras = Convert.ToInt32(no_of_stereo_cameras_str);

				float baseline_mm = 120;
                string baseline_mm_str = commandline.GetParameterValue("baseline", parameters);
                if (baseline_mm_str != "")
					baseline_mm = Convert.ToSingle(baseline_mm_str);

				// height of the cameras above the head tilt axis
				float dist_from_centre_of_tilt_mm = 0;
                string dist_from_centre_of_tilt_str = commandline.GetParameterValue("disttilt", parameters);
                if (dist_from_centre_of_tilt_str != "")
					dist_from_centre_of_tilt_mm = Convert.ToSingle(dist_from_centre_of_tilt_str);

				int image_width = 320;
				int image_height = 240;
                string resolution_str = commandline.GetParameterValue("resolution", parameters);
                if (resolution_str != "")
				{
					string[] str = resolution_str.ToLower().Split('x');
					if (str.Length == 2)
					{
						image_width = Convert.ToInt32(str[0]);
						image_height = Convert.ToInt32(str[1]);
					}					
				}

				float FOV_degrees = 65;
                string FOV_degrees_str = commandline.GetParameterValue("fov", parameters);
                if (FOV_degrees_str != "")
					FOV_degrees = Convert.ToSingle(FOV_degrees_str);

				float head_diameter_mm = 215;
                string head_diameter_mm_str = commandline.GetParameterValue("headdiam", parameters);
                if (head_diameter_mm_str != "")
					head_diameter_mm = Convert.ToSingle(head_diameter_mm_str);

				float default_head_orientation_degrees = 0;
                string default_head_orientation_degrees_str = commandline.GetParameterValue("orient", parameters);
                if (default_head_orientation_degrees_str != "")
					default_head_orientation_degrees = Convert.ToSingle(default_head_orientation_degrees_str);

				int no_of_grid_levels = 1;
                string no_of_grid_levels_str = commandline.GetParameterValue("gridlevels", parameters);
                if (no_of_grid_levels_str != "")
					no_of_grid_levels = Convert.ToInt32(no_of_grid_levels_str);
				
				int dimension_mm = 8000;
                string dimension_mm_str = commandline.GetParameterValue("griddim", parameters);
                if (dimension_mm_str != "")
					dimension_mm = Convert.ToInt32(dimension_mm_str);

				int dimension_vertical_mm = 2000;
                string dimension_vertical_mm_str = commandline.GetParameterValue("griddimvert", parameters);
                if (dimension_vertical_mm_str != "")
					dimension_vertical_mm = Convert.ToInt32(dimension_vertical_mm_str);

				int cellSize_mm = 50;
                string cellSize_mm_str = commandline.GetParameterValue("cellsize", parameters);
                if (cellSize_mm_str != "")
					cellSize_mm = Convert.ToInt32(cellSize_mm_str);
				
                string filename = commandline.GetParameterValue("filename", parameters);
                if (filename == "") filename = "steersman.xml";
                
				
				Console.WriteLine("Computing sensor models...Please wait");
				steersman visual_guidance = new steersman(
				    body_width_mm,
				    body_length_mm,
				    body_height_mm,
				    centre_of_rotation_x,
				    centre_of_rotation_y,
				    centre_of_rotation_z,
				    head_centroid_x,
				    head_centroid_y,
				    head_centroid_z,
				    sensormodels_filename,
				    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,
		            no_of_grid_levels,
				    dimension_mm, 
		            dimension_vertical_mm, 
		            cellSize_mm);
				
				visual_guidance.Save(filename);
				Console.WriteLine("Saved as " + filename);
            }
		}
示例#4
0
		public void SaveAndLoad()
		{
			string filename = "tests_steersman_SaveAndLoad.xml";
			
		    int body_width_mm = 465;
		    int body_length_mm = 380;
		    int body_height_mm= 1660;
		    int centre_of_rotation_x = body_width_mm/2;
		    int centre_of_rotation_y = body_length_mm/2;
		    int centre_of_rotation_z = 10;
		    int head_centroid_x = body_width_mm/2;
		    int head_centroid_y = 65;
		    int head_centroid_z = 1600;
		    string sensormodels_filename = "";
            int no_of_grid_levels = 2;
		    int dimension_mm = 8000; 
            int dimension_vertical_mm = 2000; 
            int cellSize_mm = 50;
			int no_of_stereo_cameras = 2;
			float baseline_mm = 120;
			float dist_to_centre_of_tilt_mm = 0;
			int image_width = 320;
			int image_height = 240;
			float FOV_degrees = 65;
			float head_diameter_mm = 180;
			float default_head_orientation_degrees = 0;
			
			steersman visual_guidance1 = null;
			
			if (File.Exists(filename))
			{
				visual_guidance1 = new steersman();
				visual_guidance1.Load(filename);
			}
			else
			{	
			    visual_guidance1 = new steersman(
		            body_width_mm,
		            body_length_mm,
		            body_height_mm,
		            centre_of_rotation_x,
		            centre_of_rotation_y,
		            centre_of_rotation_z,
		            head_centroid_x,
		            head_centroid_y,
		            head_centroid_z,
		            sensormodels_filename,
		            no_of_stereo_cameras,
		            baseline_mm,
				    dist_to_centre_of_tilt_mm,
		            image_width,
		            image_height,
		            FOV_degrees,
		            head_diameter_mm,
		            default_head_orientation_degrees,
                    no_of_grid_levels,
		            dimension_mm, 
                    dimension_vertical_mm, 
                    cellSize_mm);		                 
			}
			visual_guidance1.Save(filename);
			
			Assert.AreEqual(File.Exists(filename), true);
			
			steersman visual_guidance2 = new steersman();
			visual_guidance2.Load(filename);
			
			Assert.AreEqual(visual_guidance1.buffer.no_of_grid_levels, visual_guidance2.buffer.no_of_grid_levels);
			Assert.AreEqual(visual_guidance1.buffer.grid_type, visual_guidance2.buffer.grid_type);
		    Assert.AreEqual(visual_guidance1.buffer.dimension_mm, visual_guidance2.buffer.dimension_mm);
			Assert.AreEqual(visual_guidance1.buffer.dimension_vertical_mm, visual_guidance2.buffer.dimension_vertical_mm);
			Assert.AreEqual(visual_guidance1.buffer.cellSize_mm, visual_guidance2.buffer.cellSize_mm);
			Assert.AreEqual(visual_guidance1.buffer.localisationRadius_mm, visual_guidance2.buffer.localisationRadius_mm);
			Assert.AreEqual(visual_guidance1.buffer.maxMappingRange_mm, visual_guidance2.buffer.maxMappingRange_mm);
			Assert.AreEqual(visual_guidance1.buffer.vacancyWeighting, visual_guidance2.buffer.vacancyWeighting);
						
			Assert.AreEqual(visual_guidance1.robot_geometry.body_width_mm, visual_guidance2.robot_geometry.body_width_mm);
			Assert.AreEqual(visual_guidance1.robot_geometry.body_length_mm, visual_guidance2.robot_geometry.body_length_mm);
			Assert.AreEqual(visual_guidance1.robot_geometry.body_height_mm, visual_guidance2.robot_geometry.body_height_mm);
			
			Assert.AreEqual(visual_guidance1.robot_geometry.body_centre_of_rotation_x, visual_guidance2.robot_geometry.body_centre_of_rotation_x);
			Assert.AreEqual(visual_guidance1.robot_geometry.body_centre_of_rotation_y, visual_guidance2.robot_geometry.body_centre_of_rotation_y);
			Assert.AreEqual(visual_guidance1.robot_geometry.body_centre_of_rotation_z, visual_guidance2.robot_geometry.body_centre_of_rotation_z);
			
			Assert.AreEqual(visual_guidance1.robot_geometry.head_centroid_x, visual_guidance2.robot_geometry.head_centroid_x);
			Assert.AreEqual(visual_guidance1.robot_geometry.head_centroid_y, visual_guidance2.robot_geometry.head_centroid_y);
            Assert.AreEqual(visual_guidance1.robot_geometry.head_centroid_z, visual_guidance2.robot_geometry.head_centroid_z);
						
			for (int cam = 0; cam < no_of_stereo_cameras; cam++)
			{
				Assert.AreEqual(visual_guidance1.robot_geometry.baseline_mm[cam], visual_guidance2.robot_geometry.baseline_mm[cam]);
				Assert.AreEqual(visual_guidance1.robot_geometry.image_width[cam], visual_guidance2.robot_geometry.image_width[cam]);
				Assert.AreEqual(visual_guidance1.robot_geometry.image_height[cam], visual_guidance2.robot_geometry.image_height[cam]);
				Assert.AreEqual(visual_guidance1.robot_geometry.FOV_degrees[cam], visual_guidance2.robot_geometry.FOV_degrees[cam]);
				Assert.AreEqual(visual_guidance1.robot_geometry.stereo_camera_position_x[cam], visual_guidance2.robot_geometry.stereo_camera_position_x[cam]);
				Assert.AreEqual(visual_guidance1.robot_geometry.stereo_camera_position_y[cam], visual_guidance2.robot_geometry.stereo_camera_position_y[cam]);
				Assert.AreEqual(visual_guidance1.robot_geometry.stereo_camera_position_z[cam], visual_guidance2.robot_geometry.stereo_camera_position_z[cam]);
                Assert.AreEqual(visual_guidance1.robot_geometry.stereo_camera_pan[cam], visual_guidance2.robot_geometry.stereo_camera_pan[cam]);
                Assert.AreEqual(visual_guidance1.robot_geometry.stereo_camera_tilt[cam], visual_guidance2.robot_geometry.stereo_camera_tilt[cam]);
                Assert.AreEqual(visual_guidance1.robot_geometry.stereo_camera_roll[cam], visual_guidance2.robot_geometry.stereo_camera_roll[cam]);
			}
			
		}