/// <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); }
/// <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); }
/// <summary> /// Fast version of predict filter /// </summary> /// <param name="scene"></param> /// <param name="u">Control vector of accelerations</param> /// <param name="delta_t">time step in seconds</param> public void predict_filter_fast(Scene_Single scene, Vector u, float delta_t) { Vector xv = scene.get_xv(); // Make model calculations: results are stored in RES matrices scene.get_motion_model().func_fv_and_dfv_by_dxv(xv, u, delta_t); scene.get_motion_model().func_Q(xv, u, delta_t); Vector fvRES = scene.get_motion_model().get_fvRES(); xv.Update(fvRES); Motion_Model mm = scene.get_motion_model(); MatrixFixed Pxx = scene.get_Pxx(); MatrixFixed dfv_by_dxvRES = mm.get_dfv_by_dxvRES(); MatrixFixed dfv_by_dxvRES_transposed = dfv_by_dxvRES.Transpose(); MatrixFixed QxRES = mm.get_QxRES(); Pxx.Update((dfv_by_dxvRES * Pxx * dfv_by_dxvRES_transposed) + QxRES); // Change the covariances between vehicle state and feature states foreach (Feature it in scene.feature_list) { MatrixFixed Pxy = it.get_Pxy(); Pxy.Update(dfv_by_dxvRES * Pxy); } }
/// <summary> /// Solve the matrix-vector system M x = y, returning x. /// </summary> /// <param name="y"></param> /// <returns></returns> public Vector Solve(Vector y) { // fsm sanity check : if (y.size() != U_.Rows) { Debug.WriteLine("__FILE__ : size of rhs is incompatible with no. of rows in U_ " + "y =" + Convert.ToString(y) + "\n" + "m_=" + Convert.ToString(m_) + "\n" + "n_=" + Convert.ToString(n_) + "\n" + "U_=\n" + Convert.ToString(U_) + "V_=\n" + Convert.ToString(V_) + "W_=\n" + W_); } Vector x = new Vector(V_.Rows); // Solution matrix. if (U_.Rows < U_.Columns) { // Augment y with extra rows of Vector yy = new Vector(U_.Rows); // zeros, so that it matches yy.Fill(0); if (yy.size() < y.size()) { // fsm Debug.WriteLine("yy=" + Convert.ToString(yy)); Debug.WriteLine("y =" + Convert.ToString(y)); // the update() call on the next line will abort... } yy.Update(y); // cols of u.transpose. x = U_.ConjugateTranspose() * yy; } else { x = U_.ConjugateTranspose() * y; } for (int i = 0; i < x.size(); i++) { // multiply with diagonal 1/W float weight = W_[i, i], zero_ = 0.0f; if (weight != zero_) { x[i] /= weight; } else { x[i] = zero_; } } return(V_ * x); // premultiply with v. }
public void construct_total_internal_measurement_stuff( Vector nu_tot, MatrixFixed dh_by_dx_tot, MatrixFixed R_tot, uint total_state_size) { //uint size = internal_measurement_model.MEASUREMENT_SIZE; //assert (nu_tot.Size() == size && //dh_by_dx_tot.Rows() == size && //dh_by_dx_tot.Cols() == total_state_size && //R_tot.Rows() == size && R_tot.Cols() == size); //assert(successful_internal_measurement_flag); nu_tot.Fill(0.0f); dh_by_dx_tot.Fill(0.0f); R_tot.Fill(0.0f); nu_tot.Update(nuv, 0); dh_by_dx_tot.Update(dhv_by_dxv, 0, 0); R_tot.Update(Rv, 0, 0); }
// FIXME. this should implement the above, not the other way round. /* * public void Solve(float[] y, float[] x) * { * Solve(new Vector(y, m_)).CopyOut(x); * } */ /// <summary> /// Solve the matrix-vector system M x = y. /// Assume that the singular values W have been preinverted by the caller. /// </summary> /// <param name="y"></param> /// <param name="x_out"></param> public void SolvePreinverted(Vector y, Vector x_out) { Vector x; // solution matrix if (U_.Rows < U_.Columns) { // augment y with extra rows of //std::cout << "svd<T>::solve_preinverted() -- Augmenting y\n"; Vector yy = new Vector(U_.Rows); // zeros, so that it match yy.Fill(0); yy.Update(y); // cols of u.transpose. ?? x = U_.ConjugateTranspose() * yy; } else { x = U_.ConjugateTranspose() * y; } for (int i = 0; i < x.size(); i++) // multiply with diagonal W, assumed inverted { x[i] *= W_[i, i]; } x_out = V_ * x; // premultiply with v. }
public void construct_total_measurement_stuff(Vector nu_tot, MatrixFixed dh_by_dx_tot, MatrixFixed R_tot) { uint size = successful_measurement_vector_size; //assert (nu_tot.Size() == size && //dh_by_dx_tot.Rows() == size && //dh_by_dx_tot.Cols() == total_state_size && //R_tot.Rows() == size && R_tot.Cols() == size); nu_tot.Fill(0.0f); dh_by_dx_tot.Fill(0.0f); R_tot.Fill(0.0f); uint vector_position = 0; Feature it; for (int i=0;i<selected_feature_list.Count;i++) { it = (Feature)selected_feature_list[i]; if (it.get_successful_measurement_flag()) { nu_tot.Update(it.get_nu(), (int)vector_position); dh_by_dx_tot.Update(it.get_dh_by_dxv(), (int)vector_position, 0); dh_by_dx_tot.Update(it.get_dh_by_dy(), (int)vector_position, (int)it.get_position_in_total_state_vector()); R_tot.Update(it.get_R(), (int)vector_position, (int)vector_position); vector_position += it.get_feature_measurement_model().MEASUREMENT_SIZE; } } }
//******************************Access functions******************************* /// <summary> /// Fills a vector with the state \vct{y}_i of the currently-marked feature. /// </summary> /// <param name="y_to_fill">The vector to fill with the state.</param> /// <returns>false if no feature is currently marked (or the marked label is unknown); true otherwise.</returns> public bool get_marked_feature_state(Vector y_to_fill) { if (marked_feature_label == -1) { Debug.WriteLine("No feature marked."); return false; } Feature it = null; bool found = false; for (int i=0;i<feature_list.Count;i++) { it = (Feature)feature_list[i]; if ((int)it.get_label() == marked_feature_label) { found = true; break; } } if (!found) { Debug.WriteLine("Error: marked feature not found in list."); return false; } else { y_to_fill.Update(it.get_y()); return true; } }
/// <summary> /// Create the overall state vector by concatenating the robot state $x_v$ and all the feature states y_i /// </summary> /// <param name="V">The vector to fill with the state</param> public void construct_total_state(ref Vector V) { //assert(V.Size() == total_state_size); int y_position = 0; V.Update(xv, y_position); y_position += (int)motion_model.STATE_SIZE; uint y_feature_no = 0; Feature it; for (int i=0;i<feature_list.Count;i++) { it = (Feature)feature_list[i]; V.Update(it.get_y(), y_position); y_feature_no++; y_position += (int)it.get_feature_measurement_model().FEATURE_STATE_SIZE; } //assert (y_feature_no == feature_list.size() && y_position == total_state_size); }
//*************************Some important operations*************************** public void zero_axes() { uint size = get_total_state_size(); Vector x = new Vector(size); x.Fill(0.0f); MatrixFixed dxnew_by_dxold = new MatrixFixed(size, size); dxnew_by_dxold.Fill(0.0f); // We form the new state and Jacobian of the new state w.r.t. the old state uint state_position = 0; // Robot part motion_model.func_zeroedxv_and_dzeroedxv_by_dxv(xv); x.Update(motion_model.get_zeroedxvRES(), 0); dxnew_by_dxold.Update(motion_model.get_dzeroedxv_by_dxvRES(), 0, 0); state_position += motion_model.STATE_SIZE; // Each feature in turn uint feature_no = 0; // Copy these to be on the safe side motion_model.func_xp(xv); Vector local_xp = new Vector(motion_model.get_xpRES()); //Vector local_xp = motion_model.get_xpRES(); motion_model.func_dxp_by_dxv(xv); //MatrixFixed local_dxp_by_dxv = motion_model.get_dxp_by_dxvRES(); MatrixFixed local_dxp_by_dxv = new MatrixFixed(motion_model.get_dxp_by_dxvRES()); Feature it; MatrixFixed Temp_FS1; for (int i=0; i < feature_list.Count; i++) { it = (Feature)feature_list[i]; it.get_feature_measurement_model().func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(it.get_y(), local_xp); x.Update(it.get_feature_measurement_model().get_zeroedyiRES(), (int)state_position); // Calculate dzeroedyi_by_dxv in Temp_FS1 Temp_FS1 = it.get_feature_measurement_model().get_dzeroedyi_by_dxpRES() * local_dxp_by_dxv; dxnew_by_dxold.Update(Temp_FS1, (int)state_position, 0); dxnew_by_dxold.Update(it.get_feature_measurement_model().get_dzeroedyi_by_dyiRES(), (int)state_position, (int)state_position); // We also want to redefine xp_orig for this feature: the robot // position at which is was initialised into the map (used by // functions for checking visibility) motion_model.func_xpredef_and_dxpredef_by_dxp_and_dxpredef_by_dxpdef( it.get_xp_orig(), local_xp); it.set_xp_orig(it.get_feature_measurement_model().get_motion_model().get_xpredefRES()); feature_no++; state_position += it.get_feature_measurement_model().FEATURE_STATE_SIZE; } // Check we've counted properly //assert (feature_no == feature_list.size() && // state_position == total_state_size); // Do a Jacobian transform to get the new covariance MatrixFixed P = new MatrixFixed(size, size); construct_total_covariance(ref P); P = dxnew_by_dxold * P * dxnew_by_dxold.Transpose(); // Finally load the scene data structures with the new state and covariance fill_states(x); fill_covariances(P); }
// FIXME. this should implement the above, not the other way round. /* public void Solve(float[] y, float[] x) { Solve(new Vector(y, m_)).CopyOut(x); } */ /// <summary> /// Solve the matrix-vector system M x = y. /// Assume that the singular values W have been preinverted by the caller. /// </summary> /// <param name="y"></param> /// <param name="x_out"></param> public void SolvePreinverted(Vector y, Vector x_out) { Vector x; // solution matrix if (U_.Rows < U_.Columns) { // augment y with extra rows of //std::cout << "svd<T>::solve_preinverted() -- Augmenting y\n"; Vector yy = new Vector(U_.Rows); // zeros, so that it match yy.Fill(0); yy.Update(y); // cols of u.transpose. ?? x = U_.ConjugateTranspose() * yy; } else x = U_.ConjugateTranspose() * y; for (int i = 0; i < x.size(); i++) // multiply with diagonal W, assumed inverted x[i] *= W_[i, i]; x_out = V_ * x; // premultiply with v. }
/// <summary> /// Solve the matrix-vector system M x = y, returning x. /// </summary> /// <param name="y"></param> /// <returns></returns> public Vector Solve(Vector y) { // fsm sanity check : if (y.size() != U_.Rows) { Debug.WriteLine("__FILE__ : size of rhs is incompatible with no. of rows in U_ " + "y =" + Convert.ToString(y) + "\n" + "m_=" + Convert.ToString(m_) + "\n" + "n_=" + Convert.ToString(n_) + "\n" + "U_=\n" + Convert.ToString(U_) + "V_=\n" + Convert.ToString(V_) + "W_=\n" + W_); } Vector x = new Vector(V_.Rows); // Solution matrix. if (U_.Rows < U_.Columns) { // Augment y with extra rows of Vector yy = new Vector(U_.Rows); // zeros, so that it matches yy.Fill(0); if (yy.size() < y.size()) { // fsm Debug.WriteLine("yy=" + Convert.ToString(yy)); Debug.WriteLine("y =" + Convert.ToString(y)); // the update() call on the next line will abort... } yy.Update(y); // cols of u.transpose. x = U_.ConjugateTranspose() * yy; } else x = U_.ConjugateTranspose() * y; for (int i = 0; i < x.size(); i++) { // multiply with diagonal 1/W float weight = W_[i, i], zero_ = 0.0f; if (weight != zero_) x[i] /= weight; else x[i] = zero_; } return V_ * x; // premultiply with v. }
/// <summary> /// Create a state vector x_v from its component parts. Puts the matrices r, q, v, omega into their right places. /// </summary> /// <param name="r"></param> /// <param name="q"></param> /// <param name="v"></param> /// <param name="omega"></param> /// <param name="xv"></param> public void compose_xv(ref Vector3D r, ref Quaternion q, ref Vector3D v, ref Vector3D omega, ref Vector xv) { xv.Update(r.GetVNL3(), 0); xv.Update(q.GetRXYZ(), 3); xv.Update(v.GetVNL3(), 7); xv.Update(omega.GetVNL3(), 10); }
// 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); }