// Redefined virtual functions
public override void read_parameters(Settings settings)
{
// Initialise any base class parameters
base.read_parameters(settings);
// Initialise my camera model
Settings.Section section = settings.get_section(feature_type);
if(section == null)
{
Debug.WriteLine("Warning: no [" + feature_type +
"] section in the intialisation data. " +
"The feature measurement model will be initialised with the default " +
"camera parameters.");
}
else
{
// Read the camera model. This will be the name of another section
ArrayList values = section.get_entry("Camera");
if (values == null)
{
Debug.WriteLine("Warning: no Camera entry in the [" + feature_type +
"] section in the intialisation data. " +
"The feature measurement model will be initialised with the " +
"default camera parameters.");
}
else
{
String camera = (String)values[0];
section = settings.get_section(camera);
if(section == null)
{
Debug.WriteLine("Warning: no [" + camera +
"] section in the intialisation data. " +
"The feature measurement model will be initialised with the " +
" default camera parameters.");
}
else
((Wide_Camera_Point_Feature_Measurement_Model)wide_model).read_parameters(section);
}
}
}
public virtual classimage_mono initialise_known_feature(Feature_Measurement_Model f_m_m,
Vector yi, Settings.Section section, String path) { return (null); }
// Read the parameters of the camera from the settings.
public void read_parameters(Settings.Section section)
{
ArrayList values = section.get_entry("CameraParameters");
if (values != null)
{
String param = (String)values[0];
m_camera.ReadASCII(param);
}
else
{
String error;
error = "No CameraParameters entry found in initialisation file section [" +
section.Label() + "].";
Debug.WriteLine(error);
}
}
/// <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);
}
/**************************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
byte[] 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;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initialisation_file">The initialisation file to read. This specifies the motion- and feature-measurement models to use, the initial state and known features.</param>
/// <param name="mm_creator">The factory to use to create motion models.</param>
/// <param name="fmm_creator">The factory to use to create feature measurement models</param>
/// <param name="imm_creator">The factory to use to create internal measurement models</param>
/// <param name="number_of_features_to_select">The number of features to select for measurement at each time step</param>
/// <param name="number_of_features_to_keep_visible">The requried number of visible features. If fewer than this number are visible at any time step, the creation of a new feature is initiated</param>
/// <param name="max_features_to_init_at_once"></param>
/// <param name="min_lambda">The minimum distance from the camera (in metres) for a new feature</param>
/// <param name="max_lambda">The maximum distance from the camera (in metres) for a new feature</param>
/// <param name="number_of_particles">The number of particles to use for new features (distributed evenly in space between min_lambda and max_lambda)</param>
/// <param name="standard_deviation_depth_ratio">The ratio between standard deviation and mean to use to identify when a partially-initialised feature should be converted to a fully-initialised one</param>
/// <param name="min_number_of_particles">The minimum number of particles below which a partially-initalised feature is deleted</param>
/// <param name="prune_probability_threshold">The threshold below which a particle with low probability is deleted</param>
/// <param name="erase_partially_init_feature_after_this_many_attempts">The number of failed match attempts before a partially initialised feature is deleted.</param>
public MonoSLAM(String initialisation_file,
String path,
Motion_Model_Creator mm_creator,
Feature_Measurement_Model_Creator fmm_creator,
Internal_Measurement_Model_Creator imm_creator,
uint number_of_features_to_select,
uint number_of_features_to_keep_visible,
uint max_features_to_init_at_once,
float min_lambda,
float max_lambda,
uint number_of_particles,
float standard_deviation_depth_ratio,
uint min_number_of_particles,
float prune_probability_threshold,
uint erase_partially_init_feature_after_this_many_attempts,
float MAXIMUM_ANGLE_DIFFERENCE,
float calibration_target_width_mm,
float calibration_target_height_mm,
float calibration_target_distance_mm)
{
PATH = path;
NUMBER_OF_FEATURES_TO_SELECT = number_of_features_to_select;
NUMBER_OF_FEATURES_TO_KEEP_VISIBLE = number_of_features_to_keep_visible;
MAX_FEATURES_TO_INIT_AT_ONCE = max_features_to_init_at_once;
MIN_LAMBDA = min_lambda;
MAX_LAMBDA = max_lambda;
NUMBER_OF_PARTICLES = number_of_particles;
STANDARD_DEVIATION_DEPTH_RATIO = standard_deviation_depth_ratio;
MIN_NUMBER_OF_PARTICLES = min_number_of_particles;
PRUNE_PROBABILITY_THRESHOLD = prune_probability_threshold;
ERASE_PARTIALLY_INIT_FEATURE_AFTER_THIS_MANY_ATTEMPTS = erase_partially_init_feature_after_this_many_attempts;
number_of_visible_features = 0;
number_of_matched_features = 0;
Settings settings = new Settings();
//if no file exists create some default values
//if (!File.Exists(PATH + initialisation_file))
{
//create a settings file
settings.createDefault(PATH + initialisation_file, calibration_target_width_mm,
calibration_target_height_mm, calibration_target_distance_mm);
//settings.createDefault(PATH + initialisation_file, 210, 148.5, 600);
}
//create some known features
//createDefaultKnownFeatures(PATH);
// Create the Settings class by reading from the initialisation file
if (File.Exists(PATH + initialisation_file))
{
StreamReader stream = File.OpenText(PATH + initialisation_file);
settings.load(stream);
// Create the Scene class. This also constructs the motion model and
// internal measurement models and sets the initial state
scene = new Scene_Single(settings, mm_creator, imm_creator);
// Now sort out the feature types
ArrayList values = settings.get_entry("Models", "NewFeatureMeasurementModel");
String feature_init_type = (String)values[0];
Feature_Measurement_Model fm_model =
fmm_creator.create_model(feature_init_type, scene.get_motion_model(), MAXIMUM_ANGLE_DIFFERENCE);
if (fm_model == null)
{
Debug.WriteLine("Unable to create a feature measurement motion model of type " +
feature_init_type + " as requested in initalisation file " +
initialisation_file);
}
else
{
// Initialise this motion model
fm_model.read_parameters(settings);
// Check that this is a partially-initialised feature type
if (fm_model.fully_initialised_flag)
{
Debug.WriteLine("Feature measurement motion model " + feature_init_type +
" as requested in initalisation file " + initialisation_file +
" is not a partially-initialised feature type. ");
}
default_feature_type_for_initialisation =
(Partially_Initialised_Feature_Measurement_Model)fm_model;
// We hope that features are viewed through a camera! If so,
// the feature measurement class should derive from
// Camera_Feature_Measurement_Model
// Note the multiple inherritance workaround
Camera_Feature_Measurement_Model cfmm =
(Camera_Feature_Measurement_Model)(fm_model.wide_model);
if (cfmm == null)
{
// Oops - the feature measurement model is not derived from
// Camera_Feature_Measurement_Model!
Debug.WriteLine("The default feature measurement motion model " +
fm_model.feature_type +
" is not derived from Camera_Feature_Measurement_Model!");
}
else
{
CAMERA_WIDTH = cfmm.get_camera().ImageWidth();
CAMERA_HEIGHT = cfmm.get_camera().ImageHeight();
kalman = new Kalman();
robot = new Robot();
sim_or_rob = (Sim_Or_Rob)robot;
// Initialise any known features
SceneLib.initialise_known_features(settings, fmm_creator, sim_or_rob, scene, PATH, MAXIMUM_ANGLE_DIFFERENCE);
// Various flags
init_feature_search_region_defined_flag = false;
}
}
stream.Close();
}
else
{
Debug.WriteLine("File not found: " + initialisation_file);
}
}
/// <summary>
/// Read the initial state for this feature from the Settings. This reads the
/// state \vct{y}_i and the robot position from which it was first
/// observed, \vct{x}_p^{orig} . These are not stored in the class, so are
/// returned as parameters.
/// </summary>
/// <param name="settings">The Settings class from which to read the data</param>
/// <param name="yi">To be filled in with the initial feature state, \vct{y}_i .</param>
/// <param name="xp_orig">To be filled in with the robot position from which the feature was first observed, \vct{x}_p^{orig} .</param>
public void read_initial_state(Settings.Section section, Vector yi, Vector xp_orig)
{
ArrayList values;
yi.Resize(FEATURE_STATE_SIZE);
values = section.get_entry("yi");
yi.ReadASCII((String)values[0]);
uint size = get_motion_model().POSITION_STATE_SIZE;
xp_orig.Resize(size);
values = section.get_entry("xp_orig");
xp_orig.ReadASCII((String)values[0]);
}
// Read the parameters of this feature measurement model from the settings.
public virtual void read_parameters(Settings settings) {}
/// <summary>
/// Read the initial state vector and covariance from the settings class.
/// Since state \f$ x_v \f$ and covariance \f$ P_{xx} \f$ are not stored in
/// the class, these are passed by reference to be filled in by this function.
/// </summary>
/// <param name="settings"></param>
/// <param name="initial_xv"></param>
/// <param name="initial_Pxx"></param>
public void read_initial_state(Settings settings,
ref Vector initial_xv,
ref MatrixFixed initial_Pxx)
{
ArrayList values;
// Check that the motion model is correct
values = settings.get_entry("InitialState", "MotionModel");
if ((String)values[0] != motion_model_type)
{
Debug.WriteLine("Attempted to read an initial state with a motion model of type " +
motion_model_type + " where the initialisation data in the [InitialState] section" +
" reports the type " + settings.get_entry("InitialState", "MotionModel") + ".");
//throw Scene::InitialisationError(error.str());
}
// Make sure the vector and matrix are the correct sizes
initial_xv = new Vector(STATE_SIZE);
initial_Pxx = new MatrixFixed(STATE_SIZE, STATE_SIZE);
//initial_xv.Resize(STATE_SIZE);
//initial_Pxx.Resize(STATE_SIZE, STATE_SIZE);
initial_xv.Fill(0.0f);
initial_Pxx.Fill(0.0f);
values = settings.get_entry("InitialState", "xv");
String xv_stream = (String)values[0];
initial_xv.ReadASCII(xv_stream);
values = settings.get_entry("InitialState", "Pxx");
String Pxx_stream = (String)values[0];
initial_Pxx.ReadASCII(Pxx_stream);
}
/// <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;
}
