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