Пример #1
0
        /// <summary>
        /// Returns an instance of the requested feature measurement model (using new). 
        /// Returns null if there was no match.
        /// </summary>
        /// <param name="type"></param>
        /// <param name="motion_model"></param>
        /// <returns></returns>
        public override Feature_Measurement_Model create_model(String type, Motion_Model motion_model, float MAXIMUM_ANGLE_DIFFERENCE)
        {
            // Try creating each model that we can and see if the name is the same  
            // std::cout << "Creating a " << type << " feature measurement model" << std::endl;
            Feature_Measurement_Model pModel;

            pModel = new Fully_Init_Wide_Point_Feature_Measurement_Model(motion_model, MAXIMUM_ANGLE_DIFFERENCE);
            if (pModel.feature_type == type)
                return pModel;

            // The partially-initialised model also needs the fully-initalised
            // one that we have just created. Don't delete it - just store it
            Feature_Measurement_Model pFullModel = pModel;
            pModel = new Line_Init_Wide_Point_Feature_Measurement_Model(motion_model, pFullModel);
            if (pModel.feature_type == type)
                return pModel;
            else
            {
                pModel = null;
                pFullModel = null;
            }

            return null;
        }
Пример #2
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_);
 }
Пример #3
0
 /// <summary>
 /// Model for single camera making point measurements
 /// </summary>
 /// <param name="motion_model">The motion model describing the robot motion. This must be derived from ThreeD_Motion_Model</param>
 /// <param name="cam">The camera model to use</param>
 /// <param name="MAXIMUM_LENGTH_RATIO_"></param>
 /// <param name="MAXIMUM_ANGLE_DIFFERENCE_"></param>
 /// <param name="IMAGE_SEARCH_BOUNDARY_"></param>
 /// <param name="SD_IMAGE_SIMULATION_"></param>
 public Fully_Init_Wide_Point_Feature_Measurement_Model(
            Motion_Model motion_model, WideCamera cam, float MAXIMUM_LENGTH_RATIO_,
            float MAXIMUM_ANGLE_DIFFERENCE_, float IMAGE_SEARCH_BOUNDARY_,
            float SD_IMAGE_SIMULATION_)
 :
     base(2, 3, 3, motion_model, "CAMERA_WIDE_POINT", "THREED_POINT")
 {
     wide_model = new Wide_Camera_Point_Feature_Measurement_Model(motion_model, cam, MAXIMUM_LENGTH_RATIO_, MAXIMUM_ANGLE_DIFFERENCE_, IMAGE_SEARCH_BOUNDARY_, SD_IMAGE_SIMULATION_);
 }
Пример #4
0
 public Fully_Init_Wide_Point_Feature_Measurement_Model(Motion_Model motion_model, float WIDE_MAXIMUM_ANGLE_DIFFERENCE)
     : base(2, 3, 3, motion_model, "CAMERA_WIDE_POINT", "THREED_POINT")
 {
     WideCamera cam = new WideCamera();
     float MAXIMUM_LENGTH_RATIO_ = Camera_Constants.WIDE_MAXIMUM_LENGTH_RATIO;
     float MAXIMUM_ANGLE_DIFFERENCE_ = WIDE_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_);
 }
