/// <summary> /// Unpack and return R. /// </summary> /// <returns></returns> public MatrixFixed R() { if (R_ == null) { int m = qrdc_out_.Columns; // column-major storage int n = qrdc_out_.Rows; R_ = new MatrixFixed(m, n); MatrixFixed R = R_; for (int i = 0; i < m; ++i) { for (int j = 0; j < n; ++j) { if (i > j) { R[i, j] = 0.0f; } else { R[i, j] = qrdc_out_[j, i]; } } } } return(R_); }
/// <summary> /// Make a measurement of a feature. This function calls elliptical_search() to /// find the best match within three standard deviations of the predicted location. /// </summary> /// <param name="patch">The identifier for this feature (in this case an image patch)</param> /// <param name="z">The best image location match for the feature, to be filled in by this function</param> /// <param name="h">The expected image location</param> /// <param name="S">The expected location covariance, used to specify the search region.</param> /// <returns></returns> public override bool measure_feature(byte[] patch, int patchwidth, ref Vector z, Vector vz, Vector h, MatrixFixed S, Random rnd) { Cholesky S_cholesky = new Cholesky(S); MatrixFixed Sinv = S_cholesky.Inverse(); uint u_found = 0, v_found = 0; if (SceneLib.elliptical_search(image, image_width, image_height, patch, patchwidth, patchwidth, h, Sinv, ref u_found, ref v_found, vz, Camera_Constants.BOXSIZE, outputimage, outputimage_width, outputimage_height, show_ellipses, calibrating, rnd) != true) { // Feature not successfully matched return(false); } z.Put(0, (float)u_found); z.Put(1, (float)v_found); return(true); }
/// <summary> /// Compute inverse.\ Not efficient. /// </summary> /// <returns></returns> public unsafe MatrixFixed Inverse() { int n = A_.Columns; MatrixFixed I = new MatrixFixed(A_); float[] det = new float[2]; int job = 01; fixed(float *data = I.Datablock()) { fixed(float *data2 = det) { Netlib.dpodi_(data, &n, &n, data2, &job); } } // Copy lower triangle into upper for (int i = 0; i < n; ++i) { for (int j = i + 1; j < n; ++j) { I[i, j] = I[j, i]; } } return(I); }
// Set the current innovation covariance S_i^{-1} for this // particle. Called from // Scene_Single::predict_partially_initialised_feature_measurements() public void set_S(MatrixFixed Si) { Cholesky local_Si_cholesky = new Cholesky(Si); m_SInv.Update(local_Si_cholesky.Inverse()); m_detS = SceneLib.Determinant(Si); }
/// <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> /// Convert a vector into a 1-by-size matrix (i.e. a row vector). /// </summary> /// <returns></returns> public MatrixFixed AsRow() { MatrixFixed ret = new MatrixFixed(1, data.Length); ret.SetRow(0, this); return(ret); }
/// <summary> /// Convert a vector into a 1-by-size matrix (i.e. a column vector). /// </summary> /// <returns></returns> public MatrixFixed AsColumn() { MatrixFixed ret = new MatrixFixed(data.Length, 1); ret.SetColumn(0, this); return(ret); }
/// <summary> /// Calculate the Jacobian \partfrac{\vct{y}}{\vct{h}} for the /// Unproject() operation, for the most recent point that was projected. /// /// This is calculated in two stages: the Jacobian between undistorted and /// distorted image co-ordinates /// /// \partfrac{\vct{u}}{\vct{h}} = \emat{ /// \frac{ 2 u_c^2 k_1 }{ f^{\frac{3}{2}} } + /// \frac{ 1 }{ f^{\frac{1}{2}} } & /// \frac{ 2 u_c v_c k_1 }{ f^{\frac{3}{2}} } \\ /// \frac{ 2 v_c u_c k_1 }{ f^{\frac{3}{2}} } & /// \frac{ 2 v_c^2 k_1 }{ f^{\frac{3}{2}} } + /// \frac{ 1 }{ f^{\frac{1}{2}} } } = /// \frac{2 k_1}{f^\frac{3}{2}} \vct{u}_c \vct{u}_c^T + /// \emat{\frac{1}{\sqrt{f}} & 0 \\ /// 0 & \frac{1}{\sqrt{f}}} /// /// where f = 1 - 2 k_1 |\vct{u}_c|^2 and the Jacobian for the unprojection /// operation between image rays and (undistorted, centred) image co-ordinates /// /// \partfrac{ \vct{y} }{ \vct{u} } = \emat{ /// -\frac{ 1 }{ f_{ku} } & 0 \\ /// 0 & -\frac{ 1}{ f_{kv}} \\ /// 0 & 0 } /// /// The final Jacobian is then given by combining these two /// /// \partfrac{\vct{y}}{\vct{h}} = /// \partfrac{\vct{y}}{\vct{u}} \partfrac{\vct{u}}{\vct{h}} /// </summary> /// <returns></returns> public virtual MatrixFixed UnprojectionJacobian() { float[] a = { -1 / m_Fku, 0.0f, 0.0f, -1 / m_Fkv, 0.0f, 0.0f }; MatrixFixed dy_by_du = new MatrixFixed(3, 2, a); // Generate the outer product matrix first MatrixFixed du_by_dh = m_last_image_centred.AsColumn() * m_last_image_centred.AsRow(); // this matrix is not yet du_by_dh, it is just // [ uc*uc uc*vc ] // [ vc*uc vc*vc ] // The trace of this matrix gives the magnitude of the vector float radius2 = du_by_dh[0, 0] + du_by_dh[1, 1]; // Calculate various constants to save typing float distor = 1 - 2 * m_Kd1 * radius2; float distor1_2 = (float)Math.Sqrt(distor); float distor3_2 = distor1_2 * distor; // Now form the proper du_by_dh by manipulating the outer product matrix du_by_dh *= 2 * m_Kd1 / distor3_2; du_by_dh[0, 0] += (1 / distor1_2); du_by_dh[1, 1] += (1 / distor1_2); return(dy_by_du * du_by_dh); }
/// <summary> /// Solve the matrix equation M X = B, returning X. /// </summary> /// <param name="B"></param> /// <returns></returns> public MatrixFixed Solve(MatrixFixed B) { MatrixFixed x; // solution matrix if (U_.Rows < U_.Columns) { // augment y with extra rows of MatrixFixed yy = new MatrixFixed(U_.Rows, B.Columns); // zeros, so that it matches yy.Fill(0); yy.Update(B); // cols of u.transpose. ??? x = U_.ConjugateTranspose() * yy; } else { x = U_.ConjugateTranspose() * B; } int i, j; for (i = 0; i < x.Rows; i++) { // multiply with diagonal 1/W float weight = W_[i, i]; if (weight != 0) //vnl_numeric_traits<T>::zero) { weight = 1.0f / weight; } for (j = 0; j < x.Columns; j++) { x[i, j] *= weight; } } x = V_ * x; // premultiply with v. return(x); }
/// <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); }
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> /// update the position of the feature, but don't add it to the state /// </summary> /// <param name="lambda"></param> public void update_feature_position(Vector lambda) { 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()); }
/// <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(byte[] 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; } }
public void update_filter_internal_measurement_slow(Scene_Single scene, uint i) { // Size of measurement vector uint size = ((Internal_Measurement)(scene.internal_measurement_vector[(int)i])).get_internal_measurement_model().MEASUREMENT_SIZE; uint size2 = scene.get_total_state_size(); // Size of state vector Vector x = new Vector(size2); MatrixFixed P = new MatrixFixed(size2, size2); scene.construct_total_state_and_covariance(ref x, ref P); // cout << "x:" << x; // cout << "P:" << P; // 1. Form nu and dh_by_dx Vector nu_tot = new Vector(size); MatrixFixed dh_by_dx_tot = new MatrixFixed(size, size2); MatrixFixed R_tot = new MatrixFixed(size, size); scene.construct_total_internal_measurement_stuff(nu_tot, dh_by_dx_tot, R_tot, i); // 2. Calculate S(k+1) MatrixFixed S = new MatrixFixed(size, size); //MatrixFixed Tempss2 = new MatrixFixed(size, size2); //MatrixFixed Temps2s = new MatrixFixed(size2, size); MatrixFixed dh_by_dx_totT = dh_by_dx_tot.Transpose(); S.Update(dh_by_dx_tot * P * dh_by_dx_totT); S += R_tot; // cout << "R_tot:" << R_tot; // cout << "S:" << S; // cout << "dh_by_dx_tot:" << dh_by_dx_tot; // cout << "dh_by_dx_totT:" << dh_by_dx_totT; // 3. Calculate W(k+1) Cholesky S_cholesky = new Cholesky(S); MatrixFixed W = P * dh_by_dx_totT * S_cholesky.Inverse(); // cout << "W:" << W; // 4. Calculate x(k+1|k+1) x += W * nu_tot; // 5. Calculate P(k+1|k+1) P -= W * S * W.Transpose(); scene.fill_state_and_covariance(x, P); // cout << "x after update:" << x; // cout << "P after update:" << P; }
/// <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> /// 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); }
/// <summary> /// Calculate inverse of transpose. /// </summary> /// <returns></returns> public MatrixFixed TransposeInverse() { MatrixFixed Winverse = new MatrixFixed(Winverse_.Rows, Winverse_.Columns); Winverse.Fill(0); for (int i = 0; i < rank_; i++) { Winverse[i, i] = Winverse_[i, i]; } return(U_ * Winverse * V_.ConjugateTranspose()); }
/// <summary> /// Calculate pseudo-inverse. /// </summary> /// <param name="rank"></param> /// <returns></returns> public MatrixFixed PseudoInverse(int rank) { MatrixFixed Winverse = new MatrixFixed(Winverse_.Rows, Winverse_.Columns); Winverse.Fill(0); for (int i = 0; i < rank; i++) { Winverse[i, i] = Winverse_[i, i]; } return(V_ * Winverse * U_.ConjugateTranspose()); }
/// <summary> /// Recompose SVD to U*W*V'. /// </summary> /// <returns></returns> public MatrixFixed Recompose() { MatrixFixed W = new MatrixFixed(W_.Rows, W_.Columns); W.Fill(0); for (int i = 0; i < rank_; i++) { W[i, i] = W_[i, i]; } return(U_ * W * V_.ConjugateTranspose()); }
/// <summary> /// Returns the nxn outer product of two nd-vectors, or [v1]^T*[v2].\ O(n). /// </summary> /// <param name="v1"></param> /// <param name="v2"></param> /// <returns></returns> public static MatrixFixed OuterProduct(Vector v1, Vector v2) { MatrixFixed outp = new MatrixFixed(v1.size(), v2.size()); for (int i = 0; i < outp.Rows; i++) // v1.column() * v2.row() { for (int j = 0; j < outp.Columns; j++) { outp[i, j] = v1[i] * v2[j]; } } return(outp); }
//friend class Scene_Single; //private Random rnd = new Random(); //typedef Vector<Particle> as ParticleVector; /// <summary> /// Constructor. The class is intialised with no particles, so these must be added later using add_particle() /// </summary> /// <param name="sc">The Scene class.</param> /// <param name="f">The feature for which this class represents additional information.</param> public FeatureInitInfo(Scene_Single sc, Feature f) { scene = sc; fp = f; PARTICLE_DIMENSION = f.get_partially_initialised_feature_measurement_model().FREE_PARAMETER_SIZE; number_of_match_attempts = 0; nonzerovariance = false; mean = new Vector(PARTICLE_DIMENSION); covariance = new MatrixFixed(PARTICLE_DIMENSION, PARTICLE_DIMENSION); //if (Scene_Single::STATUSDUMP) cout << "PARTICLE_DIMENSION = " //<< PARTICLE_DIMENSION << endl; }
//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 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); } } } } }
/// <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 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; }
/// <summary> /// Return upper-triangular factor. /// </summary> /// <returns></returns> private MatrixFixed UpperTriangle() { int n = A_.Columns; MatrixFixed U = new MatrixFixed(n, n); // Zap lower triangle and transpose for (int i = 0; i < n; ++i) { U[i, i] = A_[i, i]; for (int j = i + 1; j < n; ++j) { U[i, j] = A_[j, i]; U[j, i] = 0; } } return(U); }
/// <summary> /// Return lower-triangular factor. /// </summary> /// <returns></returns> public MatrixFixed LowerTriangle() { int n = A_.Columns; MatrixFixed L = new MatrixFixed(n, n); // Zap upper triangle and transpose for (int i = 0; i < n; ++i) { L[i, i] = A_[i, i]; for (int j = i + 1; j < n; ++j) { L[j, i] = A_[j, i]; L[i, j] = 0; } } return(L); }
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()); }