public linefeature(Feature feature1, Feature feature2, int no_of_points, int history) { marked_for_deletion = false; curr_index = 0; hits = 0; this.feature1 = feature1; this.feature2 = feature2; this.no_of_points = no_of_points; this.history = history; pixel_intensity = new Byte[no_of_points, history]; feature_position = new Vector[history, 2]; for (int i = 0; i < history; i++) { feature_position[i, 0] = new Vector(3); feature_position[i, 1] = new Vector(3); } }
/// <summary> /// For a single feature, work out the predicted feature measurement and the /// Jacobians with respect to the vehicle position and the feature position. /// This calls the appropriate member functions in Feature to set the measurement /// \vct{h}, the feature location Jacobian \partfracv{h}{y}, robot /// position Jacobian \partfrac{\vct{h}}{\vct{x}_v}, measurement covariance /// \mat{R} and innovation covariance \mat{S} respectively. /// </summary> /// <param name="sfp"></param> public void predict_single_feature_measurements(Feature sfp) { motion_model.func_xp(xv); Vector xpRES = motion_model.get_xpRES(); sfp.get_fully_initialised_feature_measurement_model().func_hi_and_dhi_by_dxp_and_dhi_by_dyi(sfp.get_y(),xpRES); sfp.set_h(sfp.get_fully_initialised_feature_measurement_model().get_hiRES()); sfp.set_dh_by_dy(sfp.get_fully_initialised_feature_measurement_model().get_dhi_by_dyiRES()); motion_model.func_dxp_by_dxv(xv); sfp.set_dh_by_dxv(sfp.get_fully_initialised_feature_measurement_model().get_dhi_by_dxpRES() * motion_model.get_dxp_by_dxvRES()); sfp.get_fully_initialised_feature_measurement_model().func_Ri(sfp.get_h()); sfp.set_R(sfp.get_fully_initialised_feature_measurement_model().get_RiRES()); sfp.get_fully_initialised_feature_measurement_model().func_Si(Pxx, sfp.get_Pxy(), sfp.get_Pyy(), sfp.get_dh_by_dxv(), sfp.get_dh_by_dy(), sfp.get_R()); sfp.set_S(sfp.get_fully_initialised_feature_measurement_model().get_SiRES()); }
public void successful_measurement_of_feature(Feature sfp) { sfp.set_successful_measurement_flag(true); successful_measurement_vector_size += sfp.get_fully_initialised_feature_measurement_model().MEASUREMENT_SIZE; sfp.get_fully_initialised_feature_measurement_model().func_nui(sfp.get_h(), sfp.get_z()); sfp.set_nu(sfp.get_fully_initialised_feature_measurement_model().get_nuiRES()); sfp.increment_successful_measurements_of_feature(1); sfp.increment_attempted_measurements_of_feature(1); }
public void failed_measurement_of_feature(Feature sfp) { //if (STATUSDUMP) cout << "Measurement failed of feature with label " << sfp.label << endl; sfp.set_successful_measurement_flag(false); sfp.increment_attempted_measurements_of_feature(1); }
public FeatureAndScore(float s, Feature f) { score = s; fp = f; }
/// <summary> /// Remove this feature from the list for selection. /// </summary> /// <param name="fp">The feature to remove.</param> /// <returns></returns> public bool deselect_feature(Feature fp) { if (fp.selected_flag == false) { //if (DEBUGDUMP) cout << "Feature with label " << fp.label // << " is already deselected." << endl; } fp.set_selected_flag(false); selected_feature_list.Remove(fp); return true; }
//*******************Feature Selection and Measurement************************ /// <summary> /// Add this feature to the list for selection. /// </summary> /// <param name="fp">The feature to add.</param> /// <returns></returns> public bool select_feature(Feature fp) { if (fp.get_selected_flag() == true) { //if (DEBUGDUMP) cout << "Feature with label " << fp.get_label() //<< " is already selected." << endl; return true; } fp.set_selected_flag(true); selected_feature_list.Add(fp); return true; }
/// <summary> /// Function to unite the bookeeping stuff for both /// add_new_partially_initialised_feature() and add_new_known_feature() /// </summary> /// <param name="nf"></param> public void add_new_feature_bookeeping(Feature nf) { feature_list.Add(nf); next_free_label++; // Potential millenium-style bug when this overloads // (hello if you're reading this in the year 3000) nf.set_position_in_total_state_vector(total_state_size); total_state_size += nf.get_feature_measurement_model().FEATURE_STATE_SIZE; }
public void add_new_known_feature(classimage_mono id, Vector y_known, Vector xp_o, Feature_Measurement_Model f_m_m, uint known_feature_label) { Feature nf = new Feature(id, next_free_label, (uint)feature_list.Count, this, y_known, xp_o, f_m_m, known_feature_label); add_new_feature_bookeeping(nf); }
/// <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); }
public void set_fp(Feature new_fp) { fp = new_fp; }
//typedef Vector<Particle> as ParticleVector; /// <summary> /// Constructor. The class is intialised with no particles, so these must be added later using add_particle() /// </summary> /// <param name="sc">The Scene class.</param> /// <param name="f">The feature for which this class represents additional information.</param> public FeatureInitInfo(Scene_Single sc, Feature f) { scene = sc; fp = f; PARTICLE_DIMENSION = f.get_partially_initialised_feature_measurement_model().FREE_PARAMETER_SIZE; number_of_match_attempts = 0; nonzerovariance = false; mean = new Vector(PARTICLE_DIMENSION); covariance = new MatrixFixed(PARTICLE_DIMENSION, PARTICLE_DIMENSION); //if (Scene_Single::STATUSDUMP) cout << "PARTICLE_DIMENSION = " //<< PARTICLE_DIMENSION << endl; }