Exemple #1
0
        /// <summary>
        /// Make measurements of all the currently-selected features. Features can be
        /// selected using Scene_Single::auto_select_n_features(), or manually using
        /// Scene_Single::select_feature(). This calls
        /// Scene_Single::starting_measurements() and then Sim_Or_Rob::measure_feature()
        /// for each selected feature. Each feature for which a measurement
        /// attempt is made has its Feature::attempted_measurements_of_feature and
        /// Feature::successful_measurements_of_feature counts updated.
        /// </summary>
        /// <param name="scene">The SLAM map to use</param>
        /// <param name="sim_or_rob">The class to use for measuring features.</param>
        /// <returns>The number of features successfully measured.</returns>
        public static int make_measurements(Scene_Single scene,
                                            Sim_Or_Rob sim_or_rob,
                                            Random rnd)
        {
            int count = 0;

            if (scene.get_no_selected() == 0)
            {
                Debug.WriteLine("No features selected.");
                return(0);
            }

            scene.starting_measurements();

            Feature it;
            Vector  z;   //best position for the feature within the image

            for (int i = 0; i < scene.selected_feature_list.Count; i++)
            {
                it = (Feature)scene.selected_feature_list[i];

                z = it.get_z_noconst();
                if (sim_or_rob.measure_feature(it.get_identifier(), ref z, it.get_vz(), it.get_h(), it.get_S(), rnd) == false)
                {
                    // couldn't locate the feature
                    scene.failed_measurement_of_feature(it);
                    it.update_velocity(false);
                }
                else
                {
                    // the feature was found!
                    scene.successful_measurement_of_feature(it);
                    it.update_velocity(true);
                    count++;
                }
            }

            return(count);
        }