Beispiel #1
0
 /// <summary>
 /// Make the first measurement of a feature.
 /// </summary>
 /// <param name="z">The location of the feature to measure</param>
 /// <param name="id">The Identifier to fill with the feature measurements</param>
 /// <param name="f_m_m">The feature measurement model to use for this partially-initialised feature</param>
 /// <returns></returns>
 public virtual bool make_initial_measurement_of_feature(Vector z,
                                                  ref classimage_mono id,
                                                  Partially_Initialised_Feature_Measurement_Model f_m_m,
                                                  Vector patch_colour) { return (false); }
Beispiel #2
0
        /// <summary>
        /// Constructor for known features. The different number of 
        /// arguments differentiates it from the constructor for partially-initialised
        /// features
        /// </summary>
        /// <param name="id">reference to the feature identifier</param>
        /// <param name="?"></param>
        public Feature(classimage_mono id, uint lab, uint list_pos,
                       Scene_Single scene, Vector y_known,
                       Vector xp_o,
                       Feature_Measurement_Model f_m_m, uint k_f_l)
        {
            feature_measurement_model = f_m_m;
            feature_constructor_bookeeping();

            identifier = id;
            label = lab;
            position_in_list = list_pos;   // Position of new feature in list

            // Save the vehicle position where this feature was acquired 
            xp_orig = new Vector(xp_o);

            // Straighforward initialisation of state and covariances
            y = y_known;
            Pxy = new MatrixFixed(scene.get_motion_model().STATE_SIZE, feature_measurement_model.FEATURE_STATE_SIZE);
            Pxy.Fill(0.0f);
            Pyy = new MatrixFixed(feature_measurement_model.FEATURE_STATE_SIZE, feature_measurement_model.FEATURE_STATE_SIZE);
            Pyy.Fill(0.0f);

            int i = 0;
            MatrixFixed newPyjyi_to_store;
            foreach (Feature it in scene.get_feature_list_noconst())
            {
                if (i < position_in_list)
                {
                    newPyjyi_to_store = new MatrixFixed(
                        it.get_feature_measurement_model().FEATURE_STATE_SIZE,
                        feature_measurement_model.FEATURE_STATE_SIZE);

                    //add to the list
                    matrix_block_list.Add(newPyjyi_to_store);
                }

                i++;
            }

            known_feature_label = (int)k_f_l;

            if (feature_measurement_model.fully_initialised_flag)
            {
                partially_initialised_feature_measurement_model = null;
                fully_initialised_feature_measurement_model =
                    (Fully_Initialised_Feature_Measurement_Model)feature_measurement_model;
            }
            else
            {
                fully_initialised_feature_measurement_model = null;
                partially_initialised_feature_measurement_model =
                    (Partially_Initialised_Feature_Measurement_Model)feature_measurement_model;
            }
        }
