/// <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>
/// Constructor for known features. The different number of
/// arguments differentiates it from the constructor for partially-initialised
/// features
/// </summary>
/// <param name="id">reference to the feature identifier</param>
/// <param name="?"></param>
public Feature(byte[] id, uint lab, uint list_pos,
Scene_Single scene, Vector y_known,
Vector xp_o,
Feature_Measurement_Model f_m_m, uint k_f_l)
{
feature_measurement_model = f_m_m;
feature_constructor_bookeeping();
identifier = id;
label = lab;
position_in_list = list_pos; // Position of new feature in list
// Save the vehicle position where this feature was acquired
xp_orig = new Vector(xp_o);
// Straighforward initialisation of state and covariances
y = y_known;
Pxy = new MatrixFixed(scene.get_motion_model().STATE_SIZE, feature_measurement_model.FEATURE_STATE_SIZE);
Pxy.Fill(0.0f);
Pyy = new MatrixFixed(feature_measurement_model.FEATURE_STATE_SIZE, feature_measurement_model.FEATURE_STATE_SIZE);
Pyy.Fill(0.0f);
int i = 0;
MatrixFixed newPyjyi_to_store;
foreach (Feature it in scene.get_feature_list_noconst())
{
if (i < position_in_list)
{
newPyjyi_to_store = new MatrixFixed(
it.get_feature_measurement_model().FEATURE_STATE_SIZE,
feature_measurement_model.FEATURE_STATE_SIZE);
//add to the list
matrix_block_list.Add(newPyjyi_to_store);
}
i++;
}
known_feature_label = (int)k_f_l;
if (feature_measurement_model.fully_initialised_flag)
{
partially_initialised_feature_measurement_model = null;
fully_initialised_feature_measurement_model =
(Fully_Initialised_Feature_Measurement_Model)feature_measurement_model;
}
else
{
fully_initialised_feature_measurement_model = null;
partially_initialised_feature_measurement_model =
(Partially_Initialised_Feature_Measurement_Model)feature_measurement_model;
}
}
/// <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_);
}
/// <summary>
/// Initialise a known feature. In this case it is assumed that a known feature
/// is an image patch to be loaded from a file <code>known_patch?.bmp</code>.
/// </summary>
/// <param name="fmm"></param>
/// <param name="v"></param>
/// <param name="known_feature_label"></param>
/// <returns></returns>
public override classimage_mono initialise_known_feature(Feature_Measurement_Model fmm,
Vector v, uint known_feature_label, String path)
{
String name = Camera_Constants.known_point_patch_stem + known_feature_label + ".bmp";
classimage_mono patch = new classimage_mono();
if (!patch.loadFromBitmapMono(path + name, (int)Camera_Constants.BOXSIZE, (int)Camera_Constants.BOXSIZE))
{
patch = null;
}
return(patch);
}
/// <summary>
/// Initialise a known feature, in this case by loading an image file. The name of
/// the file is read from the Settings Section passed to this function, with the
/// entry Identifier.
/// </summary>
/// <param name="fmm"></param>
/// <param name="v"></param>
/// <param name="section"></param>
/// <returns></returns>
public override classimage_mono initialise_known_feature(Feature_Measurement_Model fmm,
Vector v, Settings.Section section,
String path)
{
ArrayList values = section.get_entry("Identifier");
String name = (String)values[0];
//cout << "Reading patch " << name << endl;
classimage_mono patch = new classimage_mono();
if (!(patch.loadFromBitmapMono(path + name, (int)Camera_Constants.BOXSIZE, (int)Camera_Constants.BOXSIZE)))
{
patch = null;
}
return(patch);
}
/// <summary>
/// Convert a partially-initialised feature to a fully-initialised feature,
/// given information about the free parameters \vct{\lambda}.
/// The new state \vct{y}_{fi} is given by calling
/// Partially_Initialised_Feature_Measurement_Model::func_yfi_and_dyfi_by_dypi_and_dyfi_by_dlambda().
/// where the various Jacobians are returned by calls to
/// Partially_Initialised_Feature_Measurement_Model, and the covariance matrices
/// \mat{P}_{kl} are already known and stored in the class, except for
/// \mat{P}_{\vct{\lambda}}, which is passed to the function.
/// </summary>
/// <param name="lambda">The mean value for \vct{\lambda}</param>
/// <param name="Plambda">The covariance for \vct{\lambda}</param>
/// <param name="scene">The SLAM map</param>
public void convert_from_partially_to_fully_initialised(
Vector lambda, MatrixFixed Plambda, Scene_Single scene)
{
// We'll do all the work here in feature.cc though probably this only
// works with scene_single...
// We calculate new state yfi(ypi, lambda)
// New feature covariance
// Pyfiyfi = dyfi_by_dypi Pypiypi dyfi_by_dypiT +
// dyfi_by_dlambda Plambda dyfi_by_dlambdaT
// And we change cross covariances as follows:
// Pxyfi = Pxypi dyfi_by_dypiT
// Pyjyfi = Pyjypi dyfi_by_dypiT for j < i (since we only store top-right
// Pyfiyj = dyfi_by_dypi Pypiyj for j > i part of covariance matrix)
partially_initialised_feature_measurement_model.func_yfi_and_dyfi_by_dypi_and_dyfi_by_dlambda(y, lambda);
MatrixFixed dyfi_by_dypiT = partially_initialised_feature_measurement_model.get_dyfi_by_dypiRES().Transpose();
MatrixFixed dyfi_by_dlambdaT = partially_initialised_feature_measurement_model.get_dyfi_by_dlambdaRES().Transpose();
// Replace y
y = new Vector(partially_initialised_feature_measurement_model.get_yfiRES());
// Replace Pxy
Pxy = Pxy * dyfi_by_dypiT;
// Replace Pyy
MatrixFixed Pypiypi_1 = partially_initialised_feature_measurement_model.get_dyfi_by_dypiRES() *
Pyy * dyfi_by_dypiT;
MatrixFixed Pypiypi_2 = partially_initialised_feature_measurement_model.get_dyfi_by_dlambdaRES() *
Plambda * dyfi_by_dlambdaT;
Pyy = Pypiypi_1 + Pypiypi_2;
// Pyjyi elements for j < i (covariance with features before i in list)
uint i = position_in_list;
MatrixFixed m_it;
int j;
for (j = 0; j < position_in_list; j++)
{
m_it = (MatrixFixed)matrix_block_list[j];
matrix_block_list[j] = m_it * dyfi_by_dypiT;
}
Feature it;
int idx = scene.feature_list.IndexOf(this);
for (j = idx + 1; j < scene.feature_list.Count; j++)
{
it = (Feature)(scene.feature_list[j]);
it.matrix_block_list[(int)i] = partially_initialised_feature_measurement_model.get_dyfi_by_dypiRES() * (MatrixFixed)it.matrix_block_list[(int)i];
it.increment_position_in_total_state_vector(-(int)feature_measurement_model.FEATURE_STATE_SIZE);
}
// Change the total state size in scene, here with a negative increment
uint size1 = partially_initialised_feature_measurement_model.more_initialised_feature_measurement_model.FEATURE_STATE_SIZE;
uint size2 = partially_initialised_feature_measurement_model.FEATURE_STATE_SIZE;
scene.increment_total_state_size((int)size1 - (int)size2);
// Change fmm for this model to fully-initialised one
feature_measurement_model =
partially_initialised_feature_measurement_model.more_initialised_feature_measurement_model;
partially_initialised_feature_measurement_model = null;
fully_initialised_feature_measurement_model =
(Fully_Initialised_Feature_Measurement_Model)feature_measurement_model;
//assert(fully_initialised_feature_measurement_model != NULL);
// Need to reallocate any other matrices
// Assume that measurement size doesn't change
dh_by_dy.Resize(feature_measurement_model.MEASUREMENT_SIZE, feature_measurement_model.FEATURE_STATE_SIZE);
}
/// <summary>
/// Constructor for partially-initialised features.
/// </summary>
/// <param name="id">reference to the feature</param>
/// <param name="lab"></param>
/// <param name="list_pos"></param>
/// <param name="scene"></param>
/// <param name="h"></param>
/// <param name="p_i_f_m_m"></param>
public Feature(byte[] id, uint lab, uint list_pos,
Scene_Single scene, Vector h,
Partially_Initialised_Feature_Measurement_Model p_i_f_m_m,
Vector feature_colour)
{
feature_measurement_model = p_i_f_m_m;
partially_initialised_feature_measurement_model = p_i_f_m_m;
fully_initialised_feature_measurement_model = null;
// Stuff below substituted from Feature_common
// Feature_common(id, lab, list_pos, scene, h);
feature_constructor_bookeeping();
identifier = id;
label = lab;
position_in_list = list_pos; // Position of new feature in list
position_in_total_state_vector = 0; // This should be set properly
colour = feature_colour;
//when feature is added
// Save the vehicle position where this feature was acquired
scene.get_motion_model().func_xp(scene.get_xv());
//xp_orig = scene.get_motion_model().get_xpRES();
xp_orig = new Vector(scene.get_motion_model().get_xpRES());
// Call model functions to calculate feature state, measurement noise
// and associated Jacobians. Results are stored in RES matrices
// First calculate "position state" and Jacobian
scene.get_motion_model().func_xp(scene.get_xv());
scene.get_motion_model().func_dxp_by_dxv(scene.get_xv());
// Now ask the model to initialise the state vector and calculate Jacobians
// so that I can go and calculate the covariance matrices
partially_initialised_feature_measurement_model.func_ypi_and_dypi_by_dxp_and_dypi_by_dhi_and_Ri(h, scene.get_motion_model().get_xpRES());
// State y
//y = partially_initialised_feature_measurement_model.get_ypiRES();
y = new Vector(partially_initialised_feature_measurement_model.get_ypiRES());
// Temp_FS1 will store dypi_by_dxv
MatrixFixed Temp_FS1 =
partially_initialised_feature_measurement_model.get_dypi_by_dxpRES() *
scene.get_motion_model().get_dxp_by_dxvRES();
// Pxy
Pxy = scene.get_Pxx() * Temp_FS1.Transpose();
// Pyy
Pyy = Temp_FS1 * scene.get_Pxx() * Temp_FS1.Transpose()
+ partially_initialised_feature_measurement_model.get_dypi_by_dhiRES()
* partially_initialised_feature_measurement_model.get_RiRES()
* partially_initialised_feature_measurement_model.get_dypi_by_dhiRES().Transpose();
// Covariances of this feature with others
int j = 0;
foreach (Feature it in scene.get_feature_list_noconst())
{
if (j < position_in_list)
{
// new Pypiyj = dypi_by_dxv . Pxyj
// Size of this is FEATURE_STATE_SIZE(new) by FEATURE_STATE_SIZE(old)
MatrixFixed m = it.get_Pxy();
MatrixFixed newPyjypi_to_store = (Temp_FS1 * m).Transpose();
//add to the list
matrix_block_list.Add(newPyjypi_to_store);
}
j++;
}
known_feature_label = -1;
}
/**************************Initialise Known Features**************************/
/// <summary>
/// Initialise the Scene_Single class with some known features, read from the
/// Settings. Each known feature has its own section, starting with
/// <code>[KnownFeature1]</code> and counting upwards. The feature type is
/// identified with the entry <code>FeatureMeasurementModel=</code>. Further
/// settings are loaded by the feature measurement model itself.
/// </summary>
/// <param name="model_creator"></param>
/// <param name="sim_or_rob"></param>
/// <param name="scene"></param>
/// <returns></returns>
public static uint initialise_known_features(Settings settings,
Feature_Measurement_Model_Creator model_creator,
Sim_Or_Rob sim_or_rob,
Scene_Single scene,
String path,
float MAXIMUM_ANGLE_DIFFERENCE)
{
uint feature_no = 1;
uint num_features = 0;
Settings.Section section = null;
do
{
// Step through the section names
String section_name = "KnownFeature" + Convert.ToString(feature_no);
section = settings.get_section(section_name);
// Does this section exist?
if (section == null)
{
return(num_features);
}
ArrayList values = section.get_entry("FeatureMeasurementModel");
if (values == null)
{
Debug.WriteLine("No FeatureMeasurementModel entry under the section [" +
section_name + "] in initalisation file.");
}
else
{
String type = (String)values[0];
Feature_Measurement_Model f_m_m =
model_creator.create_model(type, scene.get_motion_model(), MAXIMUM_ANGLE_DIFFERENCE);
if (f_m_m == null)
{
Debug.WriteLine("Unable to create a feature measurement model of type " +
type + " as requested in initalisation file.");
}
else
{
// Initialise the feature measurement model with any settings
f_m_m.read_parameters(settings);
// Read the feature state
Vector yi = new Vector(3);
Vector xp_orig = new Vector(7);
f_m_m.read_initial_state(section, yi, xp_orig);
// Initialise the feature
classimage_mono identifier =
sim_or_rob.initialise_known_feature(f_m_m, yi, section, path);
if (identifier == null)
{
Debug.WriteLine("Trouble reading known feature " +
section_name + " : skipping.");
}
else
{
scene.add_new_known_feature(identifier, yi, xp_orig, f_m_m, feature_no);
Debug.WriteLine("Added known feature " + Convert.ToString(feature_no));
num_features++;
}
}
}
feature_no++;
}while (section != null);
return(num_features);
}
public virtual classimage_mono initialise_known_feature(Feature_Measurement_Model f_m_m,
Vector yi, Settings.Section section, String path)
{
return(null);
}
public virtual classimage_mono initialise_known_feature(Feature_Measurement_Model f_m_m,
Vector yi, uint known_feature_label, String path)
{
return(null);
}
