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;
}
