Ejemplo n.º 1
0
 public MonoSLAMInterface(
     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)
     : base(initialisation_file, path,
        mm_creator,
        fmm_creator,
        imm_creator,
        number_of_features_to_select,
        number_of_features_to_keep_visible,
        max_features_to_init_at_once,
        min_lambda,
        max_lambda,
        number_of_particles,
        standard_deviation_depth_ratio,
        min_number_of_particles,
        prune_probability_threshold,
        erase_partially_init_feature_after_this_many_attempts,
        MAXIMUM_ANGLE_DIFFERENCE,
        calibration_target_width_mm,
        calibration_target_height_mm,
        calibration_target_distance_mm)
 {
 }
Ejemplo n.º 2
0
        /**************************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;
        }
Ejemplo n.º 3
0
        /// <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);
            }
        }