Exemplo n.º 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;
        }