예제 #1
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
                        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);
        }