/// <summary>
/// project a feature from 3D into 2D image coordinates
/// </summary>
/// <param name="feat"></param>
public void projectFeature(Feature feat, ref int screen_x, ref int screen_y)
{
Vector yi = feat.get_y();
Vector xp = scene.get_motion_model().get_xpRES();
Fully_Initialised_Feature_Measurement_Model fully_init_fmm =
feat.get_fully_initialised_feature_measurement_model();
fully_init_fmm.func_hi_and_dhi_by_dxp_and_dhi_by_dyi(yi, xp);
screen_x = (int)fully_init_fmm.get_hiRES()[0];
screen_y = (int)fully_init_fmm.get_hiRES()[1];
}
/// <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);
}