Пример #5
0
       /// <summary>
       /// Constructor
       /// </summary>
       /// <param name="motion_model">The motion model describing the robot motion. This must be derived from ThreeD_Motion_Model</param>
       /// <param name="cam">The camera model to use</param>
       /// <param name="MAXIMUM_LENGTH_RATIO_"></param>
       /// <param name="MAXIMUM_ANGLE_DIFFERENCE_"></param>
       /// <param name="IMAGE_SEARCH_BOUNDARY_"></param>
       /// <param name="SD_IMAGE_SIMULATION_"></param>       
       public Wide_Camera_Point_Feature_Measurement_Model(
                  Motion_Model motion_model,
                  WideCamera cam,
                  float MAXIMUM_LENGTH_RATIO_,
                  float MAXIMUM_ANGLE_DIFFERENCE_,
                  float IMAGE_SEARCH_BOUNDARY_,
                  float SD_IMAGE_SIMULATION_)
       {
           MAXIMUM_LENGTH_RATIO = MAXIMUM_LENGTH_RATIO_;
           MAXIMUM_ANGLE_DIFFERENCE = MAXIMUM_ANGLE_DIFFERENCE_;
           IMAGE_SEARCH_BOUNDARY = IMAGE_SEARCH_BOUNDARY_;
           SD_IMAGE_SIMULATION = SD_IMAGE_SIMULATION_;
           m_camera = cam;
 
           if ((String)(motion_model.motion_model_dimensionality_type) != "THREED")
           {
               String os;
               os = "Motion Model given to the Feature Measurement Model is of type " + 
                    motion_model.motion_model_dimensionality_type +
                    ", not type THREED.";
               Debug.WriteLine(os);
               //throw Scene::InitialisationError(os.str());
           }
           threed_motion_model = (ThreeD_Motion_Model)motion_model;
       }
Пример #6
0
        /// <summary>
        /// Use this version if using internal measurement models defined in settings file.
        /// </summary>
        /// <param name="mm_creator"></param>
        /// <param name="imm_creator"></param>
        public Scene_Single(Settings settings, Motion_Model_Creator mm_creator,
			                Internal_Measurement_Model_Creator imm_creator)
        {
            // What is the motion model?
            ArrayList values = settings.get_entry("Models", "MotionModel");
            String model = (String)values[0];
  
            //assert(mm_creator != NULL);
            motion_model = mm_creator.create_model(model);

            if (motion_model == null)
            {
                Debug.WriteLine("Unable to create a motion model of type " + model +
                                " as requested in the initalisation file. ");
            }

            // Initialise the motion model with any settings
            motion_model.read_parameters(settings);
  
            // Create internal measurement models if required
            if (imm_creator != null) 
            {
                uint imm_number = 0;
                // Read in and create potentially various internal measurement models
                while (true) 
                {
                    // A list of internal measurement models will be given in the 
                    // Models section of settings with entry names 
                    // InternalMeasurementModel0, InternalMeasurementModel1, etc.
                    String entry_name = "InternalMeasurementModel" + Convert.ToString(imm_number);
                    values = settings.get_entry("Models", entry_name);
                    String new_imm_type = (String)values[0];

                    // Exit loop if no more internal measurement models
                    if (new_imm_type == "") break;

                    // Initialise new model and add to Scene
                    Internal_Measurement_Model new_imm = imm_creator.create_model(new_imm_type, motion_model);

                    // Note for future: at this stage we should potentially read in
                    // parameters for new imm; potentially we could have several internal
                    // measurements define which have the same model but different parameters
                    // (just like we have multiple features)
      
                    add_internal_measurement(new_imm);

                    imm_number++;
                }
            }


            // Get the initial state settings
            Vector initial_xv = null;
            MatrixFixed initial_Pxx = null;
            motion_model.read_initial_state(settings, ref initial_xv, ref initial_Pxx);

            scene_constructor_bookkeeping(initial_xv, initial_Pxx);
        }
Пример #7
0
 /// <summary>
 /// Old style constructor
 /// Constuct the Scene_Single class and set the initial robot state.
 /// </summary>
 /// <param name="initial_xv">The initial robot state</param>
 /// <param name="initial_Pxx">The initial robot state covariance</param>
 /// <param name="m_m">The motion model to use</param>
 public Scene_Single(Vector initial_xv, 
                     MatrixFixed initial_Pxx,
                     Motion_Model m_m)
 {
     motion_model = m_m;
     scene_constructor_bookkeeping(initial_xv, initial_Pxx);
 }
