/// <summary> /// Make the first measurement of a feature. /// </summary> /// <param name="z">The location of the feature to measure</param> /// <param name="id">The Identifier to fill with the feature measurements</param> /// <param name="f_m_m">The feature measurement model to use for this partially-initialised feature</param> /// <returns></returns> public virtual bool make_initial_measurement_of_feature(Vector z, ref classimage_mono id, Partially_Initialised_Feature_Measurement_Model f_m_m, Vector patch_colour) { return (false); }
/// <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(classimage_mono 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> /// 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(classimage_mono 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; }
/// <summary> /// Create a new partially-initialised feature. This creates a new Feature, and /// creates a new empty FeatureInitInfo to store the extra initialisation /// information, which must be then filled in by the caller of this function. /// </summary> /// <param name="id">The unique identifier for this feature (e.g. the image patch)</param> /// <param name="h">The initial measured state for this feature (e.g. the image location)</param> /// <param name="f_m_m">The partially-initialised feature measurement model to apply to this feature.</param> /// <returns>A pointer to the FeatureInitInfo object to be filled in with further initialisation information.</returns> public FeatureInitInfo add_new_partially_initialised_feature(classimage_mono id, Vector h, Partially_Initialised_Feature_Measurement_Model f_m_m, Vector feature_colour) { Feature nf = new Feature(id, next_free_label, (uint)feature_list.Count, this, h, f_m_m, feature_colour); add_new_feature_bookeeping(nf); // Set stuff to store extra probability information for partially // initialised feature FeatureInitInfo feat = new FeatureInitInfo(this, nf); feature_init_info_vector.Add(feat); return (feat); }
/// <summary> /// Make the initial measurement of the currently-selected feature. This fills in /// the location and the identifier (a copy of the image patch) for the current /// feature. The feature location is set using set_image_selection_automatically() /// or manually by the user using set_image_selection(). This function just calls /// partially_initialise_point_feature() to fill in the measurement and identifier. /// </summary> /// <param name="z">The measurement vector to be filled in.</param> /// <param name="id_ptr">The identifier to be filled in.</param> /// <param name="m"></param> /// <returns>true on success, or <code>false</code> on failure (e.g. no patch is currently selected).</returns> public override bool make_initial_measurement_of_feature(Vector z, ref byte[] patch, Partially_Initialised_Feature_Measurement_Model m, Vector patch_colour) { patch = partially_initialise_point_feature(z); for (int c = 0; c < 3; c++) patch_colour[c] = image_colour.image[uu, vv, c]; if (patch != null) return true; else return false; }
/// <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); } }
// Function to find a region in an image guided by current motion prediction // which doesn't overlap existing features public static bool FindNonOverlappingRegion(Scene_Single scene, Vector local_u, float delta_t, Partially_Initialised_Feature_Measurement_Model default_feature_type_for_initialisation, uint camera_width, uint camera_height, uint BOXSIZE, ref int init_feature_search_ustart, ref int init_feature_search_vstart, ref int init_feature_search_ufinish, ref int init_feature_search_vfinish, uint FEATURE_INIT_STEPS_TO_PREDICT, float FEATURE_INIT_DEPTH_HYPOTHESIS, Random rnd) { ThreeD_Motion_Model threed_motion_model = (ThreeD_Motion_Model)scene.get_motion_model(); Vector local_xv = new Vector(scene.get_xv()); for (uint i = 0; i < FEATURE_INIT_STEPS_TO_PREDICT; i++) { scene.get_motion_model().func_fv_and_dfv_by_dxv(local_xv, local_u, delta_t); local_xv.Update(scene.get_motion_model().get_fvRES()); } threed_motion_model.func_xp(local_xv); Vector local_xp = new Vector(threed_motion_model.get_xpRES()); threed_motion_model.func_r(local_xp); Vector3D rW = threed_motion_model.get_rRES(); threed_motion_model.func_q(local_xp); Quaternion qWR = threed_motion_model.get_qRES(); // yW = rW + RWR hR Vector3D hR = new Vector3D(0.0f, 0.0f, FEATURE_INIT_DEPTH_HYPOTHESIS); // Used Inverse + transpose because this was't compiling the normal way Vector3D yW = new Vector3D(rW + qWR.RotationMatrix() * hR); // Then project that point into the current camera position scene.get_motion_model().func_xp(scene.get_xv()); Fully_Initialised_Feature_Measurement_Model fully_init_fmm = (Fully_Initialised_Feature_Measurement_Model)(default_feature_type_for_initialisation.more_initialised_feature_measurement_model); Vector yWVNL = yW.GetVNL3(); fully_init_fmm.func_hi_and_dhi_by_dxp_and_dhi_by_dyi(yWVNL, scene.get_motion_model().get_xpRES()); // Now, this defines roughly how much we expect a feature initialised to move float suggested_u = fully_init_fmm.get_hiRES()[0]; float suggested_v = fully_init_fmm.get_hiRES()[1]; float predicted_motion_u = camera_width / 2.0f - suggested_u; float predicted_motion_v = camera_height / 2.0f - suggested_v; // So, the limits of a "safe" region within which we can initialise // features so that they end up staying within the screen // (Making the approximation that the whole screen moves like the // centre point) int safe_feature_search_ustart = (int)(-predicted_motion_u); int safe_feature_search_vstart = (int)(-predicted_motion_v); int safe_feature_search_ufinish = (int)(camera_width - predicted_motion_u); int safe_feature_search_vfinish = (int)(camera_height - predicted_motion_v); if (safe_feature_search_ustart < ((int)((BOXSIZE - 1) / 2) + 1)) safe_feature_search_ustart = (int)((BOXSIZE - 1) / 2 + 1); if (safe_feature_search_ufinish > (int)camera_width - ((int)((BOXSIZE - 1) / 2) + 1)) safe_feature_search_ufinish = (int)(camera_width - (BOXSIZE - 1) / 2 - 1); if (safe_feature_search_vstart < ((int)((BOXSIZE - 1) / 2) + 1)) safe_feature_search_vstart = (int)((BOXSIZE - 1) / 2 + 1); if (safe_feature_search_vfinish > (int)camera_height - ((int)((BOXSIZE - 1) / 2) + 1)) safe_feature_search_vfinish = (int)(camera_height - (BOXSIZE - 1) / 2 - 1); return FindNonOverlappingRegionNoPredict(safe_feature_search_ustart, safe_feature_search_vstart, safe_feature_search_ufinish, safe_feature_search_vfinish, scene, ref init_feature_search_ustart, ref init_feature_search_vstart, ref init_feature_search_ufinish, ref init_feature_search_vfinish, rnd); }