C# SceneLibrary.MatrixFixed类代码示例

 public MatrixFixed(MatrixFixed m)
 {
     init(m.Rows, m.Columns);
     for (int i = 0; i < Rows; i++)
         for (int j = 0; j < Columns; j++)
             m_matrix[i, j] = m[i, j];
 }

        /// <summary>
        /// Base-class constructor. This should be called in the constructor of any 
        /// derived class.
        /// </summary>
        /// <param name="position_state_size">The number of dimensions in the position state</param>
        /// <param name="state_size">The total state size</param>
        /// <param name="control_size">The control state size</param>
        /// <param name="m_m_d_t">A string representing the motion model dimensionality type </param>
        /// <param name="m_m_t">A unique string representing this particular motion model type</param>
        public Motion_Model(uint position_state_size, 
			   uint state_size, 
			   uint control_size, 
			   String m_m_d_t, String m_m_t)
        {
            POSITION_STATE_SIZE = position_state_size;
            STATE_SIZE = state_size;
            CONTROL_SIZE = control_size;
            motion_model_dimensionality_type = m_m_d_t;
            motion_model_type = m_m_t;

            fvRES = new Vector(STATE_SIZE);
            xpRES = new Vector(POSITION_STATE_SIZE);
            fv_noisyRES = new Vector(STATE_SIZE);
            xvredefRES = new Vector(STATE_SIZE);
            xvnormRES = new Vector(STATE_SIZE);
            zeroedxvRES = new Vector(STATE_SIZE);
            xpredefRES = new Vector(POSITION_STATE_SIZE);

            dfv_by_dxvRES = new MatrixFixed(STATE_SIZE, STATE_SIZE);
            QxRES = new MatrixFixed(STATE_SIZE, STATE_SIZE);
            dxp_by_dxvRES = new MatrixFixed(POSITION_STATE_SIZE, STATE_SIZE);           
            dxvredef_by_dxvRES = new MatrixFixed(STATE_SIZE, STATE_SIZE);
            dxvredef_by_dxpdefRES = new MatrixFixed(STATE_SIZE, POSITION_STATE_SIZE);
            dxvnorm_by_dxvRES = new MatrixFixed(STATE_SIZE, STATE_SIZE);
            dzeroedxv_by_dxvRES = new MatrixFixed(STATE_SIZE, STATE_SIZE);
            dxpredef_by_dxpRES = new MatrixFixed(POSITION_STATE_SIZE, POSITION_STATE_SIZE);
            dxpredef_by_dxpdefRES = new MatrixFixed(POSITION_STATE_SIZE, POSITION_STATE_SIZE);
        }

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

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

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

        public SearchDatum(MatrixFixed _PuInv, Vector _search_centre)
        {
            PuInv = _PuInv;
            search_centre = _search_centre;
            result_flag = false;
            result_u = 0;
            result_v = 0;

            halfwidth = (uint)(SceneLib.NO_SIGMA /
                        Math.Sqrt(PuInv[0, 0] - PuInv[0, 1] * PuInv[0, 1] / PuInv[1, 1]));
            halfheight = (uint)(SceneLib.NO_SIGMA /
                         Math.Sqrt(PuInv[1, 1] - PuInv[0, 1] * PuInv[0, 1] / PuInv[0, 0]));

            if (halfwidth > 10) halfwidth = 10;
            if (halfheight > 10) halfheight = 10;
        }

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

 // Set the Jacobian \partfracv{h}{y}
 // between the feature measurement state and the feature state. 
 public void set_dh_by_dy(MatrixFixed new_dh_by_dy) { dh_by_dy.Update(new_dh_by_dy); }

 // Set the covariance, \mat{P}_{xy},  between the feature state and
 // the robot state. 
 public void set_Pxy(MatrixFixed new_Pxy) { Pxy.Update(new_Pxy); }

 /// <summary>
 /// Make a measurement of a feature.
 /// </summary>
 /// <param name="id">The identifier for this feature</param>
 /// <param name="z">The measurement of the state, to be filled in by this function</param>
 /// <param name="h">The expected measurement</param>
 /// <param name="S">The measurement covariance.</param>
 /// <returns></returns>
 public virtual bool measure_feature(byte[] id, int patchwidth, ref Vector z, Vector vz, Vector h, MatrixFixed S, Random rnd) { return (false); }