Beispiel #3
0
        /// <summary>
        /// Convert a partially-initialised feature to a fully-initialised feature,
        /// given information about the free parameters \vct{\lambda}.
        /// The new state \vct{y}_{fi} is given by calling
        /// Partially_Initialised_Feature_Measurement_Model::func_yfi_and_dyfi_by_dypi_and_dyfi_by_dlambda().
        /// where the various Jacobians are returned by calls to
        /// Partially_Initialised_Feature_Measurement_Model, and the covariance matrices
        /// \mat{P}_{kl} are already known and stored in the class, except for
        /// \mat{P}_{\vct{\lambda}}, which is passed to the function.
        /// </summary>
        /// <param name="lambda">The mean value for \vct{\lambda}</param>
        /// <param name="Plambda">The covariance for \vct{\lambda}</param>
        /// <param name="scene">The SLAM map</param>
        public void convert_from_partially_to_fully_initialised(
                Vector lambda, MatrixFixed Plambda, Scene_Single scene)
        {
            
            // We'll do all the work here in feature.cc though probably this only
            // works with scene_single...

            // We calculate new state yfi(ypi, lambda)
            // New feature covariance 
            // Pyfiyfi = dyfi_by_dypi Pypiypi dyfi_by_dypiT + 
            //           dyfi_by_dlambda Plambda dyfi_by_dlambdaT
            // And we change cross covariances as follows:
            // Pxyfi = Pxypi dyfi_by_dypiT
            // Pyjyfi = Pyjypi dyfi_by_dypiT   for j < i (since we only store top-right
            // Pyfiyj = dyfi_by_dypi Pypiyj    for j > i  part of covariance matrix)

            partially_initialised_feature_measurement_model.func_yfi_and_dyfi_by_dypi_and_dyfi_by_dlambda(y, lambda);

            MatrixFixed dyfi_by_dypiT = partially_initialised_feature_measurement_model.get_dyfi_by_dypiRES().Transpose();
            MatrixFixed dyfi_by_dlambdaT = partially_initialised_feature_measurement_model.get_dyfi_by_dlambdaRES().Transpose();

            // Replace y            
            y = new Vector(partially_initialised_feature_measurement_model.get_yfiRES());

            // Replace Pxy
            Pxy = Pxy * dyfi_by_dypiT;

            // Replace Pyy
            MatrixFixed Pypiypi_1 = partially_initialised_feature_measurement_model.get_dyfi_by_dypiRES() *
                        Pyy * dyfi_by_dypiT;
            MatrixFixed Pypiypi_2 = partially_initialised_feature_measurement_model.get_dyfi_by_dlambdaRES() *
                        Plambda * dyfi_by_dlambdaT;
            Pyy = Pypiypi_1 + Pypiypi_2;

            // Pyjyi elements for j < i (covariance with features before i in list)
            uint i = position_in_list;

            MatrixFixed m_it;
            int j;
            for (j = 0; j < position_in_list; j++)
            {
                m_it = (MatrixFixed)matrix_block_list[j];
                matrix_block_list[j] = m_it * dyfi_by_dypiT;
            }


            Feature it;
            int idx = scene.feature_list.IndexOf(this);
            for (j = idx + 1; j < scene.feature_list.Count; j++)
            {
                it = (Feature)(scene.feature_list[j]);
                it.matrix_block_list[(int)i] = partially_initialised_feature_measurement_model.get_dyfi_by_dypiRES() * (MatrixFixed)it.matrix_block_list[(int)i];
                it.increment_position_in_total_state_vector(-(int)feature_measurement_model.FEATURE_STATE_SIZE);
            }


            // Change the total state size in scene, here with a negative increment
            uint size1 = partially_initialised_feature_measurement_model.more_initialised_feature_measurement_model.FEATURE_STATE_SIZE;
            uint size2 = partially_initialised_feature_measurement_model.FEATURE_STATE_SIZE;
            scene.increment_total_state_size((int)size1 - (int)size2);

            // Change fmm for this model to fully-initialised one
            feature_measurement_model =
                partially_initialised_feature_measurement_model.more_initialised_feature_measurement_model;

            partially_initialised_feature_measurement_model = null;
            fully_initialised_feature_measurement_model =
                (Fully_Initialised_Feature_Measurement_Model)feature_measurement_model;


            //assert(fully_initialised_feature_measurement_model != NULL);

            // Need to reallocate any other matrices
            // Assume that measurement size doesn't change 
            dh_by_dy.Resize(feature_measurement_model.MEASUREMENT_SIZE, feature_measurement_model.FEATURE_STATE_SIZE);
                         
             
        }
