/// <summary> /// Fast version of predict filter /// </summary> /// <param name="scene"></param> /// <param name="u">Control vector of accelerations</param> /// <param name="delta_t">time step in seconds</param> public void predict_filter_fast(Scene_Single scene, Vector u, float delta_t) { Vector xv = scene.get_xv(); // Make model calculations: results are stored in RES matrices scene.get_motion_model().func_fv_and_dfv_by_dxv(xv, u, delta_t); scene.get_motion_model().func_Q(xv, u, delta_t); Vector fvRES = scene.get_motion_model().get_fvRES(); xv.Update(fvRES); Motion_Model mm = scene.get_motion_model(); MatrixFixed Pxx = scene.get_Pxx(); MatrixFixed dfv_by_dxvRES = mm.get_dfv_by_dxvRES(); MatrixFixed dfv_by_dxvRES_transposed = dfv_by_dxvRES.Transpose(); MatrixFixed QxRES = mm.get_QxRES(); Pxx.Update((dfv_by_dxvRES * Pxx * dfv_by_dxvRES_transposed) + QxRES); // Change the covariances between vehicle state and feature states foreach (Feature it in scene.feature_list) { MatrixFixed Pxy = it.get_Pxy(); Pxy.Update(dfv_by_dxvRES * Pxy); } }
/// <summary> /// Constructor /// </summary> /// <param name="motion_model">The motion model describing the robot motion. This must be derived from ThreeD_Motion_Model</param> /// <param name="cam">The camera model to use</param> /// <param name="MAXIMUM_LENGTH_RATIO_"></param> /// <param name="MAXIMUM_ANGLE_DIFFERENCE_"></param> /// <param name="IMAGE_SEARCH_BOUNDARY_"></param> /// <param name="SD_IMAGE_SIMULATION_"></param> public Wide_Camera_Point_Feature_Measurement_Model( Motion_Model motion_model, WideCamera cam, float MAXIMUM_LENGTH_RATIO_, float MAXIMUM_ANGLE_DIFFERENCE_, float IMAGE_SEARCH_BOUNDARY_, float SD_IMAGE_SIMULATION_) { MAXIMUM_LENGTH_RATIO = MAXIMUM_LENGTH_RATIO_; MAXIMUM_ANGLE_DIFFERENCE = MAXIMUM_ANGLE_DIFFERENCE_; IMAGE_SEARCH_BOUNDARY = IMAGE_SEARCH_BOUNDARY_; SD_IMAGE_SIMULATION = SD_IMAGE_SIMULATION_; m_camera = cam; if ((String)(motion_model.motion_model_dimensionality_type) != "THREED") { String os; os = "Motion Model given to the Feature Measurement Model is of type " + motion_model.motion_model_dimensionality_type + ", not type THREED."; Debug.WriteLine(os); //throw Scene::InitialisationError(os.str()); } threed_motion_model = (ThreeD_Motion_Model)motion_model; }
/// <summary> /// Returns an instance of the requested feature measurement model (using new). /// Returns null if there was no match. /// </summary> /// <param name="type"></param> /// <param name="motion_model"></param> /// <returns></returns> public override Feature_Measurement_Model create_model(String type, Motion_Model motion_model, float MAXIMUM_ANGLE_DIFFERENCE) { // Try creating each model that we can and see if the name is the same // std::cout << "Creating a " << type << " feature measurement model" << std::endl; Feature_Measurement_Model pModel; pModel = new Fully_Init_Wide_Point_Feature_Measurement_Model(motion_model, MAXIMUM_ANGLE_DIFFERENCE); if (pModel.feature_type == type) { return(pModel); } // The partially-initialised model also needs the fully-initalised // one that we have just created. Don't delete it - just store it Feature_Measurement_Model pFullModel = pModel; pModel = new Line_Init_Wide_Point_Feature_Measurement_Model(motion_model, pFullModel); if (pModel.feature_type == type) { return(pModel); } else { pModel = null; pFullModel = null; } return(null); }
/// <summary> /// Model for single camera making point measurements /// </summary> /// <param name="motion_model">The motion model describing the robot motion. This must be derived from ThreeD_Motion_Model</param> /// <param name="cam">The camera model to use</param> /// <param name="MAXIMUM_LENGTH_RATIO_"></param> /// <param name="MAXIMUM_ANGLE_DIFFERENCE_"></param> /// <param name="IMAGE_SEARCH_BOUNDARY_"></param> /// <param name="SD_IMAGE_SIMULATION_"></param> public Fully_Init_Wide_Point_Feature_Measurement_Model( Motion_Model motion_model, WideCamera cam, float MAXIMUM_LENGTH_RATIO_, float MAXIMUM_ANGLE_DIFFERENCE_, float IMAGE_SEARCH_BOUNDARY_, float SD_IMAGE_SIMULATION_) : base(2, 3, 3, motion_model, "CAMERA_WIDE_POINT", "THREED_POINT") { wide_model = new Wide_Camera_Point_Feature_Measurement_Model(motion_model, cam, MAXIMUM_LENGTH_RATIO_, MAXIMUM_ANGLE_DIFFERENCE_, IMAGE_SEARCH_BOUNDARY_, SD_IMAGE_SIMULATION_); }
/// <summary> /// Constructor /// </summary> /// <param name="motion_model">The motion model describing the robot motion. This must be derived from ThreeD_Motion_Model</param> /// <param name="fully_init_f_m_m">The type of feature measurement model to convert to when the feature is fully initialised.</param> /// <param name="cam">The camera model to use</param> /// <param name="MAXIMUM_LENGTH_RATIO_"></param> /// <param name="MAXIMUM_ANGLE_DIFFERENCE_"></param> /// <param name="IMAGE_SEARCH_BOUNDARY_"></param> /// <param name="SD_IMAGE_SIMULATION_"></param> public Line_Init_Wide_Point_Feature_Measurement_Model(Motion_Model motion_model, Feature_Measurement_Model fully_init_f_m_m, WideCamera cam, float MAXIMUM_LENGTH_RATIO_, float MAXIMUM_ANGLE_DIFFERENCE_, float IMAGE_SEARCH_BOUNDARY_, float SD_IMAGE_SIMULATION_) : base(2, 6, 6, motion_model, "CAMERA_WIDE_LINE_INIT", "THREED_SEMI_INFINITE_LINE", 1, fully_init_f_m_m) { wide_model = new Wide_Camera_Point_Feature_Measurement_Model(motion_model, cam, MAXIMUM_LENGTH_RATIO_, MAXIMUM_ANGLE_DIFFERENCE_, IMAGE_SEARCH_BOUNDARY_, SD_IMAGE_SIMULATION_); }
public Fully_Init_Wide_Point_Feature_Measurement_Model(Motion_Model motion_model, float WIDE_MAXIMUM_ANGLE_DIFFERENCE) : base(2, 3, 3, motion_model, "CAMERA_WIDE_POINT", "THREED_POINT") { WideCamera cam = new WideCamera(); float MAXIMUM_LENGTH_RATIO_ = Camera_Constants.WIDE_MAXIMUM_LENGTH_RATIO; float MAXIMUM_ANGLE_DIFFERENCE_ = WIDE_MAXIMUM_ANGLE_DIFFERENCE; //Camera_Constants.WIDE_MAXIMUM_ANGLE_DIFFERENCE; float IMAGE_SEARCH_BOUNDARY_ = Camera_Constants.WIDE_IMAGE_SEARCH_BOUNDARY; float SD_IMAGE_SIMULATION_ = Camera_Constants.WIDE_SD_IMAGE_SIMULATION; wide_model = new Wide_Camera_Point_Feature_Measurement_Model(motion_model, cam, MAXIMUM_LENGTH_RATIO_, MAXIMUM_ANGLE_DIFFERENCE_, IMAGE_SEARCH_BOUNDARY_, SD_IMAGE_SIMULATION_); }