// Redefined virtual functions public override void read_parameters(Settings settings) { // Initialise any base class parameters base.read_parameters(settings); // Initialise my camera model Settings.Section section = settings.get_section(feature_type); if(section == null) { Debug.WriteLine("Warning: no [" + feature_type + "] section in the intialisation data. " + "The feature measurement model will be initialised with the default " + "camera parameters."); } else { // Read the camera model. This will be the name of another section ArrayList values = section.get_entry("Camera"); if (values == null) { Debug.WriteLine("Warning: no Camera entry in the [" + feature_type + "] section in the intialisation data. " + "The feature measurement model will be initialised with the " + "default camera parameters."); } else { String camera = (String)values[0]; section = settings.get_section(camera); if(section == null) { Debug.WriteLine("Warning: no [" + camera + "] section in the intialisation data. " + "The feature measurement model will be initialised with the " + " default camera parameters."); } else ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).read_parameters(section); } } }
public virtual classimage_mono initialise_known_feature(Feature_Measurement_Model f_m_m, Vector yi, Settings.Section section, String path) { return (null); }
// Read the parameters of the camera from the settings. public void read_parameters(Settings.Section section) { ArrayList values = section.get_entry("CameraParameters"); if (values != null) { String param = (String)values[0]; m_camera.ReadASCII(param); } else { String error; error = "No CameraParameters entry found in initialisation file section [" + section.Label() + "]."; Debug.WriteLine(error); } }
/// <summary> /// Use this version if using internal measurement models defined in settings file. /// </summary> /// <param name="mm_creator"></param> /// <param name="imm_creator"></param> public Scene_Single(Settings settings, Motion_Model_Creator mm_creator, Internal_Measurement_Model_Creator imm_creator) { // What is the motion model? ArrayList values = settings.get_entry("Models", "MotionModel"); String model = (String)values[0]; //assert(mm_creator != NULL); motion_model = mm_creator.create_model(model); if (motion_model == null) { Debug.WriteLine("Unable to create a motion model of type " + model + " as requested in the initalisation file. "); } // Initialise the motion model with any settings motion_model.read_parameters(settings); // Create internal measurement models if required if (imm_creator != null) { uint imm_number = 0; // Read in and create potentially various internal measurement models while (true) { // A list of internal measurement models will be given in the // Models section of settings with entry names // InternalMeasurementModel0, InternalMeasurementModel1, etc. String entry_name = "InternalMeasurementModel" + Convert.ToString(imm_number); values = settings.get_entry("Models", entry_name); String new_imm_type = (String)values[0]; // Exit loop if no more internal measurement models if (new_imm_type == "") break; // Initialise new model and add to Scene Internal_Measurement_Model new_imm = imm_creator.create_model(new_imm_type, motion_model); // Note for future: at this stage we should potentially read in // parameters for new imm; potentially we could have several internal // measurements define which have the same model but different parameters // (just like we have multiple features) add_internal_measurement(new_imm); imm_number++; } } // Get the initial state settings Vector initial_xv = null; MatrixFixed initial_Pxx = null; motion_model.read_initial_state(settings, ref initial_xv, ref initial_Pxx); scene_constructor_bookkeeping(initial_xv, initial_Pxx); }
/**************************Initialise Known Features**************************/ /// <summary> /// Initialise the Scene_Single class with some known features, read from the /// Settings. Each known feature has its own section, starting with /// <code>[KnownFeature1]</code> and counting upwards. The feature type is /// identified with the entry <code>FeatureMeasurementModel=</code>. Further /// settings are loaded by the feature measurement model itself. /// </summary> /// <param name="model_creator"></param> /// <param name="sim_or_rob"></param> /// <param name="scene"></param> /// <returns></returns> public static uint initialise_known_features(Settings settings, Feature_Measurement_Model_Creator model_creator, Sim_Or_Rob sim_or_rob, Scene_Single scene, String path, float MAXIMUM_ANGLE_DIFFERENCE) { uint feature_no = 1; uint num_features = 0; Settings.Section section = null; do { // Step through the section names String section_name = "KnownFeature" + Convert.ToString(feature_no); section = settings.get_section(section_name); // Does this section exist? if (section == null) { return num_features; } ArrayList values = section.get_entry("FeatureMeasurementModel"); if (values == null) { Debug.WriteLine("No FeatureMeasurementModel entry under the section [" + section_name + "] in initalisation file."); } else { String type = (String)values[0]; Feature_Measurement_Model f_m_m = model_creator.create_model(type, scene.get_motion_model(), MAXIMUM_ANGLE_DIFFERENCE); if (f_m_m == null) { Debug.WriteLine("Unable to create a feature measurement model of type " + type + " as requested in initalisation file."); } else { // Initialise the feature measurement model with any settings f_m_m.read_parameters(settings); // Read the feature state Vector yi = new Vector(3); Vector xp_orig = new Vector(7); f_m_m.read_initial_state(section, yi, xp_orig); // Initialise the feature byte[] identifier = sim_or_rob.initialise_known_feature(f_m_m, yi, section, path); if (identifier == null) { Debug.WriteLine("Trouble reading known feature " + section_name + " : skipping."); } else { scene.add_new_known_feature(identifier, yi, xp_orig, f_m_m, feature_no); Debug.WriteLine("Added known feature " + Convert.ToString(feature_no)); num_features++; } } } feature_no++; } while (section != null); return num_features; }
/// <summary> /// Constructor /// </summary> /// <param name="initialisation_file">The initialisation file to read. This specifies the motion- and feature-measurement models to use, the initial state and known features.</param> /// <param name="mm_creator">The factory to use to create motion models.</param> /// <param name="fmm_creator">The factory to use to create feature measurement models</param> /// <param name="imm_creator">The factory to use to create internal measurement models</param> /// <param name="number_of_features_to_select">The number of features to select for measurement at each time step</param> /// <param name="number_of_features_to_keep_visible">The requried number of visible features. If fewer than this number are visible at any time step, the creation of a new feature is initiated</param> /// <param name="max_features_to_init_at_once"></param> /// <param name="min_lambda">The minimum distance from the camera (in metres) for a new feature</param> /// <param name="max_lambda">The maximum distance from the camera (in metres) for a new feature</param> /// <param name="number_of_particles">The number of particles to use for new features (distributed evenly in space between min_lambda and max_lambda)</param> /// <param name="standard_deviation_depth_ratio">The ratio between standard deviation and mean to use to identify when a partially-initialised feature should be converted to a fully-initialised one</param> /// <param name="min_number_of_particles">The minimum number of particles below which a partially-initalised feature is deleted</param> /// <param name="prune_probability_threshold">The threshold below which a particle with low probability is deleted</param> /// <param name="erase_partially_init_feature_after_this_many_attempts">The number of failed match attempts before a partially initialised feature is deleted.</param> public MonoSLAM(String initialisation_file, String path, Motion_Model_Creator mm_creator, Feature_Measurement_Model_Creator fmm_creator, Internal_Measurement_Model_Creator imm_creator, uint number_of_features_to_select, uint number_of_features_to_keep_visible, uint max_features_to_init_at_once, float min_lambda, float max_lambda, uint number_of_particles, float standard_deviation_depth_ratio, uint min_number_of_particles, float prune_probability_threshold, uint erase_partially_init_feature_after_this_many_attempts, float MAXIMUM_ANGLE_DIFFERENCE, float calibration_target_width_mm, float calibration_target_height_mm, float calibration_target_distance_mm) { PATH = path; NUMBER_OF_FEATURES_TO_SELECT = number_of_features_to_select; NUMBER_OF_FEATURES_TO_KEEP_VISIBLE = number_of_features_to_keep_visible; MAX_FEATURES_TO_INIT_AT_ONCE = max_features_to_init_at_once; MIN_LAMBDA = min_lambda; MAX_LAMBDA = max_lambda; NUMBER_OF_PARTICLES = number_of_particles; STANDARD_DEVIATION_DEPTH_RATIO = standard_deviation_depth_ratio; MIN_NUMBER_OF_PARTICLES = min_number_of_particles; PRUNE_PROBABILITY_THRESHOLD = prune_probability_threshold; ERASE_PARTIALLY_INIT_FEATURE_AFTER_THIS_MANY_ATTEMPTS = erase_partially_init_feature_after_this_many_attempts; number_of_visible_features = 0; number_of_matched_features = 0; Settings settings = new Settings(); //if no file exists create some default values //if (!File.Exists(PATH + initialisation_file)) { //create a settings file settings.createDefault(PATH + initialisation_file, calibration_target_width_mm, calibration_target_height_mm, calibration_target_distance_mm); //settings.createDefault(PATH + initialisation_file, 210, 148.5, 600); } //create some known features //createDefaultKnownFeatures(PATH); // Create the Settings class by reading from the initialisation file if (File.Exists(PATH + initialisation_file)) { StreamReader stream = File.OpenText(PATH + initialisation_file); settings.load(stream); // Create the Scene class. This also constructs the motion model and // internal measurement models and sets the initial state scene = new Scene_Single(settings, mm_creator, imm_creator); // Now sort out the feature types ArrayList values = settings.get_entry("Models", "NewFeatureMeasurementModel"); String feature_init_type = (String)values[0]; Feature_Measurement_Model fm_model = fmm_creator.create_model(feature_init_type, scene.get_motion_model(), MAXIMUM_ANGLE_DIFFERENCE); if (fm_model == null) { Debug.WriteLine("Unable to create a feature measurement motion model of type " + feature_init_type + " as requested in initalisation file " + initialisation_file); } else { // Initialise this motion model fm_model.read_parameters(settings); // Check that this is a partially-initialised feature type if (fm_model.fully_initialised_flag) { Debug.WriteLine("Feature measurement motion model " + feature_init_type + " as requested in initalisation file " + initialisation_file + " is not a partially-initialised feature type. "); } default_feature_type_for_initialisation = (Partially_Initialised_Feature_Measurement_Model)fm_model; // We hope that features are viewed through a camera! If so, // the feature measurement class should derive from // Camera_Feature_Measurement_Model // Note the multiple inherritance workaround Camera_Feature_Measurement_Model cfmm = (Camera_Feature_Measurement_Model)(fm_model.wide_model); if (cfmm == null) { // Oops - the feature measurement model is not derived from // Camera_Feature_Measurement_Model! Debug.WriteLine("The default feature measurement motion model " + fm_model.feature_type + " is not derived from Camera_Feature_Measurement_Model!"); } else { CAMERA_WIDTH = cfmm.get_camera().ImageWidth(); CAMERA_HEIGHT = cfmm.get_camera().ImageHeight(); kalman = new Kalman(); robot = new Robot(); sim_or_rob = (Sim_Or_Rob)robot; // Initialise any known features SceneLib.initialise_known_features(settings, fmm_creator, sim_or_rob, scene, PATH, MAXIMUM_ANGLE_DIFFERENCE); // Various flags init_feature_search_region_defined_flag = false; } } stream.Close(); } else { Debug.WriteLine("File not found: " + initialisation_file); } }
/// <summary> /// Read the initial state for this feature from the Settings. This reads the /// state \vct{y}_i and the robot position from which it was first /// observed, \vct{x}_p^{orig} . These are not stored in the class, so are /// returned as parameters. /// </summary> /// <param name="settings">The Settings class from which to read the data</param> /// <param name="yi">To be filled in with the initial feature state, \vct{y}_i .</param> /// <param name="xp_orig">To be filled in with the robot position from which the feature was first observed, \vct{x}_p^{orig} .</param> public void read_initial_state(Settings.Section section, Vector yi, Vector xp_orig) { ArrayList values; yi.Resize(FEATURE_STATE_SIZE); values = section.get_entry("yi"); yi.ReadASCII((String)values[0]); uint size = get_motion_model().POSITION_STATE_SIZE; xp_orig.Resize(size); values = section.get_entry("xp_orig"); xp_orig.ReadASCII((String)values[0]); }
// Read the parameters of this feature measurement model from the settings. public virtual void read_parameters(Settings settings) {}
/// <summary> /// Read the initial state vector and covariance from the settings class. /// Since state \f$ x_v \f$ and covariance \f$ P_{xx} \f$ are not stored in /// the class, these are passed by reference to be filled in by this function. /// </summary> /// <param name="settings"></param> /// <param name="initial_xv"></param> /// <param name="initial_Pxx"></param> public void read_initial_state(Settings settings, ref Vector initial_xv, ref MatrixFixed initial_Pxx) { ArrayList values; // Check that the motion model is correct values = settings.get_entry("InitialState", "MotionModel"); if ((String)values[0] != motion_model_type) { Debug.WriteLine("Attempted to read an initial state with a motion model of type " + motion_model_type + " where the initialisation data in the [InitialState] section" + " reports the type " + settings.get_entry("InitialState", "MotionModel") + "."); //throw Scene::InitialisationError(error.str()); } // Make sure the vector and matrix are the correct sizes initial_xv = new Vector(STATE_SIZE); initial_Pxx = new MatrixFixed(STATE_SIZE, STATE_SIZE); //initial_xv.Resize(STATE_SIZE); //initial_Pxx.Resize(STATE_SIZE, STATE_SIZE); initial_xv.Fill(0.0f); initial_Pxx.Fill(0.0f); values = settings.get_entry("InitialState", "xv"); String xv_stream = (String)values[0]; initial_xv.ReadASCII(xv_stream); values = settings.get_entry("InitialState", "Pxx"); String Pxx_stream = (String)values[0]; initial_Pxx.ReadASCII(Pxx_stream); }
/// <summary> /// Initialise a known feature, in this case by loading an image file. The name of /// the file is read from the Settings Section passed to this function, with the /// entry Identifier. /// </summary> /// <param name="fmm"></param> /// <param name="v"></param> /// <param name="section"></param> /// <returns></returns> public override classimage_mono initialise_known_feature(Feature_Measurement_Model fmm, Vector v, Settings.Section section, String path) { ArrayList values = section.get_entry("Identifier"); String name = (String)values[0]; //cout << "Reading patch " << name << endl; classimage_mono patch = new classimage_mono(); if (!(patch.loadFromBitmapMono(path + name, (int)Camera_Constants.BOXSIZE, (int)Camera_Constants.BOXSIZE))) { patch = null; } return patch; }