Beispiel #4
0
        /// <summary>
        /// Constructor for partially-initialised features.
        /// </summary>
        /// <param name="id">reference to the feature</param>
        /// <param name="lab"></param>
        /// <param name="list_pos"></param>
        /// <param name="scene"></param>
        /// <param name="h"></param>
        /// <param name="p_i_f_m_m"></param>
        public Feature(classimage_mono id, uint lab, uint list_pos,
                       Scene_Single scene, Vector h,
                       Partially_Initialised_Feature_Measurement_Model p_i_f_m_m,
                       Vector feature_colour)
        {
            feature_measurement_model = p_i_f_m_m;
            partially_initialised_feature_measurement_model = p_i_f_m_m;
            fully_initialised_feature_measurement_model = null;

            // Stuff below substituted from Feature_common
            //   Feature_common(id, lab, list_pos, scene, h);

            feature_constructor_bookeeping();

            identifier = id;
            label = lab;
            position_in_list = list_pos;   // Position of new feature in list
            position_in_total_state_vector = 0; // This should be set properly
            colour = feature_colour;
            //when feature is added 

            // Save the vehicle position where this feature was acquired 
            scene.get_motion_model().func_xp(scene.get_xv());
            //xp_orig = scene.get_motion_model().get_xpRES();
            xp_orig = new Vector(scene.get_motion_model().get_xpRES());

            // Call model functions to calculate feature state, measurement noise
            // and associated Jacobians. Results are stored in RES matrices 

            // First calculate "position state" and Jacobian
            scene.get_motion_model().func_xp(scene.get_xv());
            scene.get_motion_model().func_dxp_by_dxv(scene.get_xv());

            // Now ask the model to initialise the state vector and calculate Jacobians
            // so that I can go and calculate the covariance matrices
            partially_initialised_feature_measurement_model.func_ypi_and_dypi_by_dxp_and_dypi_by_dhi_and_Ri(h, scene.get_motion_model().get_xpRES());

            // State y
            //y = partially_initialised_feature_measurement_model.get_ypiRES();
            y = new Vector(partially_initialised_feature_measurement_model.get_ypiRES());

            // Temp_FS1 will store dypi_by_dxv
            MatrixFixed Temp_FS1 =
                     partially_initialised_feature_measurement_model.get_dypi_by_dxpRES() *
                     scene.get_motion_model().get_dxp_by_dxvRES();

            // Pxy  
            Pxy = scene.get_Pxx() * Temp_FS1.Transpose();

            // Pyy
            Pyy = Temp_FS1 * scene.get_Pxx() * Temp_FS1.Transpose()
                  + partially_initialised_feature_measurement_model.get_dypi_by_dhiRES()
                  * partially_initialised_feature_measurement_model.get_RiRES()
                  * partially_initialised_feature_measurement_model.get_dypi_by_dhiRES().Transpose();

            // Covariances of this feature with others
            int j = 0;
            foreach (Feature it in scene.get_feature_list_noconst())
            {
                if (j < position_in_list)
                {
                    // new Pypiyj = dypi_by_dxv . Pxyj
                    // Size of this is FEATURE_STATE_SIZE(new) by FEATURE_STATE_SIZE(old)
                    MatrixFixed m = it.get_Pxy();
                    MatrixFixed newPyjypi_to_store = (Temp_FS1 * m).Transpose();

                    //add to the list
                    matrix_block_list.Add(newPyjypi_to_store);
                }
                j++;
            }

            known_feature_label = -1;
        }
Beispiel #5
0
        /// <summary>
        /// Create a new partially-initialised feature. This creates a new Feature, and
        /// creates a new empty FeatureInitInfo to store the extra initialisation
        /// information, which must be then filled in by the caller of this function.
        /// </summary>
        /// <param name="id">The unique identifier for this feature (e.g. the image patch)</param>
        /// <param name="h">The initial measured state for this feature (e.g. the image location)</param>
        /// <param name="f_m_m">The partially-initialised feature measurement model to apply to this feature.</param>
        /// <returns>A pointer to the FeatureInitInfo object to be filled in with further initialisation information.</returns>
        public FeatureInitInfo add_new_partially_initialised_feature(classimage_mono id, 
                               Vector h, Partially_Initialised_Feature_Measurement_Model f_m_m,
                               Vector feature_colour)
        {
            Feature nf = new Feature(id, next_free_label, (uint)feature_list.Count, this, h, f_m_m, feature_colour);
            
            add_new_feature_bookeeping(nf);

            // Set stuff to store extra probability information for partially
            // initialised feature
            FeatureInitInfo feat = new FeatureInitInfo(this, nf);
            feature_init_info_vector.Add(feat);

            return (feat);
        }
Beispiel #6
0
        /// <summary>
        /// Make the initial measurement of the currently-selected feature. This fills in
        /// the location and the identifier (a copy of the image patch) for the current
        /// feature. The feature location is set using set_image_selection_automatically()
        /// or manually by the user using set_image_selection(). This function just calls
        /// partially_initialise_point_feature() to fill in the measurement and identifier.
        /// </summary>
        /// <param name="z">The measurement vector to be filled in.</param>
        /// <param name="id_ptr">The identifier to be filled in.</param>
        /// <param name="m"></param>
        /// <returns>true on success, or <code>false</code> on failure (e.g. no patch is currently selected).</returns>
        public override bool make_initial_measurement_of_feature(Vector z, ref byte[] patch, Partially_Initialised_Feature_Measurement_Model m, Vector patch_colour)
        {
            patch = partially_initialise_point_feature(z);

            for (int c = 0; c < 3; c++) patch_colour[c] = image_colour.image[uu, vv, c];

            if (patch != null)
                return true;
            else
                return false;
        }
