C# Vector.Fill方法代码示例

        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);

        }

        //*************************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);
        }

        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;
                }
            }
        }

        /// <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>
        /// 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;
//.........这里部分代码省略.........

        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;

//.........这里部分代码省略.........

 /// <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>
        /// 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());

        }

        /// <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_;
        }