/// <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;
}
public Line_Init_Wide_Point_Feature_Measurement_Model(Motion_Model motion_model,
Feature_Measurement_Model fully_init_f_m_m) :
base(2, 6, 6, motion_model, "CAMERA_WIDE_LINE_INIT", "THREED_SEMI_INFINITE_LINE", 1, fully_init_f_m_m)
{
WideCamera cam = new WideCamera();
float MAXIMUM_LENGTH_RATIO_ = Camera_Constants.WIDE_MAXIMUM_LENGTH_RATIO;
float 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_);
}
/// <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_);
}
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_);
}
/// <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>
/// Use this version if using internal measurement models defined in settings file.
/// </summary>
/// <param name="mm_creator"></param>
/// <param name="imm_creator"></param>
public Scene_Single(Settings settings, Motion_Model_Creator mm_creator,
Internal_Measurement_Model_Creator imm_creator)
{
// What is the motion model?
ArrayList values = settings.get_entry("Models", "MotionModel");
String model = (String)values[0];
//assert(mm_creator != NULL);
motion_model = mm_creator.create_model(model);
if (motion_model == null)
{
Debug.WriteLine("Unable to create a motion model of type " + model +
" as requested in the initalisation file. ");
}
// Initialise the motion model with any settings
motion_model.read_parameters(settings);
// Create internal measurement models if required
if (imm_creator != null)
{
uint imm_number = 0;
// Read in and create potentially various internal measurement models
while (true)
{
// A list of internal measurement models will be given in the
// Models section of settings with entry names
// InternalMeasurementModel0, InternalMeasurementModel1, etc.
String entry_name = "InternalMeasurementModel" + Convert.ToString(imm_number);
values = settings.get_entry("Models", entry_name);
String new_imm_type = (String)values[0];
// Exit loop if no more internal measurement models
if (new_imm_type == "") break;
// Initialise new model and add to Scene
Internal_Measurement_Model new_imm = imm_creator.create_model(new_imm_type, motion_model);
// Note for future: at this stage we should potentially read in
// parameters for new imm; potentially we could have several internal
// measurements define which have the same model but different parameters
// (just like we have multiple features)
add_internal_measurement(new_imm);
imm_number++;
}
}
// Get the initial state settings
Vector initial_xv = null;
MatrixFixed initial_Pxx = null;
motion_model.read_initial_state(settings, ref initial_xv, ref initial_Pxx);
scene_constructor_bookkeeping(initial_xv, initial_Pxx);
}
/// <summary>
/// Old style constructor
/// Constuct the Scene_Single class and set the initial robot state.
/// </summary>
/// <param name="initial_xv">The initial robot state</param>
/// <param name="initial_Pxx">The initial robot state covariance</param>
/// <param name="m_m">The motion model to use</param>
public Scene_Single(Vector initial_xv,
MatrixFixed initial_Pxx,
Motion_Model m_m)
{
motion_model = m_m;
scene_constructor_bookkeeping(initial_xv, initial_Pxx);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="measurement_size">The number of parameters representing a measurement of the feature.</param>
/// <param name="feature_state_size">The number of parameters to repreent the state of the feature.</param>
/// <param name="graphics_state_size">The number of parameters to represent an abstraction of the feature for use in a graphical display.</param>
/// <param name="motion_model">The robot motion model. Needed to understand and use the robot position statex_pwhen measuring features.</param>
/// <param name="feature_type">The name for this feature measurement model (set by the derived class).</param>
/// <param name="feature_graphics_type">The name for this type of feature, so that it can be drawn (set by the derived class).</param>
public Fully_Initialised_Feature_Measurement_Model(
uint measurement_size,
uint feature_state_size,
uint graphics_state_size,
Motion_Model motion_model,
String feature_type,
String feature_graphics_type)
: base(measurement_size,
feature_state_size,
graphics_state_size,
motion_model,
feature_type,
feature_graphics_type, true)
{
hiRES = new Vector(MEASUREMENT_SIZE);
dhi_by_dxpRES = new MatrixFixed(MEASUREMENT_SIZE,
motion_model.POSITION_STATE_SIZE);
dhi_by_dyiRES = new MatrixFixed(MEASUREMENT_SIZE, FEATURE_STATE_SIZE);
nuiRES = new Vector(MEASUREMENT_SIZE);
hi_noisyRES = new Vector(MEASUREMENT_SIZE);
}
/// <summary>
/// Constructor.
/// </summary>
/// <param name="measurement_size">The number of parameters representing a measurement of the feature</param>
/// <param name="feature_state_size">The number of parameters to repreent the state of the feature.</param>
/// <param name="graphics_state_size">The number of parameters to represent an abstraction of the feature for use in a graphical display</param>
/// <param name="m_m">The robot motion model. Needed to understand and use the robot position statex_pwhen measuring features</param>
/// <param name="f_t">The name for this feature measurement model (set by the derived class).</param>
/// <param name="f_g_t">The name for this type of feature, so that it can be drawn (set by the derived class)</param>
/// <param name="f_i_f">Is this model for fully-initialised features? (if <code>false</code> it is for partially-initialised features.). Set by the derived class.</param>
public Feature_Measurement_Model(
uint measurement_size,
uint feature_state_size,
uint graphics_state_size,
Motion_Model m_m,
String f_t, String f_g_t,
bool f_i_f)
{
MEASUREMENT_SIZE = measurement_size;
FEATURE_STATE_SIZE = feature_state_size;
GRAPHICS_STATE_SIZE = graphics_state_size;
feature_type = f_t;
feature_graphics_type = f_g_t;
fully_initialised_flag = f_i_f;
_motion_model = m_m;
// Allocate matrices for storage of results
yigraphicsRES = new Vector(GRAPHICS_STATE_SIZE);
PyiyigraphicsRES = new MatrixFixed(GRAPHICS_STATE_SIZE, GRAPHICS_STATE_SIZE);
zeroedyigraphicsRES = new Vector(GRAPHICS_STATE_SIZE);
PzeroedyigraphicsRES = new MatrixFixed(GRAPHICS_STATE_SIZE, GRAPHICS_STATE_SIZE);
RiRES = new MatrixFixed(MEASUREMENT_SIZE, MEASUREMENT_SIZE);
SiRES = new MatrixFixed(MEASUREMENT_SIZE, MEASUREMENT_SIZE);
zeroedyiRES = new Vector(FEATURE_STATE_SIZE);
dzeroedyi_by_dxpRES = new MatrixFixed(FEATURE_STATE_SIZE,
get_motion_model().POSITION_STATE_SIZE);
dzeroedyi_by_dyiRES = new MatrixFixed(FEATURE_STATE_SIZE, FEATURE_STATE_SIZE);
}
/// Returns an instance of the requested internal measurement model
/// (usually by using the new operator. TODO: who
/// deletes them?
public virtual Internal_Measurement_Model create_model(String type,
Motion_Model motion_model) { return (null); }
public Internal_Measurement_Model(
uint measurement_size,
Motion_Model m_m,
String i_t)
{
MEASUREMENT_SIZE = measurement_size;
motion_model = m_m;
internal_type = i_t;
hvRES = new Vector(MEASUREMENT_SIZE);
dhv_by_dxvRES = new MatrixFixed(MEASUREMENT_SIZE, motion_model.STATE_SIZE);
RvRES = new MatrixFixed(MEASUREMENT_SIZE, MEASUREMENT_SIZE);
nuvRES = new Vector(MEASUREMENT_SIZE);
SvRES = new MatrixFixed(MEASUREMENT_SIZE, MEASUREMENT_SIZE);
hv_noisyRES = new Vector(MEASUREMENT_SIZE);
}
/// Returns an instance of the requested feature measurement model
/// (usually by using the new operator. TODO: who
/// deletes them?
public virtual Feature_Measurement_Model create_model(String type, Motion_Model motion_model, float MAXIMUM_ANGLE_DIFFERENCE) { return (null); }
/// <summary>
/// Constructor
/// </summary>
/// <param name="measurement_size">The number of parameters representing a measurement of the feature.</param>
/// <param name="feature_state_size">The number of parameters to repreent the state of the feature.</param>
/// <param name="graphics_state_size">The number of parameters to represent an abstraction of the feature for use in a graphical display.</param>
/// <param name="motion_model">The robot motion model. Needed to understand and use the robot position statex_pwhen measuring features.</param>
/// <param name="feature_type">The name for this feature measurement model (set by the derived class).</param>
/// <param name="feature_graphics_type">The name for this type of feature, so that it can be drawn (set by the derived class).</param>
/// <param name="free_parameter_size">The number of parameters that should be left outside the main Gaussian-based representation and represented by other means, such as particles.</param>
/// <param name="m_i_f_m_m">What feature measurement model will this turn into once it's fully initialised?</param>
public Partially_Initialised_Feature_Measurement_Model(
uint measurement_size,
uint feature_state_size,
uint graphics_state_size,
Motion_Model motion_model,
String feature_type,
String feature_graphics_type,
uint free_parameter_size,
Feature_Measurement_Model m_i_f_m_m)
: base(measurement_size,
feature_state_size,
graphics_state_size,
motion_model,
feature_type,
feature_graphics_type, false)
{
FREE_PARAMETER_SIZE = free_parameter_size;
more_initialised_feature_measurement_model = m_i_f_m_m;
ypiRES = new Vector(FEATURE_STATE_SIZE);
dypi_by_dxpRES = new MatrixFixed(FEATURE_STATE_SIZE,
motion_model.POSITION_STATE_SIZE);
dypi_by_dhiRES = new MatrixFixed(FEATURE_STATE_SIZE,
MEASUREMENT_SIZE);
hpiRES = new Vector(MEASUREMENT_SIZE);
dhpi_by_dxpRES = new MatrixFixed(MEASUREMENT_SIZE,
motion_model.POSITION_STATE_SIZE);
dhpi_by_dyiRES = new MatrixFixed(MEASUREMENT_SIZE, FEATURE_STATE_SIZE);
yfiRES = new Vector(more_initialised_feature_measurement_model.FEATURE_STATE_SIZE);
dyfi_by_dypiRES = new MatrixFixed(more_initialised_feature_measurement_model.FEATURE_STATE_SIZE, FEATURE_STATE_SIZE);
dyfi_by_dlambdaRES = new MatrixFixed(more_initialised_feature_measurement_model.FEATURE_STATE_SIZE, FREE_PARAMETER_SIZE);
}