Beispiel #7
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);
            }
        }
        // Function to find a region in an image guided by current motion prediction
        // which doesn't overlap existing features
        public static bool FindNonOverlappingRegion(Scene_Single scene,
                  Vector local_u,
                  float delta_t,
                  Partially_Initialised_Feature_Measurement_Model default_feature_type_for_initialisation,
                  uint camera_width,
                  uint camera_height,
                  uint BOXSIZE,
                  ref int init_feature_search_ustart,
                  ref int init_feature_search_vstart,
                  ref int init_feature_search_ufinish,
                  ref int init_feature_search_vfinish,
                  uint FEATURE_INIT_STEPS_TO_PREDICT,
                  float FEATURE_INIT_DEPTH_HYPOTHESIS,
                  Random rnd)
        {
            
            ThreeD_Motion_Model threed_motion_model = (ThreeD_Motion_Model)scene.get_motion_model();

            Vector local_xv = new Vector(scene.get_xv());
            
            for (uint i = 0; i < FEATURE_INIT_STEPS_TO_PREDICT; i++)
            {
                scene.get_motion_model().func_fv_and_dfv_by_dxv(local_xv, local_u, delta_t);
                local_xv.Update(scene.get_motion_model().get_fvRES());
            }
            
            threed_motion_model.func_xp(local_xv);
            Vector local_xp = new Vector(threed_motion_model.get_xpRES());

            threed_motion_model.func_r(local_xp);
            Vector3D rW = threed_motion_model.get_rRES();
            threed_motion_model.func_q(local_xp);
            Quaternion qWR = threed_motion_model.get_qRES();

            // yW =  rW + RWR hR
            Vector3D hR = new Vector3D(0.0f, 0.0f, FEATURE_INIT_DEPTH_HYPOTHESIS);

            // Used Inverse + transpose because this was't compiling the normal way
            Vector3D yW = new Vector3D(rW + qWR.RotationMatrix() * hR);

            // Then project that point into the current camera position
            scene.get_motion_model().func_xp(scene.get_xv());

            Fully_Initialised_Feature_Measurement_Model fully_init_fmm =
                (Fully_Initialised_Feature_Measurement_Model)(default_feature_type_for_initialisation.more_initialised_feature_measurement_model);


            Vector yWVNL = yW.GetVNL3();
            fully_init_fmm.func_hi_and_dhi_by_dxp_and_dhi_by_dyi(yWVNL, scene.get_motion_model().get_xpRES());

            // Now, this defines roughly how much we expect a feature initialised to move
            float suggested_u = fully_init_fmm.get_hiRES()[0];
            float suggested_v = fully_init_fmm.get_hiRES()[1];
            float predicted_motion_u = camera_width / 2.0f - suggested_u;
            float predicted_motion_v = camera_height / 2.0f - suggested_v;

            // So, the limits of a "safe" region within which we can initialise
            // features so that they end up staying within the screen
            // (Making the approximation that the whole screen moves like the 
            // centre point)
            int safe_feature_search_ustart = (int)(-predicted_motion_u);
            int safe_feature_search_vstart = (int)(-predicted_motion_v);
            int safe_feature_search_ufinish = (int)(camera_width - predicted_motion_u);
            int safe_feature_search_vfinish = (int)(camera_height - predicted_motion_v);

            if (safe_feature_search_ustart < ((int)((BOXSIZE - 1) / 2) + 1))
                safe_feature_search_ustart = (int)((BOXSIZE - 1) / 2 + 1);
            if (safe_feature_search_ufinish > (int)camera_width - ((int)((BOXSIZE - 1) / 2) + 1))
                safe_feature_search_ufinish = (int)(camera_width - (BOXSIZE - 1) / 2 - 1);
            if (safe_feature_search_vstart < ((int)((BOXSIZE - 1) / 2) + 1))
                safe_feature_search_vstart = (int)((BOXSIZE - 1) / 2 + 1);
            if (safe_feature_search_vfinish > (int)camera_height - ((int)((BOXSIZE - 1) / 2) + 1))
                safe_feature_search_vfinish = (int)(camera_height - (BOXSIZE - 1) / 2 - 1);

            return FindNonOverlappingRegionNoPredict(safe_feature_search_ustart,
                       safe_feature_search_vstart,
                       safe_feature_search_ufinish,
                       safe_feature_search_vfinish,
                       scene,
                       ref init_feature_search_ustart,
                       ref init_feature_search_vstart,
                       ref init_feature_search_ufinish,
                       ref init_feature_search_vfinish, rnd);
        }