C# SceneLibrary.Vector类代码示例

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

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

        public void successful_internal_measurement(Vector zv_in)
        {
            //assert(zv.Size() == internal_measurement_model.MEASUREMENT_SIZE);
            zv.Update(zv_in);

            //if (DEBUGDUMP) cout << "Internal measurement made      : zv " << endl << zv << endl;

            successful_internal_measurement_flag = true;

            internal_measurement_model.func_nuv(hv, zv);
            nuv.Update(internal_measurement_model.get_nuvRES());
        }

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

        public linefeature(Feature feature1, Feature feature2, int no_of_points, int history)
        {
            marked_for_deletion = false;
            curr_index = 0;
            hits = 0;
            this.feature1 = feature1;
            this.feature2 = feature2;
            this.no_of_points = no_of_points;
            this.history = history;
            pixel_intensity = new Byte[no_of_points, history];
            feature_position = new Vector[history, 2];

            for (int i = 0; i < history; i++)
            {
                feature_position[i, 0] = new Vector(3);
                feature_position[i, 1] = new Vector(3);
            }
        }

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

 public override void func_nui(Vector hi, Vector zi)
 {
     nuiRES.Update(zi - hi);
 }

        public override void func_hi_noisy(Vector yi_true, Vector xp_true)
        {
            func_hi_and_dhi_by_dxp_and_dhi_by_dyi(yi_true, xp_true);

            hi_noisyRES.Put(0, SceneLib.SampleNormal(hiRES[0], ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).SD_IMAGE_SIMULATION, rnd));
            hi_noisyRES.Put(1, SceneLib.SampleNormal(hiRES[1], ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).SD_IMAGE_SIMULATION, rnd));
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="xp">current robot position state</param>
        /// <param name="yi">3D position of the feature</param>
        /// <param name="xp_orig">robot position state when the feature was initially observed</param>
        /// <param name="hi">image coordinates of the feature</param>
        /// <returns></returns>
        public override uint visibility_test(Vector xp, Vector yi, Vector xp_orig, Vector hi)
        {
            float image_search_boundary = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).IMAGE_SEARCH_BOUNDARY;
            uint wdth = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).m_camera.ImageWidth();
            uint hght = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).m_camera.ImageHeight();
            uint cant_see_flag = 0;
  
            // Test image boundaries 
            float x = hi[0];
            float y = hi[1];
            if ((x < 0.0f + image_search_boundary) ||
                (x > (float)(wdth - 1 - image_search_boundary))) 
            {
                cant_see_flag |= Wide_Camera_Point_Feature_Measurement_Model.LEFT_RIGHT_FAIL;
                //if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("Visibility test left / right fail.");
            }
            if ((y < 0.0f + image_search_boundary) ||
                (y > (float)(hght - 1 - image_search_boundary))) 
            {
                cant_see_flag |= Wide_Camera_Point_Feature_Measurement_Model.UP_DOWN_FAIL;
                //if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("Visibility test up / down fail.");
            }

            // Do tests on length and angle of predicted view 

            // hLWi is current predicted vector from head to feature in 
            // world frame

            // This function gives relative position of feature
            func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(yi, xp);

            // Test the feature's not behind the camera (because projection
            // may do strange things)
            if (zeroedyiRES[2] <= 0) 
            {
                cant_see_flag |= Wide_Camera_Point_Feature_Measurement_Model.BEHIND_CAMERA_FAIL;
                //if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("Behind camera fail.");
            }

            ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.func_q(xp);
            RotationMatrix RWR = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.get_qRES().RotationMatrix();

            Vector3D hLWi = new Vector3D(RWR * (new Point3D(zeroedyiRES)));

            // hLWi_orig is vector from head to feature in world frame
            // WHEN THAT FEATURE WAS FIRST MEASURED: i.e. when the image
            // patch was saved
            func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(yi, xp_orig);

            ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.func_q(xp_orig);
            RotationMatrix RWR_orig = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.get_qRES().RotationMatrix();
  
            Vector3D hLWi_orig = new Vector3D(RWR_orig * (new Point3D(zeroedyiRES)));

            // Compare hLWi and hLWi_orig for length and orientation
            float mod_hLWi = hLWi.Norm();
            float mod_hLWi_orig = hLWi_orig.Norm();

            float length_ratio = mod_hLWi / mod_hLWi_orig;
            float max_length_ratio = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).MAXIMUM_LENGTH_RATIO;
            if ((length_ratio > max_length_ratio) ||
                 (length_ratio < (1.0f / max_length_ratio))) 
            {
                cant_see_flag |= Wide_Camera_Point_Feature_Measurement_Model.DISTANCE_FAIL;
                //if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("Distance fail.");
            }

            float dot_prod = hLWi * hLWi_orig;

            float angle = (float)Math.Acos(dot_prod / (mod_hLWi * mod_hLWi_orig));
            if (angle == float.NaN) Debug.WriteLine("Maths error: " + dot_prod + " / " + (mod_hLWi * mod_hLWi_orig));
            angle = (angle >= 0.0f ? angle : -angle);  // Make angle positive
            if (angle > ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).MAXIMUM_ANGLE_DIFFERENCE) 
            {
                cant_see_flag |= Wide_Camera_Point_Feature_Measurement_Model.ANGLE_FAIL;
                //if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("Angle fail.");
            }

            return cant_see_flag;   // 0 if OK, otherwise error code
        }

        public override void func_hi_and_dhi_by_dxp_and_dhi_by_dyi(Vector yi, Vector xp)
        {
            // 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);

            // Project this from 3D into the 2D image using our camera
            Vector feature_3D_position = get_zeroedyiRES();
            WideCamera cam = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).m_camera;
            hiRES = cam.Project(feature_3D_position);

            // And ask the camera what the Jacobian of this projection was
            MatrixFixed dhid_by_dzeroedyi = cam.ProjectionJacobian();

            // Form the required Jacobians
            dhi_by_dxpRES = dhid_by_dzeroedyi * dzeroedyi_by_dxpRES;
            dhi_by_dyiRES = dhid_by_dzeroedyi * dzeroedyi_by_dyiRES;
        }