Пример #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>
 public Fully_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)
     : base(measurement_size, 
            feature_state_size,
            graphics_state_size,
            motion_model,
            feature_type,
            feature_graphics_type, true)
 {
     hiRES = new Vector(MEASUREMENT_SIZE);
     dhi_by_dxpRES = new MatrixFixed(MEASUREMENT_SIZE, 
                motion_model.POSITION_STATE_SIZE);
     dhi_by_dyiRES = new MatrixFixed(MEASUREMENT_SIZE, FEATURE_STATE_SIZE);
     nuiRES = new Vector(MEASUREMENT_SIZE);
     hi_noisyRES = new Vector(MEASUREMENT_SIZE);
 }
Пример #9
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="m_m">The robot motion model. Needed to understand and use the robot position statex_pwhen measuring features</param>
        /// <param name="f_t">The name for this feature measurement model (set by the derived class).</param>
        /// <param name="f_g_t">The name for this type of feature, so that it can be drawn (set by the derived class)</param>
        /// <param name="f_i_f">Is this model for fully-initialised features? (if <code>false</code> it is for partially-initialised features.). Set by the derived class.</param>
        public Feature_Measurement_Model(
                    uint measurement_size, 
				    uint feature_state_size,
		       	    uint graphics_state_size,
			        Motion_Model m_m, 
				    String f_t, String f_g_t,
                    bool f_i_f)
        {
            MEASUREMENT_SIZE = measurement_size; 
            FEATURE_STATE_SIZE = feature_state_size;
            GRAPHICS_STATE_SIZE = graphics_state_size;
            feature_type = f_t;
            feature_graphics_type = f_g_t;
            fully_initialised_flag = f_i_f;
            _motion_model = m_m;

            // Allocate matrices for storage of results
            yigraphicsRES = new Vector(GRAPHICS_STATE_SIZE);
            PyiyigraphicsRES = new MatrixFixed(GRAPHICS_STATE_SIZE, GRAPHICS_STATE_SIZE);
            zeroedyigraphicsRES = new Vector(GRAPHICS_STATE_SIZE);
            PzeroedyigraphicsRES = new MatrixFixed(GRAPHICS_STATE_SIZE, GRAPHICS_STATE_SIZE);
            RiRES = new MatrixFixed(MEASUREMENT_SIZE, MEASUREMENT_SIZE);
            SiRES = new MatrixFixed(MEASUREMENT_SIZE, MEASUREMENT_SIZE);
            zeroedyiRES = new Vector(FEATURE_STATE_SIZE);
            dzeroedyi_by_dxpRES = new MatrixFixed(FEATURE_STATE_SIZE, 
			     get_motion_model().POSITION_STATE_SIZE);
            dzeroedyi_by_dyiRES = new MatrixFixed(FEATURE_STATE_SIZE, FEATURE_STATE_SIZE);
        }
Пример #10
0
 /// Returns an instance of the requested internal measurement model
 /// (usually by using the new operator. TODO: who
 /// deletes them?
 public virtual Internal_Measurement_Model create_model(String type,
                                                        Motion_Model motion_model) { return (null); }
Пример #11
0
        public Internal_Measurement_Model(
            uint measurement_size,
            Motion_Model m_m,
            String i_t)
        {
            MEASUREMENT_SIZE = measurement_size;
            motion_model = m_m;
            internal_type = i_t;

            hvRES = new Vector(MEASUREMENT_SIZE);
            dhv_by_dxvRES = new MatrixFixed(MEASUREMENT_SIZE, motion_model.STATE_SIZE);
            RvRES = new MatrixFixed(MEASUREMENT_SIZE, MEASUREMENT_SIZE);
            nuvRES = new Vector(MEASUREMENT_SIZE);
            SvRES = new MatrixFixed(MEASUREMENT_SIZE, MEASUREMENT_SIZE);
            hv_noisyRES = new Vector(MEASUREMENT_SIZE);
        }
Пример #12
0
 /// Returns an instance of the requested feature measurement model
 /// (usually by using the new operator. TODO: who
 /// deletes them?
 public virtual Feature_Measurement_Model create_model(String type, Motion_Model motion_model, float MAXIMUM_ANGLE_DIFFERENCE) { return (null); }
Пример #13
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);
        }