/// <summary>
/// returns the closest two neighbours to the given feature
/// </summary>
/// <param name="feat"></param>
/// <param name="neighbour1"></param>
/// <param name="neighbour2"></param>
private void closest_neighbours(Feature feat, ref Feature neighbour1, ref Feature neighbour2)
{
int i, x, y;
int dist, min_dist;
x = (int)feat.get_h()[0];
y = (int)feat.get_h()[1];
neighbour1 = null;
neighbour2 = null;
i = 0;
min_dist = 9999;
while (i < features.Count)
{
Feature f = (Feature)features[i];
if (f != feat)
{
if (featureVisible(f))
{
Vector screen_position = f.get_h();
dist = (int)Math.Abs(screen_position[0] - x);
dist += (int)Math.Abs(screen_position[1] - y);
if (dist < min_dist)
{
min_dist = dist;
neighbour2 = neighbour1;
neighbour1 = f;
}
}
}
i++;
}
}
public void update(Scene_Single scene)
{
this.scene = scene;
if (features_pending.Count > 0)
{
bool found = false;
int i = 0;
while ((i < features_pending.Count) && (!found))
{
Feature f = (Feature)features_pending[i];
Vector pos = f.get_h();
if ((pos[0] != 0) || (pos[1] != 0))
{
if (featureVisible(f))
{
addFeature(f);
//found = true;
}
features_pending.Remove(f);
}
i++;
}
}
}
/// <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);
}
/// <summary>
/// is the given feature inside the triangle ?
/// </summary>
/// <param name="feat"></param>
/// <returns></returns>
public bool isInside(Feature feat)
{
Vector screen_position = feat.get_h();
return(isInside((int)screen_position[0], (int)screen_position[1]));
}
