/// <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>
/// Try to match the partially-initialised features, then update their
/// distributions. If possible (if the standard deviation ratio \Sigma_{11} /
/// \mu_1 is small enough) this also converts them to fully-initalised
/// features. In addition, this also calls
/// Scene_Single::delete_partially_initialised_features_past_sell_by_date() to
/// delete any that have not collapses fast enough.
/// </summary>
public void MatchPartiallyInitialisedFeatures()
{
// Go through all partially initialised features and decide which ones
// to try to measure; predict measurements for these
scene.predict_partially_initialised_feature_measurements();
//if (Camera_Constants.DEBUGDUMP) cout << "Match: time after setting up search stuff "
// << timer1 << endl;
// Loop through partially initialised features
foreach (FeatureInitInfo feat in scene.get_feature_init_info_vector_noconst())
{
if (feat.get_making_measurement_on_this_step_flag())
{
// Making partially initialised measurements
// Make search for this feature over overlapping ellipses of particles
robot.measure_feature_with_multiple_priors(feat.get_fp().get_identifier(), feat.get_particle_vector_noconst());
}
}
// Update particle distributions with measurements, normalise, prune, etc.
scene.update_partially_initialised_feature_probabilities(PRUNE_PROBABILITY_THRESHOLD);
// Now go through the features again and decide whether to convert
// any to fully-initialised features
// Do this here rather than generically in Scene because the test
// for conversion is specific to this application
for (int i = 0; i < scene.get_feature_init_info_vector_noconst().Count; i++)
{
FeatureInitInfo feat = (FeatureInitInfo)scene.get_feature_init_info_vector_noconst()[i];
// Only try to convert if we've been making measurements!
if (feat.get_making_measurement_on_this_step_flag())
{
float mean_sd_ratio = (float)Math.Sqrt(feat.get_covariance()[0, 0]) / feat.get_mean()[0];
// Checking whether to convert feature
int no_of_particles = feat.get_particle_vector().Count;
if ((feat.nonzerovariance) &&
(mean_sd_ratio < STANDARD_DEVIATION_DEPTH_RATIO) &&
(no_of_particles > MIN_NUMBER_OF_PARTICLES))
{
if (feat != range_feature)
{
// Yes, converting feature
feat.get_fp_noconst().convert_from_partially_to_fully_initialised(
feat.get_mean(), feat.get_covariance(), scene);
scene.get_feature_init_info_vector_noconst().Remove(feat);
i--;
}
else
{
// take the feature's position only
feat.get_fp_noconst().update_feature_position(feat.get_mean());
feat.number_of_match_attempts = ERASE_PARTIALLY_INIT_FEATURE_AFTER_THIS_MANY_ATTEMPTS + 1;
ranged_features.Add(feat.get_fp_noconst()); // TODO: does this get too large?
range_feature_ctr = 0;
range_feature = null;
}
}
}
}
if (range_feature != null)
{
range_feature_ctr++;
if (range_feature_ctr > ERASE_PARTIALLY_INIT_FEATURE_AFTER_THIS_MANY_ATTEMPTS)
{
range_feature_ctr = 0;
range_feature.number_of_match_attempts = ERASE_PARTIALLY_INIT_FEATURE_AFTER_THIS_MANY_ATTEMPTS + 1;
range_feature = null;
}
}
// Go through and get rid of any partially initialised features whose
// distributions haven't collapsed fast enough
scene.delete_partially_initialised_features_past_sell_by_date(
ERASE_PARTIALLY_INIT_FEATURE_AFTER_THIS_MANY_ATTEMPTS,
MIN_NUMBER_OF_PARTICLES);
}
public void delete_partially_initialised_feature(FeatureInitInfo feat)
{
//feat.get_fp().get_identifier();
// Save currently marked feature
int currently_marked_feature = get_marked_feature_label();
// Mark feature to delete
mark_feature_by_lab((int)feat.get_fp().get_label());
delete_feature();
feature_init_info_vector.Remove(feat);
if (currently_marked_feature != -1)
mark_feature_by_lab(currently_marked_feature);
}
/// <summary>
/// Initialise a range feature, used to build depth maps, at a position determined
/// automatically. This predicts where the image centre will be soon (in 10 frames),
/// and tries initialising a feature near there. This may not necessarily give a
/// new feature - the score could not be good enough, or no suitable non-overlapping
/// region might be found.
/// </summary>
/// <returns>true on success, or false on failure (i.e. if no non-overlapping region is found, or if no feature could be found with a good enough score).</returns>
public bool AutoInitialiseRangeFeature()
{
// A cute method for deciding where to centre our search for a new feature
// Idea: look for a point in a position that we expect to be near the
// image centre soon
// First find a suitable patch
float SUITABLE_PATCH_SCORE_THRESHOLD = 15000;
Vector local_u = new Vector(scene.get_motion_model().CONTROL_SIZE);
// randomly pick an area within which to search
int w = robot.image_width / 6;
int h = robot.image_height / 6;
int wdth = robot.image_width - (int)(Camera_Constants.BOXSIZE * 2) - w;
int hght = robot.image_height - (int)(Camera_Constants.BOXSIZE * 2) - h;
init_feature_search_ustart = (int)Camera_Constants.BOXSIZE + rnd.Next(wdth);
init_feature_search_ufinish = init_feature_search_ustart + w;
init_feature_search_vstart = (int)Camera_Constants.BOXSIZE + rnd.Next(hght);
init_feature_search_vfinish = init_feature_search_vfinish + h;
init_feature_search_region_defined_flag = true;
if (robot.set_image_selection_automatically(
(uint)init_feature_search_ustart, (uint)init_feature_search_vstart,
(uint)init_feature_search_ufinish, (uint)init_feature_search_vfinish) >
SUITABLE_PATCH_SCORE_THRESHOLD)
{
// Then initialise it
range_feature = InitialiseFeature();
}
else
{
// Score not good enough; feature not initialised.
return false;
}
return true;
}
