C# Vector.Update方法代码示例

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

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

        }

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