public MatrixFixed(MatrixFixed m) { init(m.Rows, m.Columns); for (int i = 0; i < Rows; i++) for (int j = 0; j < Columns; j++) m_matrix[i, j] = m[i, j]; }
/// <summary> /// Base-class constructor. This should be called in the constructor of any /// derived class. /// </summary> /// <param name="position_state_size">The number of dimensions in the position state</param> /// <param name="state_size">The total state size</param> /// <param name="control_size">The control state size</param> /// <param name="m_m_d_t">A string representing the motion model dimensionality type </param> /// <param name="m_m_t">A unique string representing this particular motion model type</param> public Motion_Model(uint position_state_size, uint state_size, uint control_size, String m_m_d_t, String m_m_t) { POSITION_STATE_SIZE = position_state_size; STATE_SIZE = state_size; CONTROL_SIZE = control_size; motion_model_dimensionality_type = m_m_d_t; motion_model_type = m_m_t; fvRES = new Vector(STATE_SIZE); xpRES = new Vector(POSITION_STATE_SIZE); fv_noisyRES = new Vector(STATE_SIZE); xvredefRES = new Vector(STATE_SIZE); xvnormRES = new Vector(STATE_SIZE); zeroedxvRES = new Vector(STATE_SIZE); xpredefRES = new Vector(POSITION_STATE_SIZE); dfv_by_dxvRES = new MatrixFixed(STATE_SIZE, STATE_SIZE); QxRES = new MatrixFixed(STATE_SIZE, STATE_SIZE); dxp_by_dxvRES = new MatrixFixed(POSITION_STATE_SIZE, STATE_SIZE); dxvredef_by_dxvRES = new MatrixFixed(STATE_SIZE, STATE_SIZE); dxvredef_by_dxpdefRES = new MatrixFixed(STATE_SIZE, POSITION_STATE_SIZE); dxvnorm_by_dxvRES = new MatrixFixed(STATE_SIZE, STATE_SIZE); dzeroedxv_by_dxvRES = new MatrixFixed(STATE_SIZE, STATE_SIZE); dxpredef_by_dxpRES = new MatrixFixed(POSITION_STATE_SIZE, POSITION_STATE_SIZE); dxpredef_by_dxpdefRES = new MatrixFixed(POSITION_STATE_SIZE, POSITION_STATE_SIZE); }
/// <summary> /// Calculate trace of a matrix /// </summary> /// <param name="m"></param> /// <returns></returns> public static float Trace(MatrixFixed M) { float sum = 0; int N = (M.Rows < M.Columns ? M.Rows : M.Columns); for (int i=0; i<N; ++i) sum += M[i, i]; return sum; }
private void init() { m_centre = new Vector(2); m_C = new MatrixFixed(3, 3); m_Cinv = new MatrixFixed(3, 3); m_last_camera = new Vector(3); m_last_image_centred = new Vector(2); }
//friend class FeatureInitInfo; //friend class Scene_Single; /// <summary> /// Constructor. This is protected since it is only called /// from FeatureInitInfo::add_particle() /// </summary> /// <param name="l">The value(s) for the free parameters \lambda represented by this particle.</param> /// <param name="p">The initial probability for this particle</param> /// <param name="MEASUREMENT_SIZE">The number of parameters representing a measurement of a feature</param> public Particle(Vector l, float p, uint MEASUREMENT_SIZE) { lambda = new Vector(l); probability = p; m_z = new Vector(MEASUREMENT_SIZE); m_h = new Vector(MEASUREMENT_SIZE); m_SInv = new MatrixFixed(MEASUREMENT_SIZE, MEASUREMENT_SIZE); }
// Default NULL version public virtual bool make_internal_measurement(Internal_Measurement_Model m, Vector v, Vector v2, MatrixFixed mt) { Debug.WriteLine("WARNING: make_internal_measurement not implemented."); return false; }
/// <summary> /// Simple overall prediction /// </summary> /// <param name="scene"></param> /// <param name="u"></param> /// <param name="delta_t"></param> public void predict_filter_slow (Scene_Single scene, Vector u, float delta_t) { Debug.WriteLine("*** SLOW PREDICTION ***"); // What we need to do for the prediction: // Calculate f and grad_f_x // Calculate Q // Form x(k+1|k) and P(k+1|k) int size = (int)scene.get_total_state_size(); // First form original total state and covariance Vector x = new Vector(size); MatrixFixed P = new MatrixFixed(size, size); scene.construct_total_state_and_covariance(ref x, ref P); // Make model calculations: store results in RES matrices Vector xv = scene.get_xv(); //Vector xv = new Vector(scene.get_xv()); scene.get_motion_model().func_fv_and_dfv_by_dxv(xv, u, delta_t); scene.get_motion_model().func_Q(scene.get_xv(), u, delta_t); // Find new state f Vector f = new Vector(size); // Feature elements of f are the same as x f.Update(x); f.Update(scene.get_motion_model().get_fvRES(), 0); // Find new P // Since most elements of df_by_dx are zero... MatrixFixed df_by_dx = new MatrixFixed(size, size); df_by_dx.Fill(0.0f); // Fill the rest of the elements of df_by_dx: 1 on diagonal for features for (int i = (int)scene.get_motion_model().STATE_SIZE; i < df_by_dx.Rows; i++) df_by_dx[i,i] = 1.0f; df_by_dx.Update(scene.get_motion_model().get_dfv_by_dxvRES(), 0, 0); // Calculate the process noise MatrixFixed Q = new MatrixFixed(size, size); Q.Fill(0.0f); Q.Update(scene.get_motion_model().get_QxRES(), 0, 0); P.Update(df_by_dx * P * df_by_dx.Transpose()); P += Q; scene.fill_state_and_covariance(f, P); }
//friend class Scene_Single; //friend class Kalman; public Internal_Measurement(Internal_Measurement_Model i_m_m) { internal_measurement_model = i_m_m; hv = new Vector(internal_measurement_model.MEASUREMENT_SIZE); zv = new Vector(internal_measurement_model.MEASUREMENT_SIZE); nuv = new Vector(internal_measurement_model.MEASUREMENT_SIZE); dhv_by_dxv = new MatrixFixed(internal_measurement_model.MEASUREMENT_SIZE, internal_measurement_model.get_motion_model().STATE_SIZE); Rv = new MatrixFixed(internal_measurement_model.MEASUREMENT_SIZE, internal_measurement_model.MEASUREMENT_SIZE); Sv = new MatrixFixed(internal_measurement_model.MEASUREMENT_SIZE, internal_measurement_model.MEASUREMENT_SIZE); // if (DEBUGDUMP) cout << "Adding Internal Measurement type " // << internal_measurement_model.internal_type << endl; }
public void predict_internal_measurement(Vector xv, MatrixFixed Pxx) { internal_measurement_model.func_hv_and_dhv_by_dxv(xv); hv.Update(internal_measurement_model.get_hvRES()); dhv_by_dxv.Update(internal_measurement_model.get_dhv_by_dxvRES()); internal_measurement_model.func_Rv(hv); Rv.Update(internal_measurement_model.get_RvRES()); internal_measurement_model.func_Sv(Pxx, dhv_by_dxv, Rv); Sv.Update(internal_measurement_model.get_SvRES()); //if (DEBUGDUMP) cout << "Internal measurement prediction: hv " << endl //<< hv << endl; }
public SearchDatum(MatrixFixed _PuInv, Vector _search_centre) { PuInv = _PuInv; search_centre = _search_centre; result_flag = false; result_u = 0; result_v = 0; halfwidth = (uint)(SceneLib.NO_SIGMA / Math.Sqrt(PuInv[0, 0] - PuInv[0, 1] * PuInv[0, 1] / PuInv[1, 1])); halfheight = (uint)(SceneLib.NO_SIGMA / Math.Sqrt(PuInv[1, 1] - PuInv[0, 1] * PuInv[0, 1] / PuInv[0, 0])); if (halfwidth > 10) halfwidth = 10; if (halfheight > 10) halfheight = 10; }
/// <summary> /// Cholesky decomposition. /// Make cholesky decomposition of M optionally computing /// the reciprocal condition number. If mode is estimate_condition, the /// condition number and an approximate nullspace are estimated, at a cost /// of a factor of (1 + 18/n). Here's a table of 1 + 18/n: ///<pre> /// n: 3 5 10 50 100 500 1000 /// slowdown: 7.0f 4.6 2.8 1.4 1.18 1.04 1.02 /// </summary> /// <param name="M"></param> /// <param name="mode"></param> public unsafe void init(MatrixFixed M, Operation mode) { A_ = new MatrixFixed(M); int n = M.Columns; //assert(n == (int)(M.Rows())); num_dims_rank_def_ = -1; int num_dims_rank_def_temp = num_dims_rank_def_; // BJT: This warning is pointless - it often doesn't detect non symmetry and // if you know what you're doing you don't want to be slowed down // by a cerr /* if (Math.Abs(M[0,n-1] - M[n-1,0]) > 1e-8) { Debug.WriteLine("cholesky: WARNING: unsymmetric: " + M); } */ if (mode != Operation.estimate_condition) { // Quick factorization fixed (float* data = A_.Datablock()) { Netlib.dpofa_(data, &n, &n, &num_dims_rank_def_temp); } //if ((mode == Operation.verbose) && (num_dims_rank_def_temp != 0)) // Debug.WriteLine("cholesky:: " + Convert.ToString(num_dims_rank_def_temp) + " dimensions of non-posdeffness"); } else { Vector nullvector = new Vector(n); float rcond_temp = rcond_; fixed (float* data = A_.Datablock()) { fixed (float* data2 = nullvector.Datablock()) { Netlib.dpoco_(data, &n, &n, &rcond_temp, data2, &num_dims_rank_def_temp); } } rcond_ = rcond_temp; } num_dims_rank_def_ = num_dims_rank_def_temp; }
public unsafe QR(MatrixFixed M) { qrdc_out_ = new MatrixFixed(M.Columns, M.Rows); qraux_ = new Vector(M.Columns); jpvt_ = new int[M.Rows]; Q_ = null; R_ = null; // Fill transposed O/P matrix int c = M.Columns; int r = M.Rows; for (int i = 0; i < r; ++i) for (int j = 0; j < c; ++j) qrdc_out_[j,i] = M[i,j]; int do_pivot = 0; // Enable[!=0]/disable[==0] pivoting. for (int i = 0; i < jpvt_.Length; i++) jpvt_[i] = 0; Vector work = new Vector(M.Rows); fixed (float* data = qrdc_out_.Datablock()) { fixed (float* data2 = qraux_.Datablock()) { fixed (int* data3 = jpvt_) { fixed (float* data4 = work.Datablock()) { Netlib.dqrdc_(data, // On output, UT is R, below diag is mangled Q &r, &r, &c, data2, // Further information required to demangle Q data3, data4, &do_pivot); } } } } }
// Set the Jacobian \partfracv{h}{y} // between the feature measurement state and the feature state. public void set_dh_by_dy(MatrixFixed new_dh_by_dy) { dh_by_dy.Update(new_dh_by_dy); }
// Set the covariance, \mat{P}_{xy}, between the feature state and // the robot state. public void set_Pxy(MatrixFixed new_Pxy) { Pxy.Update(new_Pxy); }
/// <summary> /// Make a measurement of a feature. /// </summary> /// <param name="id">The identifier for this feature</param> /// <param name="z">The measurement of the state, to be filled in by this function</param> /// <param name="h">The expected measurement</param> /// <param name="S">The measurement covariance.</param> /// <returns></returns> public virtual bool measure_feature(byte[] id, int patchwidth, ref Vector z, Vector vz, Vector h, MatrixFixed S, Random rnd) { return (false); }
/// <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); }
// Set the feature state covariance, \mat{P}_{yy}. public void set_Pyy(MatrixFixed new_Pyy) { Pyy.Update(new_Pyy); }
/// <summary> /// Function which unites common stuff for constructors below /// Constructor stuff which is common to more than one constructor /// </summary> protected void feature_constructor_bookeeping() { selected_flag = false; // Feature is unselected when first detected scheduled_for_termination_flag = false; attempted_measurements_of_feature = 0; successful_measurements_of_feature = 0; // Allocate matrices for storing predicted and actual measurements h = new Vector(feature_measurement_model.MEASUREMENT_SIZE); z = new Vector(feature_measurement_model.MEASUREMENT_SIZE); prev_z = new Vector(feature_measurement_model.MEASUREMENT_SIZE); nu = new Vector(feature_measurement_model.MEASUREMENT_SIZE); vz = new Vector(2); accn_z = new Vector(2); dh_by_dxv = new MatrixFixed(feature_measurement_model.MEASUREMENT_SIZE, feature_measurement_model.get_motion_model().STATE_SIZE); dh_by_dy = new MatrixFixed(feature_measurement_model.MEASUREMENT_SIZE, feature_measurement_model.FEATURE_STATE_SIZE); R = new MatrixFixed(feature_measurement_model.MEASUREMENT_SIZE, feature_measurement_model.MEASUREMENT_SIZE); S = new MatrixFixed(feature_measurement_model.MEASUREMENT_SIZE, feature_measurement_model.MEASUREMENT_SIZE); }
/// <summary> /// Make a measurement of a feature. /// </summary> /// <param name="id">The identifier for this feature</param> /// <param name="z">The measurement of the state, to be filled in by this function</param> /// <param name="h">The expected measurement</param> /// <param name="S">The measurement covariance.</param> /// <returns></returns> public virtual bool measure_feature(classimage_mono id, ref Vector z, Vector vz, Vector h, MatrixFixed S, Random rnd) { return (false); }
public override float selection_score(MatrixFixed m) { // Always measureable for now return 100000.0f; }
public override void func_zeroedyigraphics_and_Pzeroedyigraphics(Vector yi, Vector xv, MatrixFixed Pxx, MatrixFixed Pxyi, MatrixFixed Pyiyi) { ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.func_xp(xv); // In this case (where the feature state is the same as the graphics // state) zeroedyigraphics is the same as zeroedyi func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(yi, ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.get_xpRES()); zeroedyigraphicsRES.Update(zeroedyiRES); MatrixFixed dzeroedyigraphics_by_dxv = dzeroedyi_by_dxpRES * ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.get_dxp_by_dxvRES(); PzeroedyigraphicsRES.Update(dzeroedyigraphics_by_dxv * Pxx * dzeroedyigraphics_by_dxv.Transpose() + dzeroedyi_by_dyiRES * Pxyi.Transpose() * dzeroedyigraphics_by_dxv.Transpose() + dzeroedyigraphics_by_dxv * Pxyi * dzeroedyi_by_dyiRES.Transpose() + dzeroedyi_by_dyiRES * Pyiyi * dzeroedyi_by_dyiRES.Transpose()); }
public override float selection_score(MatrixFixed Si) { // Return the trace of the innovation covariance return SceneLib.Trace(Si); }
// In this case the graphics representation y_i^{graphics} and its // covariance is the same as the feature state y_i and covariance. public override void func_yigraphics_and_Pyiyigraphics(Vector yi, MatrixFixed Pyiyi) { // The graphics representation is the same as the state yigraphicsRES.Update(yi); PyiyigraphicsRES.Update(Pyiyi); }
// Set the covariance of the measurement \mat{R} // (for example, its uncertainty due to image resolution). public void set_R(MatrixFixed new_R) { R.Update(new_R); }
public override void func_yigraphics_and_Pyiyigraphics(Vector yi, MatrixFixed Pyiyi) { yigraphicsRES.Update(yi); PyiyigraphicsRES.Update(Pyiyi); }
// Set the innovation covariance \mat{S} // (for example, the overall uncertainty in image location). public void set_S(MatrixFixed new_S) { S.Update(new_S); }
public override void func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(Vector yi, Vector xp) { Wide_Camera_Point_Feature_Measurement_Model wm = (Wide_Camera_Point_Feature_Measurement_Model)wide_model; // Extract cartesian and quaternion components of xp wm.threed_motion_model.func_r(xp); wm.threed_motion_model.func_q(xp); // Extract ri and hhati components of yi = ypi func_ri(yi); func_hhati(yi); // ri part: transformation is just the same as in the normal point case // zeroedri = RRW(rWi - rW) //commented out in original code // ri - r Vector3D yWiminusrW = new Vector3D(riRES - wm.threed_motion_model.get_rRES()); Quaternion qRW = wm.threed_motion_model.get_qRES().Inverse(); MatrixFixed dqRW_by_dq = MatrixFixed.dqbar_by_dq(); // Rotation RRW RotationMatrix RRW = qRW.RotationMatrix(); // RRW(rWi - rW) Vector3D zeroedri = new Vector3D(RRW * yWiminusrW); // Now calculate Jacobians // dzeroedri_by_dri is RRW // dzeroedri_by_dhhati = 0 MatrixFixed dzeroedri_by_dri = new MatrixFixed(RRW); // dzeroedyi_by_dxp: // dzeroedri_by_dr = -RRW // dzeroedri_by_dq = d_dq(RRW (ri - r)) MatrixFixed dzeroedri_by_dr = RRW * -1.0f; MatrixFixed dzeroedri_by_dqRW = MatrixFixed.dRq_times_a_by_dq(qRW, yWiminusrW); MatrixFixed dzeroedri_by_dq = dzeroedri_by_dqRW * dqRW_by_dq; // Now for the hhati part (easier...) // zeroedhhati = RRW hhati Vector3D zeroedhhati = new Vector3D(RRW * hhatiRES); // Jacobians // dzeroedhhati_by_dr = 0 // dzeroedhhati_by_dq = d_dq(RRW hhati) // dzeroedhhati_by_dhhati = RRW // dzeroedhhati_by_dri = 0 MatrixFixed dzeroedhhati_by_dqRW = MatrixFixed.dRq_times_a_by_dq(qRW, hhatiRES); MatrixFixed dzeroedhhati_by_dq = dzeroedhhati_by_dqRW * dqRW_by_dq; MatrixFixed dzeroedhhati_by_dhhati = new MatrixFixed(RRW); // And put it all together zeroedyiRES.Update(zeroedri.GetVNL3(), 0); zeroedyiRES.Update(zeroedhhati.GetVNL3(), 3); //cout << "Line: zeroedri = " << zeroedri << "zeroedhhati = " << zeroedhhati; dzeroedyi_by_dxpRES.Fill(0.0f); dzeroedyi_by_dxpRES.Update(dzeroedri_by_dr, 0, 0); dzeroedyi_by_dxpRES.Update(dzeroedri_by_dq, 0, 3); dzeroedyi_by_dxpRES.Update(dzeroedhhati_by_dq, 3, 3); dzeroedyi_by_dyiRES.Fill(0.0f); dzeroedyi_by_dyiRES.Update(dzeroedri_by_dri, 0, 0); dzeroedyi_by_dyiRES.Update(dzeroedhhati_by_dhhati, 3, 3); }
/// <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; }
/// <summary> /// Predict the image location \vct{h}_i = (x,y) for /// partially-initialised feature with state \vct{y}_i , given the current /// camera location \vct{x}_p and the depth parameter \lambda . /// </summary> /// <param name="yi"></param> /// <param name="xp"></param> /// <param name="lambda"></param> public override void func_hpi_and_dhpi_by_dxp_and_dhpi_by_dyi(Vector yi, Vector xp, Vector lambda) { // This function gives relative position of feature: also call this hR // (vector from camera to feature in robot frame) func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(yi, xp); // Parameters of vector hLR from camera to feature in robot frame // hLR = zeroedri + lambda * zeroedhhati // Calculate the vector from the camera to the feature given the current // lambda Vector hLR = zeroedyiRES.Extract(3, 0) + zeroedyiRES.Extract(3, 3) * lambda[0]; // Project this into the image hpiRES = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).m_camera.Project(hLR); // What is the Jacobian of this projection? MatrixFixed dhpi_by_dhLRi = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).m_camera.ProjectionJacobian(); // Calculate the required result Jacobians // Now how the vector to the feature depends on the parameterised line // (this is a function of lambda) float[] b = new float[18]; b[0] = 1.0f; b[1] = 0.0f; b[2] = 0.0f; b[3] = lambda[0]; b[4] = 0.0f; b[5] = 0.0f; b[6] = 0.0f; b[7] = 1.0f; b[8] = 0.0f; b[9] = 0.0f; b[10] = lambda[0]; b[11] = 0.0f; b[12] = 0.0f; b[13] = 0.0f; b[14] = 1.0f; b[15] = 0.0f; b[16] = 0.0f; b[17] = lambda[0]; MatrixFixed dhLRi_by_dzeroedyi = new MatrixFixed(3, 6, b); dhpi_by_dxpRES = dhpi_by_dhLRi * dhLRi_by_dzeroedyi * dzeroedyi_by_dxpRES; dhpi_by_dyiRES = dhpi_by_dhLRi * dhLRi_by_dzeroedyi * dzeroedyi_by_dyiRES; /* if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("func_hpi: yi = " + yi + "," + "xp = " + xp + "," + "lambda = " + lambda + "," + "hpiRES = " + hpiRES); */ }
/// <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; } }