/// <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; } } }
//*************************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. }
private unsafe void init(MatrixFixed M, float zero_out_tol) { m_ = M.Rows; n_ = M.Columns; U_ = new MatrixFixed(m_, n_); W_ = new DiagMatrix(n_); Winverse_ = new DiagMatrix(n_); V_ = new MatrixFixed(n_, n_); //assert(m_ > 0); //assert(n_ > 0); int n = M.Rows; int p = M.Columns; int mm = Netlib.min(n+1,p); // Copy source matrix into fortran storage // SVD is slow, don't worry about the cost of this transpose. Vector X = Vector.fortran_copy(M); // Make workspace vectors Vector work = new Vector(n); work.Fill(0); Vector uspace = new Vector(n*p); uspace.Fill(0); Vector vspace = new Vector(p*p); vspace.Fill(0); Vector wspace = new Vector(mm); wspace.Fill(0); // complex fortran routine actually _wants_ complex W! Vector espace = new Vector(p); espace.Fill(0); // Call Linpack SVD int info = 0; int job = 21; fixed (float* data = X.Datablock()) { fixed (float* data2 = wspace.Datablock()) { fixed (float* data3 = espace.Datablock()) { fixed (float* data4 = uspace.Datablock()) { fixed (float* data5 = vspace.Datablock()) { fixed (float* data6 = work.Datablock()) { Netlib.dsvdc_(data, &n, &n, &p, data2, data3, data4, &n, data5, &p, data6, &job, &info); } } } } } } // Error return? if (info != 0) { // If info is non-zero, it contains the number of singular values // for this the SVD algorithm failed to converge. The condition is // not bogus. Even if the returned singular values are sensible, // the singular vectors can be utterly wrong. // It is possible the failure was due to NaNs or infinities in the // matrix. Check for that now. M.assert_finite(); // If we get here it might be because // 1. The scalar type has such // extreme precision that too few iterations were performed to // converge to within machine precision (that is the svdc criterion). // One solution to that is to increase the maximum number of // iterations in the netlib code. // // 2. The LINPACK dsvdc_ code expects correct IEEE rounding behaviour, // which some platforms (notably x86 processors) // have trouble doing. For example, gcc can output // code in -O2 and static-linked code that causes this problem. // One solution to this is to persuade gcc to output slightly different code // by adding and -fPIC option to the command line for v3p\netlib\dsvdc.c. If // that doesn't work try adding -ffloat-store, which should fix the problem // at the expense of being significantly slower for big problems. Note that // if this is the cause, vxl/vnl/tests/test_svd should have failed. // // You may be able to diagnose the problem here by printing a warning message. Debug.WriteLine("__FILE__ : suspicious return value (" + Convert.ToString(info) + ") from SVDC" + "__FILE__ : M is " + Convert.ToString(M.Rows) + "x" + Convert.ToString(M.Columns)); valid_ = false; } else valid_ = true; // Copy fortran outputs into our storage int ctr = 0; for (int j = 0; j < p; ++j) for (int i = 0; i < n; ++i) { U_[i, j] = uspace[ctr]; ctr++; } for (int j = 0; j < mm; ++j) W_[j, j] = Math.Abs(wspace[j]); // we get rid of complexness here. for (int j = mm; j < n_; ++j) W_[j, j] = 0; ctr = 0; for (int j = 0; j < p; ++j) for (int i = 0; i < p; ++i) { V_[i, j] = vspace[ctr]; ctr++; } //if (test_heavily) { // Test that recomposed matrix == M //float recomposition_residual = Math.Abs((Recompose() - M).FrobeniusNorm()); //float n2 = Math.Abs(M.FrobeniusNorm()); //float thresh = m_ * (float)(eps) * n2; //if (recomposition_residual > thresh) { //std::cerr << "VNL::SVD<T>::SVD<T>() -- Warning, recomposition_residual = " //<< recomposition_residual << std::endl //<< "FrobeniusNorm(M) = " << n << std::endl //<< "eps*FrobeniusNorm(M) = " << thresh << std::endl //<< "Press return to continue\n"; //char x; //std::cin.get(&x, 1, '\n'); } } if (zero_out_tol >= 0) // Zero out small sv's and update rank count. ZeroOutAbsolute((float)(+zero_out_tol)); else // negative tolerance implies relative to max elt. ZeroOutRelative((float)(-zero_out_tol)); }
/// <summary> /// Form the covariance matrix Q of the process noise associated with x_v . /// </summary> /// <param name="xv"></param> /// <param name="v"></param> /// <param name="delta_t"></param> public override void func_Q(Vector xv, Vector v, float delta_t) { // Fill noise covariance matrix Pnn: this is the covariance of // the noise vector (V) // (Omega) // that gets added to the state. // Form of this could change later, but for now assume that // V and Omega are independent, and that each of their components is // independent... float linear_velocity_noise_variance = SD_A_component_filter * SD_A_component_filter * delta_t * delta_t; float angular_velocity_noise_variance = SD_alpha_component_filter * SD_alpha_component_filter * delta_t * delta_t; // Independence means that the matrix is diagonal MatrixFixed Pnn = new MatrixFixed(6, 6); Pnn.Fill(0.0f); Pnn.Put(0, 0, linear_velocity_noise_variance); Pnn.Put(1, 1, linear_velocity_noise_variance); Pnn.Put(2, 2, linear_velocity_noise_variance); Pnn.Put(3, 3, angular_velocity_noise_variance); Pnn.Put(4, 4, angular_velocity_noise_variance); Pnn.Put(5, 5, angular_velocity_noise_variance); // Form Jacobian dxnew_by_dn // Is like this: // I * delta_t 0 // 0 dqnew_by_dOmega // Start by zeroing MatrixFixed dxnew_by_dn = new MatrixFixed(7, 6); dxnew_by_dn.Fill(0.0f); // The translation part is just I \Delta t MatrixFixed Temp33A = new MatrixFixed(3, 3); Temp33A.SetIdentity(); Temp33A *= delta_t; dxnew_by_dn.Update(Temp33A, 0, 0); // qnew = q x \Omega \Deltat // dqnew_by_d\Omega = dqnew_by_d\Omega\Delta t . d\Omega\Delta t_by_d\Omega // Get the first part Vector qRXYZ = xv.Extract(4, 3); Quaternion qold = new Quaternion(); qold.SetRXYZ(qRXYZ); MatrixFixed Temp44A = MatrixFixed.dq3_by_dq1(qold); // Use function below for dqwt_by_dOmega Vector Omega = new Vector(3); Omega.Fill(SD_alpha_component_filter); MatrixFixed Temp43A = new MatrixFixed(4, 3); dqomegadt_by_domega(new Vector3D(Omega), delta_t, Temp43A); // Multiply them together MatrixFixed Temp43B = Temp44A * Temp43A; // And then plug into Jacobian dxnew_by_dn.Update(Temp43B, 3, 3); // Finally do Q = dxnew_by_dn . Pnn . dxnew_by_dnT QxRES.Update(dxnew_by_dn * Pnn * dxnew_by_dn.Transpose()); }
private unsafe void init(MatrixFixed M, float zero_out_tol) { m_ = M.Rows; n_ = M.Columns; U_ = new MatrixFixed(m_, n_); W_ = new DiagMatrix(n_); Winverse_ = new DiagMatrix(n_); V_ = new MatrixFixed(n_, n_); //assert(m_ > 0); //assert(n_ > 0); int n = M.Rows; int p = M.Columns; int mm = Netlib.min(n + 1, p); // Copy source matrix into fortran storage // SVD is slow, don't worry about the cost of this transpose. Vector X = Vector.fortran_copy(M); // Make workspace vectors Vector work = new Vector(n); work.Fill(0); Vector uspace = new Vector(n * p); uspace.Fill(0); Vector vspace = new Vector(p * p); vspace.Fill(0); Vector wspace = new Vector(mm); wspace.Fill(0); // complex fortran routine actually _wants_ complex W! Vector espace = new Vector(p); espace.Fill(0); // Call Linpack SVD int info = 0; int job = 21; fixed(float *data = X.Datablock()) { fixed(float *data2 = wspace.Datablock()) { fixed(float *data3 = espace.Datablock()) { fixed(float *data4 = uspace.Datablock()) { fixed(float *data5 = vspace.Datablock()) { fixed(float *data6 = work.Datablock()) { Netlib.dsvdc_(data, &n, &n, &p, data2, data3, data4, &n, data5, &p, data6, &job, &info); } } } } } } // Error return? if (info != 0) { // If info is non-zero, it contains the number of singular values // for this the SVD algorithm failed to converge. The condition is // not bogus. Even if the returned singular values are sensible, // the singular vectors can be utterly wrong. // It is possible the failure was due to NaNs or infinities in the // matrix. Check for that now. M.assert_finite(); // If we get here it might be because // 1. The scalar type has such // extreme precision that too few iterations were performed to // converge to within machine precision (that is the svdc criterion). // One solution to that is to increase the maximum number of // iterations in the netlib code. // // 2. The LINPACK dsvdc_ code expects correct IEEE rounding behaviour, // which some platforms (notably x86 processors) // have trouble doing. For example, gcc can output // code in -O2 and static-linked code that causes this problem. // One solution to this is to persuade gcc to output slightly different code // by adding and -fPIC option to the command line for v3p\netlib\dsvdc.c. If // that doesn't work try adding -ffloat-store, which should fix the problem // at the expense of being significantly slower for big problems. Note that // if this is the cause, vxl/vnl/tests/test_svd should have failed. // // You may be able to diagnose the problem here by printing a warning message. Debug.WriteLine("__FILE__ : suspicious return value (" + Convert.ToString(info) + ") from SVDC" + "__FILE__ : M is " + Convert.ToString(M.Rows) + "x" + Convert.ToString(M.Columns)); valid_ = false; } else { valid_ = true; } // Copy fortran outputs into our storage int ctr = 0; for (int j = 0; j < p; ++j) { for (int i = 0; i < n; ++i) { U_[i, j] = uspace[ctr]; ctr++; } } for (int j = 0; j < mm; ++j) { W_[j, j] = Math.Abs(wspace[j]); // we get rid of complexness here. } for (int j = mm; j < n_; ++j) { W_[j, j] = 0; } ctr = 0; for (int j = 0; j < p; ++j) { for (int i = 0; i < p; ++i) { V_[i, j] = vspace[ctr]; ctr++; } } //if (test_heavily) { // Test that recomposed matrix == M //float recomposition_residual = Math.Abs((Recompose() - M).FrobeniusNorm()); //float n2 = Math.Abs(M.FrobeniusNorm()); //float thresh = m_ * (float)(eps) * n2; //if (recomposition_residual > thresh) { //std::cerr << "VNL::SVD<T>::SVD<T>() -- Warning, recomposition_residual = " //<< recomposition_residual << std::endl //<< "FrobeniusNorm(M) = " << n << std::endl //<< "eps*FrobeniusNorm(M) = " << thresh << std::endl //<< "Press return to continue\n"; //char x; //std::cin.get(&x, 1, '\n'); } } if (zero_out_tol >= 0) { // Zero out small sv's and update rank count. ZeroOutAbsolute((float)(+zero_out_tol)); } else { // negative tolerance implies relative to max elt. ZeroOutRelative((float)(-zero_out_tol)); } }
/// <summary> /// Step the MonoSLAM application on by one frame. This should be called every time /// a new frame is captured (and care should be taken to avoid skipping frames). /// Before calling this function, Scene_Single::load_new_image() should be called /// (e.g. using /// <code>monoslaminterface.GetRobotNoConst()->load_new_image()</code>), since the /// first parameter to GoOneStep() is currently ignored. /// /// GoOneStep() performs the following processing steps: /// - Kalman filter prediction step (by calling Kalman::predict_filter_fast(), /// with a zero control vector) /// - Select a set of features to make measurements from (set by /// SetNumberOfFeaturesToSelect()) /// - Predict the locations and and make measurements of those features /// - Kalman filter update step /// - Delete any bad features (those that have repeatedly failed to be matched) /// - If we are not currently initialising a enough new features, and the camera is /// translating, and <code>currently_mapping_flag</code> is set, initialise a new /// feature somewhere sensible /// - Update the partially-initialised features /// </summary> /// <param name="img">camera image</param> /// <param name="delta_t">The time between frames in seconds</param> /// <param name="currently_mapping_flag">Set to be true if new features should be detected and added to the map.</param> /// <returns></returns> public bool GoOneStep(float delta_t, bool currently_mapping_flag) { if (delta_t > 0) { // update the integral image for use in feature detection //img.updateIntegralImage(); // nullify image selection robot.nullify_image_selection(); init_feature_search_region_defined_flag = false; // Control vector of accelerations Vector u = new Vector(3); u.Fill(0.0f); sim_or_rob.set_control(u, delta_t); // Record the current position so that I can estimate velocity // (We can guarantee that the state vector has position; we can't // guarantee that it has velocity.) Vector xv = scene.get_xv(); scene.get_motion_model().func_xp(xv); Vector prev_xp_pos = (scene.get_motion_model().get_xpRES()).Extract(3); // Prediction step if (currently_mapping_flag) kalman.predict_filter_fast(scene, u, delta_t); // if features are not seen the first time try a few more times int tries = 0; number_of_visible_features = 0; while (((tries == 0) || (scene.get_no_selected() < 2)) && (tries < 5)) { number_of_visible_features = scene.auto_select_n_features(NUMBER_OF_FEATURES_TO_SELECT); if (scene.get_no_selected() > 0) { //scene.predict_measurements(); // commented out in original code number_of_matched_features = (uint)SceneLib.make_measurements(scene, sim_or_rob, rnd); if (scene.get_successful_measurement_vector_size() != 0) { // this function is the slowest part of the algorithm kalman.total_update_filter_slow(scene); scene.normalise_state(); } } tries++; } if (currently_mapping_flag) scene.delete_bad_features(); // Let's enforce symmetry of covariance matrix... // Add to transpose and divide by 2 uint tot_state_size = scene.get_total_state_size(); MatrixFixed Pxx = new MatrixFixed(tot_state_size, tot_state_size); scene.construct_total_covariance(ref Pxx); MatrixFixed PxxT = Pxx.Transpose(); Pxx.Update(Pxx * 0.5f + PxxT * 0.5f); scene.fill_covariances(Pxx); // Look at camera speed estimate // Get the current position and estimate the speed from it xv = scene.get_xv(); scene.get_motion_model().func_xp(xv); Vector xp_pos = scene.get_motion_model().get_xpRES().Extract(3); velocity = (xp_pos - prev_xp_pos) / delta_t; speed = (float)Math.Sqrt(velocity.SquaredMagnitude()); // This section of code is a workaround for a situation where // the camera suddenly shoots off at high speed, which must be // a bug perhaps in the state update. If the camera suddenly accelerates // at a high rate then this just sets the state back to the previous one. if (prev_speed == 0) prev_speed = speed; else { float speed_change = speed / prev_speed; if ((speed > 1) && (speed_change > 1.2) && (prev_xv != null)) { xv.Update(prev_xv); } } prev_speed = speed; if (prev_xv != null) // TODO: minor bug here with vector != operator prev_xv.Update(xv); else prev_xv = new Vector(xv); if (currently_mapping_flag) { if (speed > 0.2) { if ((number_of_visible_features < NUMBER_OF_FEATURES_TO_KEEP_VISIBLE) && (scene.get_feature_init_info_vector().Count < (uint)(MAX_FEATURES_TO_INIT_AT_ONCE))) { // if the number of features is low make more attempts // to initialise new ones tries = 1; if (number_of_visible_features < 8) tries = 2; if (number_of_visible_features < 5) tries = 3; for (int i = 0; i < tries; i++) AutoInitialiseFeature(u, delta_t); } } } MatchPartiallyInitialisedFeatures(); recordCameraHistory(); } return true; }
/// <summary> /// Unpack and return unitary part Q. /// </summary> /// <returns></returns> public MatrixFixed Q() { int m = qrdc_out_.Columns; // column-major storage int n = qrdc_out_.Rows; bool verbose = false; if (Q_ == null) { Q_ = new MatrixFixed(m, m); // extract Q. if (verbose) { Debug.WriteLine("__FILE__: VNL::QR<T>Q()"); Debug.WriteLine("m,n = " + Convert.ToString(m) + ", " + Convert.ToString(n)); Debug.WriteLine("qr0 = [" + qrdc_out_ + "];"); Debug.WriteLine("aux = [" + qraux_ + "];"); } Q_.SetIdentity(); MatrixFixed Q = Q_; Vector v = new Vector(m); v.Fill(0); Vector w = new Vector(m); w.Fill(0); // Golub and vanLoan, p199. backward accumulation of householder matrices // Householder vector k is [zeros(1,k-1) qraux_[k] qrdc_out_[k,:]] for (int k = n - 1; k >= 0; --k) { if (k >= m) { continue; } // Make housevec v, and accumulate norm at the same time. v[k] = qraux_[k]; float sq = (v[k] * v[k]); for (int j = k + 1; j < m; ++j) { v[j] = qrdc_out_[k, j]; sq += (v[j] * v[j]); } //if (verbose) MatlabPrint(std::cerr, v, "v"); // Premultiply emerging Q by house(v), noting that v[0..k-1] == 0. // Q_new = (1 - (2/v'*v) v v')Q // or Q -= (2/v'*v) v (v'Q) if (sq > 0.0) { float scale = 2.0f / sq; // w = (2/v'*v) v' Q for (int i = k; i < m; ++i) { w[i] = 0.0f; for (int j = k; j < m; ++j) { w[i] += scale * v[j] * Q[j, i]; } //w[i] += scale * ComplexTraits.Conjugate(v[j]) * Q[j, i]; } //if (verbose) VNL::MatlabPrint(std::cerr, w, "w"); // Q -= v w for (int i = k; i < m; ++i) { for (int j = k; j < m; ++j) { Q[i, j] -= (v[i]) * (w[j]); } } } } } return(Q_); }
/// <summary> /// Construct a diagonal matrix with diagonal elements equal to value. /// </summary> /// <param name="nn"></param> /// <param name="value"></param> public DiagMatrix(int nn, float value) { diagonal_ = new Vector(nn); diagonal_.Fill(value); }
/// <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); }
/// <summary> /// Read the initial state vector and covariance from a stream. /// 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="stream"></param> /// <param name="initial_xv"></param> /// <param name="initial_Pxx"></param> public void read_initial_state(BinaryReader stream, ref Vector initial_xv, ref MatrixFixed initial_Pxx) { // Make sure the vector and matrix are the correct sizes initial_xv.Resize(STATE_SIZE); initial_Pxx.Resize(STATE_SIZE, STATE_SIZE); initial_xv.Fill(0.0f); initial_Pxx.Fill(0.0f); for (int r = 0; r < STATE_SIZE; r++) initial_xv[r] = stream.ReadSingle(); for (int r = 0; r < STATE_SIZE; r++) for (int c = 0; c < STATE_SIZE; c++) initial_Pxx[r, c] = stream.ReadSingle(); }
/// <summary> /// Unpack and return unitary part Q. /// </summary> /// <returns></returns> public MatrixFixed Q() { int m = qrdc_out_.Columns; // column-major storage int n = qrdc_out_.Rows; bool verbose = false; if (Q_ == null) { Q_ = new MatrixFixed(m,m); // extract Q. if (verbose) { Debug.WriteLine("__FILE__: VNL::QR<T>Q()"); Debug.WriteLine("m,n = " + Convert.ToString(m) + ", " + Convert.ToString(n)); Debug.WriteLine("qr0 = [" + qrdc_out_ + "];"); Debug.WriteLine("aux = [" + qraux_ + "];"); } Q_.SetIdentity(); MatrixFixed Q = Q_; Vector v = new Vector(m); v.Fill(0); Vector w = new Vector(m); w.Fill(0); // Golub and vanLoan, p199. backward accumulation of householder matrices // Householder vector k is [zeros(1,k-1) qraux_[k] qrdc_out_[k,:]] for (int k = n-1; k >= 0; --k) { if (k >= m) continue; // Make housevec v, and accumulate norm at the same time. v[k] = qraux_[k]; float sq = (v[k] * v[k]); for (int j = k+1; j < m; ++j) { v[j] = qrdc_out_[k,j]; sq += (v[j] * v[j]); } //if (verbose) MatlabPrint(std::cerr, v, "v"); // Premultiply emerging Q by house(v), noting that v[0..k-1] == 0. // Q_new = (1 - (2/v'*v) v v')Q // or Q -= (2/v'*v) v (v'Q) if (sq > 0.0) { float scale = 2.0f/sq; // w = (2/v'*v) v' Q for (int i = k; i < m; ++i) { w[i] = 0.0f; for (int j = k; j < m; ++j) w[i] += scale * v[j] * Q[j, i]; //w[i] += scale * ComplexTraits.Conjugate(v[j]) * Q[j, i]; } //if (verbose) VNL::MatlabPrint(std::cerr, w, "w"); // Q -= v w for (int i = k; i < m; ++i) for (int j = k; j < m; ++j) Q[i,j] -= (v[i]) * (w[j]); } } } return Q_; }