/// <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); }
/// <summary> /// Constructor for known features. The different number of /// arguments differentiates it from the constructor for partially-initialised /// features /// </summary> /// <param name="id">reference to the feature identifier</param> /// <param name="?"></param> public Feature(byte[] id, uint lab, uint list_pos, Scene_Single scene, Vector y_known, Vector xp_o, Feature_Measurement_Model f_m_m, uint k_f_l) { feature_measurement_model = f_m_m; feature_constructor_bookeeping(); identifier = id; label = lab; position_in_list = list_pos; // Position of new feature in list // Save the vehicle position where this feature was acquired xp_orig = new Vector(xp_o); // Straighforward initialisation of state and covariances y = y_known; Pxy = new MatrixFixed(scene.get_motion_model().STATE_SIZE, feature_measurement_model.FEATURE_STATE_SIZE); Pxy.Fill(0.0f); Pyy = new MatrixFixed(feature_measurement_model.FEATURE_STATE_SIZE, feature_measurement_model.FEATURE_STATE_SIZE); Pyy.Fill(0.0f); int i = 0; MatrixFixed newPyjyi_to_store; foreach (Feature it in scene.get_feature_list_noconst()) { if (i < position_in_list) { newPyjyi_to_store = new MatrixFixed( it.get_feature_measurement_model().FEATURE_STATE_SIZE, feature_measurement_model.FEATURE_STATE_SIZE); //add to the list matrix_block_list.Add(newPyjyi_to_store); } i++; } known_feature_label = (int)k_f_l; if (feature_measurement_model.fully_initialised_flag) { partially_initialised_feature_measurement_model = null; fully_initialised_feature_measurement_model = (Fully_Initialised_Feature_Measurement_Model)feature_measurement_model; } else { fully_initialised_feature_measurement_model = null; partially_initialised_feature_measurement_model = (Partially_Initialised_Feature_Measurement_Model)feature_measurement_model; } }
/// <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="fully_init_f_m_m">The type of feature measurement model to convert to when the feature is fully initialised.</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 Line_Init_Wide_Point_Feature_Measurement_Model(Motion_Model motion_model, Feature_Measurement_Model fully_init_f_m_m, WideCamera cam, float MAXIMUM_LENGTH_RATIO_, float MAXIMUM_ANGLE_DIFFERENCE_, float IMAGE_SEARCH_BOUNDARY_, float SD_IMAGE_SIMULATION_) : base(2, 6, 6, motion_model, "CAMERA_WIDE_LINE_INIT", "THREED_SEMI_INFINITE_LINE", 1, fully_init_f_m_m) { 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> /// Initialise a known feature. In this case it is assumed that a known feature /// is an image patch to be loaded from a file <code>known_patch?.bmp</code>. /// </summary> /// <param name="fmm"></param> /// <param name="v"></param> /// <param name="known_feature_label"></param> /// <returns></returns> public override classimage_mono initialise_known_feature(Feature_Measurement_Model fmm, Vector v, uint known_feature_label, String path) { String name = Camera_Constants.known_point_patch_stem + known_feature_label + ".bmp"; classimage_mono patch = new classimage_mono(); if (!patch.loadFromBitmapMono(path + name, (int)Camera_Constants.BOXSIZE, (int)Camera_Constants.BOXSIZE)) { patch = null; } return(patch); }
/// <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); }
/// <summary> /// Convert a partially-initialised feature to a fully-initialised feature, /// given information about the free parameters \vct{\lambda}. /// The new state \vct{y}_{fi} is given by calling /// Partially_Initialised_Feature_Measurement_Model::func_yfi_and_dyfi_by_dypi_and_dyfi_by_dlambda(). /// where the various Jacobians are returned by calls to /// Partially_Initialised_Feature_Measurement_Model, and the covariance matrices /// \mat{P}_{kl} are already known and stored in the class, except for /// \mat{P}_{\vct{\lambda}}, which is passed to the function. /// </summary> /// <param name="lambda">The mean value for \vct{\lambda}</param> /// <param name="Plambda">The covariance for \vct{\lambda}</param> /// <param name="scene">The SLAM map</param> public void convert_from_partially_to_fully_initialised( Vector lambda, MatrixFixed Plambda, Scene_Single scene) { // We'll do all the work here in feature.cc though probably this only // works with scene_single... // We calculate new state yfi(ypi, lambda) // New feature covariance // Pyfiyfi = dyfi_by_dypi Pypiypi dyfi_by_dypiT + // dyfi_by_dlambda Plambda dyfi_by_dlambdaT // And we change cross covariances as follows: // Pxyfi = Pxypi dyfi_by_dypiT // Pyjyfi = Pyjypi dyfi_by_dypiT for j < i (since we only store top-right // Pyfiyj = dyfi_by_dypi Pypiyj for j > i part of covariance matrix) partially_initialised_feature_measurement_model.func_yfi_and_dyfi_by_dypi_and_dyfi_by_dlambda(y, lambda); MatrixFixed dyfi_by_dypiT = partially_initialised_feature_measurement_model.get_dyfi_by_dypiRES().Transpose(); MatrixFixed dyfi_by_dlambdaT = partially_initialised_feature_measurement_model.get_dyfi_by_dlambdaRES().Transpose(); // Replace y y = new Vector(partially_initialised_feature_measurement_model.get_yfiRES()); // Replace Pxy Pxy = Pxy * dyfi_by_dypiT; // Replace Pyy MatrixFixed Pypiypi_1 = partially_initialised_feature_measurement_model.get_dyfi_by_dypiRES() * Pyy * dyfi_by_dypiT; MatrixFixed Pypiypi_2 = partially_initialised_feature_measurement_model.get_dyfi_by_dlambdaRES() * Plambda * dyfi_by_dlambdaT; Pyy = Pypiypi_1 + Pypiypi_2; // Pyjyi elements for j < i (covariance with features before i in list) uint i = position_in_list; MatrixFixed m_it; int j; for (j = 0; j < position_in_list; j++) { m_it = (MatrixFixed)matrix_block_list[j]; matrix_block_list[j] = m_it * dyfi_by_dypiT; } Feature it; int idx = scene.feature_list.IndexOf(this); for (j = idx + 1; j < scene.feature_list.Count; j++) { it = (Feature)(scene.feature_list[j]); it.matrix_block_list[(int)i] = partially_initialised_feature_measurement_model.get_dyfi_by_dypiRES() * (MatrixFixed)it.matrix_block_list[(int)i]; it.increment_position_in_total_state_vector(-(int)feature_measurement_model.FEATURE_STATE_SIZE); } // Change the total state size in scene, here with a negative increment uint size1 = partially_initialised_feature_measurement_model.more_initialised_feature_measurement_model.FEATURE_STATE_SIZE; uint size2 = partially_initialised_feature_measurement_model.FEATURE_STATE_SIZE; scene.increment_total_state_size((int)size1 - (int)size2); // Change fmm for this model to fully-initialised one feature_measurement_model = partially_initialised_feature_measurement_model.more_initialised_feature_measurement_model; partially_initialised_feature_measurement_model = null; fully_initialised_feature_measurement_model = (Fully_Initialised_Feature_Measurement_Model)feature_measurement_model; //assert(fully_initialised_feature_measurement_model != NULL); // Need to reallocate any other matrices // Assume that measurement size doesn't change dh_by_dy.Resize(feature_measurement_model.MEASUREMENT_SIZE, feature_measurement_model.FEATURE_STATE_SIZE); }
/// <summary> /// Constructor for partially-initialised features. /// </summary> /// <param name="id">reference to the feature</param> /// <param name="lab"></param> /// <param name="list_pos"></param> /// <param name="scene"></param> /// <param name="h"></param> /// <param name="p_i_f_m_m"></param> public Feature(byte[] id, uint lab, uint list_pos, Scene_Single scene, Vector h, Partially_Initialised_Feature_Measurement_Model p_i_f_m_m, Vector feature_colour) { feature_measurement_model = p_i_f_m_m; partially_initialised_feature_measurement_model = p_i_f_m_m; fully_initialised_feature_measurement_model = null; // Stuff below substituted from Feature_common // Feature_common(id, lab, list_pos, scene, h); feature_constructor_bookeeping(); identifier = id; label = lab; position_in_list = list_pos; // Position of new feature in list position_in_total_state_vector = 0; // This should be set properly colour = feature_colour; //when feature is added // Save the vehicle position where this feature was acquired scene.get_motion_model().func_xp(scene.get_xv()); //xp_orig = scene.get_motion_model().get_xpRES(); xp_orig = new Vector(scene.get_motion_model().get_xpRES()); // Call model functions to calculate feature state, measurement noise // and associated Jacobians. Results are stored in RES matrices // First calculate "position state" and Jacobian scene.get_motion_model().func_xp(scene.get_xv()); scene.get_motion_model().func_dxp_by_dxv(scene.get_xv()); // Now ask the model to initialise the state vector and calculate Jacobians // so that I can go and calculate the covariance matrices partially_initialised_feature_measurement_model.func_ypi_and_dypi_by_dxp_and_dypi_by_dhi_and_Ri(h, scene.get_motion_model().get_xpRES()); // State y //y = partially_initialised_feature_measurement_model.get_ypiRES(); y = new Vector(partially_initialised_feature_measurement_model.get_ypiRES()); // Temp_FS1 will store dypi_by_dxv MatrixFixed Temp_FS1 = partially_initialised_feature_measurement_model.get_dypi_by_dxpRES() * scene.get_motion_model().get_dxp_by_dxvRES(); // Pxy Pxy = scene.get_Pxx() * Temp_FS1.Transpose(); // Pyy Pyy = Temp_FS1 * scene.get_Pxx() * Temp_FS1.Transpose() + partially_initialised_feature_measurement_model.get_dypi_by_dhiRES() * partially_initialised_feature_measurement_model.get_RiRES() * partially_initialised_feature_measurement_model.get_dypi_by_dhiRES().Transpose(); // Covariances of this feature with others int j = 0; foreach (Feature it in scene.get_feature_list_noconst()) { if (j < position_in_list) { // new Pypiyj = dypi_by_dxv . Pxyj // Size of this is FEATURE_STATE_SIZE(new) by FEATURE_STATE_SIZE(old) MatrixFixed m = it.get_Pxy(); MatrixFixed newPyjypi_to_store = (Temp_FS1 * m).Transpose(); //add to the list matrix_block_list.Add(newPyjypi_to_store); } j++; } known_feature_label = -1; }
/**************************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 classimage_mono 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); }
public virtual classimage_mono initialise_known_feature(Feature_Measurement_Model f_m_m, Vector yi, Settings.Section section, String path) { return(null); }
public virtual classimage_mono initialise_known_feature(Feature_Measurement_Model f_m_m, Vector yi, uint known_feature_label, String path) { return(null); }