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