/// <summary> /// Read the initial state for this feature from the Settings. This reads the /// state \vct{y}_i and the robot position from which it was first /// observed, \vct{x}_p^{orig} . These are not stored in the class, so are /// returned as parameters. /// </summary> /// <param name="settings">The Settings class from which to read the data</param> /// <param name="yi">To be filled in with the initial feature state, \vct{y}_i .</param> /// <param name="xp_orig">To be filled in with the robot position from which the feature was first observed, \vct{x}_p^{orig} .</param> public void read_initial_state(Settings.Section section, Vector yi, Vector xp_orig) { ArrayList values; yi.Resize(FEATURE_STATE_SIZE); values = section.get_entry("yi"); yi.ReadASCII((String)values[0]); uint size = get_motion_model().POSITION_STATE_SIZE; xp_orig.Resize(size); values = section.get_entry("xp_orig"); xp_orig.ReadASCII((String)values[0]); }
/// <summary> /// Read the initial state vector and covariance from the settings class. /// Since state \f$ x_v \f$ and covariance \f$ P_{xx} \f$ are not stored in /// the class, these are passed by reference to be filled in by this function. /// </summary> /// <param name="settings"></param> /// <param name="initial_xv"></param> /// <param name="initial_Pxx"></param> public void read_initial_state(Settings settings, ref Vector initial_xv, ref MatrixFixed initial_Pxx) { ArrayList values; // Check that the motion model is correct values = settings.get_entry("InitialState", "MotionModel"); if ((String)values[0] != motion_model_type) { Debug.WriteLine("Attempted to read an initial state with a motion model of type " + motion_model_type + " where the initialisation data in the [InitialState] section" + " reports the type " + settings.get_entry("InitialState", "MotionModel") + "."); //throw Scene::InitialisationError(error.str()); } // Make sure the vector and matrix are the correct sizes initial_xv = new Vector(STATE_SIZE); initial_Pxx = new MatrixFixed(STATE_SIZE, STATE_SIZE); //initial_xv.Resize(STATE_SIZE); //initial_Pxx.Resize(STATE_SIZE, STATE_SIZE); initial_xv.Fill(0.0f); initial_Pxx.Fill(0.0f); values = settings.get_entry("InitialState", "xv"); String xv_stream = (String)values[0]; initial_xv.ReadASCII(xv_stream); values = settings.get_entry("InitialState", "Pxx"); String Pxx_stream = (String)values[0]; initial_Pxx.ReadASCII(Pxx_stream); }