Ejemplo n.º 1
0
 public Line_Init_Wide_Point_Feature_Measurement_Model(Motion_Model motion_model,
                                                       Feature_Measurement_Model fully_init_f_m_m) :
     base(2, 6, 6, motion_model, "CAMERA_WIDE_LINE_INIT", "THREED_SEMI_INFINITE_LINE", 1, fully_init_f_m_m)
 {
     WideCamera cam = new WideCamera();
     float MAXIMUM_LENGTH_RATIO_ = Camera_Constants.WIDE_MAXIMUM_LENGTH_RATIO;
     float MAXIMUM_ANGLE_DIFFERENCE_ = Camera_Constants.WIDE_MAXIMUM_ANGLE_DIFFERENCE;
     float IMAGE_SEARCH_BOUNDARY_ = Camera_Constants.WIDE_IMAGE_SEARCH_BOUNDARY;
     float SD_IMAGE_SIMULATION_ = Camera_Constants.WIDE_SD_IMAGE_SIMULATION;
     wide_model = new Wide_Camera_Point_Feature_Measurement_Model(motion_model, cam, MAXIMUM_LENGTH_RATIO_, MAXIMUM_ANGLE_DIFFERENCE_, IMAGE_SEARCH_BOUNDARY_, SD_IMAGE_SIMULATION_);
 }
Ejemplo n.º 2
0
 public virtual classimage_mono initialise_known_feature(Feature_Measurement_Model f_m_m,
                                             Vector yi, uint known_feature_label, String path) { return (null); }
Ejemplo n.º 3
0
 public virtual classimage_mono initialise_known_feature(Feature_Measurement_Model f_m_m,
                                                    Vector yi, Settings.Section section, String path) { return (null); }
Ejemplo n.º 4
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;
            }
        }
Ejemplo n.º 5
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);
                         
             
        }
Ejemplo n.º 6
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;
        }
Ejemplo n.º 7
0
 public void add_new_known_feature(classimage_mono id, Vector y_known, 
                                   Vector xp_o, Feature_Measurement_Model f_m_m,
                                   uint known_feature_label)
 {
     Feature nf = new Feature(id, next_free_label, (uint)feature_list.Count, 
                              this, y_known, xp_o, f_m_m, known_feature_label);
     add_new_feature_bookeeping(nf);
 }
Ejemplo n.º 8
0
        /// <summary>
        /// Constructor
        /// </summary>
        /// <param name="measurement_size">The number of parameters representing a measurement of the feature.</param>
        /// <param name="feature_state_size">The number of parameters to repreent the state of the feature.</param>
        /// <param name="graphics_state_size">The number of parameters to represent an abstraction of the feature for use in a graphical display.</param>
        /// <param name="motion_model">The robot motion model. Needed to understand and use the robot position statex_pwhen measuring features.</param>
        /// <param name="feature_type">The name for this feature measurement model (set by the derived class).</param>
        /// <param name="feature_graphics_type">The name for this type of feature, so that it can be drawn (set by the derived class).</param>
        /// <param name="free_parameter_size">The number of parameters that should be left outside the main Gaussian-based representation and represented by other means, such as particles.</param>
        /// <param name="m_i_f_m_m">What feature measurement model will this turn into once it's fully initialised?</param>
        public Partially_Initialised_Feature_Measurement_Model(
            uint measurement_size,
            uint feature_state_size,
            uint graphics_state_size,
            Motion_Model motion_model,
            String feature_type,
            String feature_graphics_type,
            uint free_parameter_size,
            Feature_Measurement_Model m_i_f_m_m)
            : base(measurement_size, 
                   feature_state_size,
                   graphics_state_size,
                   motion_model,
                   feature_type,
                   feature_graphics_type, false)
            
        {
            FREE_PARAMETER_SIZE = free_parameter_size;
            more_initialised_feature_measurement_model = m_i_f_m_m;

            ypiRES = new Vector(FEATURE_STATE_SIZE);
            dypi_by_dxpRES = new MatrixFixed(FEATURE_STATE_SIZE, 
                        motion_model.POSITION_STATE_SIZE);
            dypi_by_dhiRES = new MatrixFixed(FEATURE_STATE_SIZE,
                        MEASUREMENT_SIZE);
            hpiRES = new Vector(MEASUREMENT_SIZE);
            dhpi_by_dxpRES = new MatrixFixed(MEASUREMENT_SIZE, 
                        motion_model.POSITION_STATE_SIZE);
            dhpi_by_dyiRES = new MatrixFixed(MEASUREMENT_SIZE, FEATURE_STATE_SIZE);
            yfiRES = new Vector(more_initialised_feature_measurement_model.FEATURE_STATE_SIZE);
            dyfi_by_dypiRES = new MatrixFixed(more_initialised_feature_measurement_model.FEATURE_STATE_SIZE, FEATURE_STATE_SIZE);
            dyfi_by_dlambdaRES = new MatrixFixed(more_initialised_feature_measurement_model.FEATURE_STATE_SIZE, FREE_PARAMETER_SIZE);
        }
Ejemplo n.º 9
0
        /// <summary>
        /// Initialise a known feature, in this case by loading an image file. The name of
        /// the file is read from the Settings Section passed to this function, with the
        /// entry Identifier.
        /// </summary>
        /// <param name="fmm"></param>
        /// <param name="v"></param>
        /// <param name="section"></param>
        /// <returns></returns>
        public override classimage_mono initialise_known_feature(Feature_Measurement_Model fmm,
                                                            Vector v, Settings.Section section,
                                                            String path)
        {
            ArrayList values = section.get_entry("Identifier");
            String name = (String)values[0];            

            //cout << "Reading patch " << name << endl;

            classimage_mono patch = new classimage_mono();
            if (!(patch.loadFromBitmapMono(path + name, (int)Camera_Constants.BOXSIZE, (int)Camera_Constants.BOXSIZE)))
            {
                patch = null;
            }

            return patch;
        }
Ejemplo n.º 10
0
        /// <summary>
        /// Initialise a known feature. In this case it is assumed that a known feature
        /// is an image patch to be loaded from a file <code>known_patch?.bmp</code>.
        /// </summary>
        /// <param name="fmm"></param>
        /// <param name="v"></param>
        /// <param name="known_feature_label"></param>
        /// <returns></returns>
        public override classimage_mono initialise_known_feature(Feature_Measurement_Model fmm,
                                                   Vector v, uint known_feature_label, String path)
        {
            String name = Camera_Constants.known_point_patch_stem + known_feature_label + ".bmp";

            

            classimage_mono patch = new classimage_mono();
            if (!patch.loadFromBitmapMono(path + name, (int)Camera_Constants.BOXSIZE, (int)Camera_Constants.BOXSIZE))
            {
                patch = null;
            }

            return patch;
        }