/// <summary> /// Returns an instance of the requested feature measurement model (using new). /// Returns null if there was no match. /// </summary> /// <param name="type"></param> /// <param name="motion_model"></param> /// <returns></returns> public override Feature_Measurement_Model create_model(String type, Motion_Model motion_model, float MAXIMUM_ANGLE_DIFFERENCE) { // Try creating each model that we can and see if the name is the same // std::cout << "Creating a " << type << " feature measurement model" << std::endl; Feature_Measurement_Model pModel; pModel = new Fully_Init_Wide_Point_Feature_Measurement_Model(motion_model, MAXIMUM_ANGLE_DIFFERENCE); if (pModel.feature_type == type) return pModel; // The partially-initialised model also needs the fully-initalised // one that we have just created. Don't delete it - just store it Feature_Measurement_Model pFullModel = pModel; pModel = new Line_Init_Wide_Point_Feature_Measurement_Model(motion_model, pFullModel); if (pModel.feature_type == type) return pModel; else { pModel = null; pFullModel = null; } return null; }
public Line_Init_Wide_Point_Feature_Measurement_Model(Motion_Model motion_model, Feature_Measurement_Model fully_init_f_m_m) : base(2, 6, 6, motion_model, "CAMERA_WIDE_LINE_INIT", "THREED_SEMI_INFINITE_LINE", 1, fully_init_f_m_m) { WideCamera cam = new WideCamera(); float MAXIMUM_LENGTH_RATIO_ = Camera_Constants.WIDE_MAXIMUM_LENGTH_RATIO; float MAXIMUM_ANGLE_DIFFERENCE_ = Camera_Constants.WIDE_MAXIMUM_ANGLE_DIFFERENCE; float IMAGE_SEARCH_BOUNDARY_ = Camera_Constants.WIDE_IMAGE_SEARCH_BOUNDARY; float SD_IMAGE_SIMULATION_ = Camera_Constants.WIDE_SD_IMAGE_SIMULATION; wide_model = new Wide_Camera_Point_Feature_Measurement_Model(motion_model, cam, MAXIMUM_LENGTH_RATIO_, MAXIMUM_ANGLE_DIFFERENCE_, IMAGE_SEARCH_BOUNDARY_, SD_IMAGE_SIMULATION_); }
/// <summary> /// Model for single camera making point measurements /// </summary> /// <param name="motion_model">The motion model describing the robot motion. This must be derived from ThreeD_Motion_Model</param> /// <param name="cam">The camera model to use</param> /// <param name="MAXIMUM_LENGTH_RATIO_"></param> /// <param name="MAXIMUM_ANGLE_DIFFERENCE_"></param> /// <param name="IMAGE_SEARCH_BOUNDARY_"></param> /// <param name="SD_IMAGE_SIMULATION_"></param> public Fully_Init_Wide_Point_Feature_Measurement_Model( Motion_Model motion_model, WideCamera cam, float MAXIMUM_LENGTH_RATIO_, float MAXIMUM_ANGLE_DIFFERENCE_, float IMAGE_SEARCH_BOUNDARY_, float SD_IMAGE_SIMULATION_) : base(2, 3, 3, motion_model, "CAMERA_WIDE_POINT", "THREED_POINT") { wide_model = new Wide_Camera_Point_Feature_Measurement_Model(motion_model, cam, MAXIMUM_LENGTH_RATIO_, MAXIMUM_ANGLE_DIFFERENCE_, IMAGE_SEARCH_BOUNDARY_, SD_IMAGE_SIMULATION_); }
public Fully_Init_Wide_Point_Feature_Measurement_Model(Motion_Model motion_model, float WIDE_MAXIMUM_ANGLE_DIFFERENCE) : base(2, 3, 3, motion_model, "CAMERA_WIDE_POINT", "THREED_POINT") { WideCamera cam = new WideCamera(); float MAXIMUM_LENGTH_RATIO_ = Camera_Constants.WIDE_MAXIMUM_LENGTH_RATIO; float MAXIMUM_ANGLE_DIFFERENCE_ = WIDE_MAXIMUM_ANGLE_DIFFERENCE; //Camera_Constants.WIDE_MAXIMUM_ANGLE_DIFFERENCE; float IMAGE_SEARCH_BOUNDARY_ = Camera_Constants.WIDE_IMAGE_SEARCH_BOUNDARY; float SD_IMAGE_SIMULATION_ = Camera_Constants.WIDE_SD_IMAGE_SIMULATION; wide_model = new Wide_Camera_Point_Feature_Measurement_Model(motion_model, cam, MAXIMUM_LENGTH_RATIO_, MAXIMUM_ANGLE_DIFFERENCE_, IMAGE_SEARCH_BOUNDARY_, SD_IMAGE_SIMULATION_); }
/// <summary> /// Constructor /// </summary> /// <param name="motion_model">The motion model describing the robot motion. This must be derived from ThreeD_Motion_Model</param> /// <param name="cam">The camera model to use</param> /// <param name="MAXIMUM_LENGTH_RATIO_"></param> /// <param name="MAXIMUM_ANGLE_DIFFERENCE_"></param> /// <param name="IMAGE_SEARCH_BOUNDARY_"></param> /// <param name="SD_IMAGE_SIMULATION_"></param> public Wide_Camera_Point_Feature_Measurement_Model( Motion_Model motion_model, WideCamera cam, float MAXIMUM_LENGTH_RATIO_, float MAXIMUM_ANGLE_DIFFERENCE_, float IMAGE_SEARCH_BOUNDARY_, float SD_IMAGE_SIMULATION_) { MAXIMUM_LENGTH_RATIO = MAXIMUM_LENGTH_RATIO_; MAXIMUM_ANGLE_DIFFERENCE = MAXIMUM_ANGLE_DIFFERENCE_; IMAGE_SEARCH_BOUNDARY = IMAGE_SEARCH_BOUNDARY_; SD_IMAGE_SIMULATION = SD_IMAGE_SIMULATION_; m_camera = cam; if ((String)(motion_model.motion_model_dimensionality_type) != "THREED") { String os; os = "Motion Model given to the Feature Measurement Model is of type " + motion_model.motion_model_dimensionality_type + ", not type THREED."; Debug.WriteLine(os); //throw Scene::InitialisationError(os.str()); } threed_motion_model = (ThreeD_Motion_Model)motion_model; }
/// <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); }
/// <summary> /// Old style constructor /// Constuct the Scene_Single class and set the initial robot state. /// </summary> /// <param name="initial_xv">The initial robot state</param> /// <param name="initial_Pxx">The initial robot state covariance</param> /// <param name="m_m">The motion model to use</param> public Scene_Single(Vector initial_xv, MatrixFixed initial_Pxx, Motion_Model m_m) { motion_model = m_m; scene_constructor_bookkeeping(initial_xv, initial_Pxx); }
/// <summary> /// Constructor /// </summary> /// <param name="measurement_size">The number of parameters representing a measurement of the feature.</param> /// <param name="feature_state_size">The number of parameters to repreent the state of the feature.</param> /// <param name="graphics_state_size">The number of parameters to represent an abstraction of the feature for use in a graphical display.</param> /// <param name="motion_model">The robot motion model. Needed to understand and use the robot position statex_pwhen measuring features.</param> /// <param name="feature_type">The name for this feature measurement model (set by the derived class).</param> /// <param name="feature_graphics_type">The name for this type of feature, so that it can be drawn (set by the derived class).</param> public Fully_Initialised_Feature_Measurement_Model( uint measurement_size, uint feature_state_size, uint graphics_state_size, Motion_Model motion_model, String feature_type, String feature_graphics_type) : base(measurement_size, feature_state_size, graphics_state_size, motion_model, feature_type, feature_graphics_type, true) { hiRES = new Vector(MEASUREMENT_SIZE); dhi_by_dxpRES = new MatrixFixed(MEASUREMENT_SIZE, motion_model.POSITION_STATE_SIZE); dhi_by_dyiRES = new MatrixFixed(MEASUREMENT_SIZE, FEATURE_STATE_SIZE); nuiRES = new Vector(MEASUREMENT_SIZE); hi_noisyRES = new Vector(MEASUREMENT_SIZE); }
/// <summary> /// Constructor. /// </summary> /// <param name="measurement_size">The number of parameters representing a measurement of the feature</param> /// <param name="feature_state_size">The number of parameters to repreent the state of the feature.</param> /// <param name="graphics_state_size">The number of parameters to represent an abstraction of the feature for use in a graphical display</param> /// <param name="m_m">The robot motion model. Needed to understand and use the robot position statex_pwhen measuring features</param> /// <param name="f_t">The name for this feature measurement model (set by the derived class).</param> /// <param name="f_g_t">The name for this type of feature, so that it can be drawn (set by the derived class)</param> /// <param name="f_i_f">Is this model for fully-initialised features? (if <code>false</code> it is for partially-initialised features.). Set by the derived class.</param> public Feature_Measurement_Model( uint measurement_size, uint feature_state_size, uint graphics_state_size, Motion_Model m_m, String f_t, String f_g_t, bool f_i_f) { MEASUREMENT_SIZE = measurement_size; FEATURE_STATE_SIZE = feature_state_size; GRAPHICS_STATE_SIZE = graphics_state_size; feature_type = f_t; feature_graphics_type = f_g_t; fully_initialised_flag = f_i_f; _motion_model = m_m; // Allocate matrices for storage of results yigraphicsRES = new Vector(GRAPHICS_STATE_SIZE); PyiyigraphicsRES = new MatrixFixed(GRAPHICS_STATE_SIZE, GRAPHICS_STATE_SIZE); zeroedyigraphicsRES = new Vector(GRAPHICS_STATE_SIZE); PzeroedyigraphicsRES = new MatrixFixed(GRAPHICS_STATE_SIZE, GRAPHICS_STATE_SIZE); RiRES = new MatrixFixed(MEASUREMENT_SIZE, MEASUREMENT_SIZE); SiRES = new MatrixFixed(MEASUREMENT_SIZE, MEASUREMENT_SIZE); zeroedyiRES = new Vector(FEATURE_STATE_SIZE); dzeroedyi_by_dxpRES = new MatrixFixed(FEATURE_STATE_SIZE, get_motion_model().POSITION_STATE_SIZE); dzeroedyi_by_dyiRES = new MatrixFixed(FEATURE_STATE_SIZE, FEATURE_STATE_SIZE); }
/// Returns an instance of the requested internal measurement model /// (usually by using the new operator. TODO: who /// deletes them? public virtual Internal_Measurement_Model create_model(String type, Motion_Model motion_model) { return (null); }
public Internal_Measurement_Model( uint measurement_size, Motion_Model m_m, String i_t) { MEASUREMENT_SIZE = measurement_size; motion_model = m_m; internal_type = i_t; hvRES = new Vector(MEASUREMENT_SIZE); dhv_by_dxvRES = new MatrixFixed(MEASUREMENT_SIZE, motion_model.STATE_SIZE); RvRES = new MatrixFixed(MEASUREMENT_SIZE, MEASUREMENT_SIZE); nuvRES = new Vector(MEASUREMENT_SIZE); SvRES = new MatrixFixed(MEASUREMENT_SIZE, MEASUREMENT_SIZE); hv_noisyRES = new Vector(MEASUREMENT_SIZE); }
/// Returns an instance of the requested feature measurement model /// (usually by using the new operator. TODO: who /// deletes them? public virtual Feature_Measurement_Model create_model(String type, Motion_Model motion_model, float MAXIMUM_ANGLE_DIFFERENCE) { return (null); }
/// <summary> /// Constructor /// </summary> /// <param name="measurement_size">The number of parameters representing a measurement of the feature.</param> /// <param name="feature_state_size">The number of parameters to repreent the state of the feature.</param> /// <param name="graphics_state_size">The number of parameters to represent an abstraction of the feature for use in a graphical display.</param> /// <param name="motion_model">The robot motion model. Needed to understand and use the robot position statex_pwhen measuring features.</param> /// <param name="feature_type">The name for this feature measurement model (set by the derived class).</param> /// <param name="feature_graphics_type">The name for this type of feature, so that it can be drawn (set by the derived class).</param> /// <param name="free_parameter_size">The number of parameters that should be left outside the main Gaussian-based representation and represented by other means, such as particles.</param> /// <param name="m_i_f_m_m">What feature measurement model will this turn into once it's fully initialised?</param> public Partially_Initialised_Feature_Measurement_Model( uint measurement_size, uint feature_state_size, uint graphics_state_size, Motion_Model motion_model, String feature_type, String feature_graphics_type, uint free_parameter_size, Feature_Measurement_Model m_i_f_m_m) : base(measurement_size, feature_state_size, graphics_state_size, motion_model, feature_type, feature_graphics_type, false) { FREE_PARAMETER_SIZE = free_parameter_size; more_initialised_feature_measurement_model = m_i_f_m_m; ypiRES = new Vector(FEATURE_STATE_SIZE); dypi_by_dxpRES = new MatrixFixed(FEATURE_STATE_SIZE, motion_model.POSITION_STATE_SIZE); dypi_by_dhiRES = new MatrixFixed(FEATURE_STATE_SIZE, MEASUREMENT_SIZE); hpiRES = new Vector(MEASUREMENT_SIZE); dhpi_by_dxpRES = new MatrixFixed(MEASUREMENT_SIZE, motion_model.POSITION_STATE_SIZE); dhpi_by_dyiRES = new MatrixFixed(MEASUREMENT_SIZE, FEATURE_STATE_SIZE); yfiRES = new Vector(more_initialised_feature_measurement_model.FEATURE_STATE_SIZE); dyfi_by_dypiRES = new MatrixFixed(more_initialised_feature_measurement_model.FEATURE_STATE_SIZE, FEATURE_STATE_SIZE); dyfi_by_dlambdaRES = new MatrixFixed(more_initialised_feature_measurement_model.FEATURE_STATE_SIZE, FREE_PARAMETER_SIZE); }