C# Matrix.SetIdentity方法代码示例

 public SyntheticData()
 {
     state = new Matrix<float>(4, 1);
     state[0, 0] = Cursor.Position.X; // x-pos
     state[1, 0] = Cursor.Position.Y; // y-pos
     state[2, 0] = 0f; // x-velocity
     state[3, 0] = 0f; // y-velocity
     transitionMatrix = new Matrix<float>(new float[,]
         {
             {1, 0, 1, 0},
             {0, 1, 0, 1},
             {0, 0, 1, 0},
             {0, 0, 0, 1}
         });
     measurementMatrix = new Matrix<float>(new float[,]
         {
             { 1, 0, 0, 0 },
             { 0, 1, 0, 0 }
         });
     measurementMatrix.SetIdentity();
     processNoise = new Matrix<float>(4, 4);
     processNoise.SetIdentity(new MCvScalar(1.0e-4));
     measurementNoise = new Matrix<float>(2, 2);
     measurementNoise.SetIdentity(new MCvScalar(1.5e-1));
     errorCovariancePost = new Matrix<float>(4, 4);
     errorCovariancePost.SetIdentity();
 }

 public SyntheticData()
 {
     state = new Matrix<float>(4, 1);
     state[0, 0] = 0f; // x-pos
     state[1, 0] = 0f; // y-pos
     state[2, 0] = 0f; // x-velocity
     state[3, 0] = 0f; // y-velocity
     transitionMatrix = new Matrix<float>(new float[,]
             {
                 {1, 0, 1, 0},  // x-pos, y-pos, x-velocity, y-velocity
                 {0, 1, 0, 1},
                 {0, 0, 1, 0},
                 {0, 0, 0, 1}
             });
     measurementMatrix = new Matrix<float>(new float[,]
             {
                 { 1, 0, 0, 0 },
                 { 0, 1, 0, 0 }
             });
     measurementMatrix.SetIdentity();
     processNoise = new Matrix<float>(4, 4); //Linked to the size of the transition matrix
     processNoise.SetIdentity(new MCvScalar(1.0e-4)); //The smaller the value the more resistance to noise
     measurementNoise = new Matrix<float>(2, 2); //Fixed accordiong to input data
     measurementNoise.SetIdentity(new MCvScalar(1.0e-1));
     errorCovariancePost = new Matrix<float>(4, 4); //Linked to the size of the transition matrix
     errorCovariancePost.SetIdentity();
 }

        public static Matrix<double> Get2DTranslationMatrix(double x, double y)
        {
            Matrix<double> m = new Matrix<double>(3, 3);
            m.SetIdentity();
            m[0, 3] = x;
            m[1, 3] = y;

            return m;
        }

      public void TestInvert()
      {
         Matrix<Single> m = new Matrix<Single>(3, 3);
         Matrix<Single> mInvert = new Matrix<Single>(3, 3);

         m.SetIdentity();

         CvInvoke.Invert(m, mInvert, Emgu.CV.CvEnum.DecompMethod.LU);

         EmguAssert.IsTrue(m.Equals(mInvert));
      }

      public void TestSolve()
      {
         Matrix<Single> lhs = new Matrix<Single>(3, 3);
         lhs.SetIdentity();
         Matrix<Single> rhs = new Matrix<Single>(new float[,] { { 0.1f }, { 0.2f }, { 0.5f } });
         Matrix<Single> result = new Matrix<float>(3, 1);
         CvInvoke.Solve(lhs, rhs, result, CvEnum.DecompMethod.LU);

         EmguAssert.AreEqual(rhs[0, 0], result[0, 0]);
         EmguAssert.AreEqual(rhs[1, 0], result[1, 0]);
         EmguAssert.AreEqual(rhs[2, 0], result[2, 0]);
      }

        public static Matrix<double> ConvertToHomogenous(Matrix<double> matrix)
        {
            if (matrix.Rows < 2 || matrix.Cols < 2 || (matrix.Rows != matrix.Cols))
            {
                throw new ArgumentException("symmetric matrix > 1x1 expected");
            }

            Matrix<double> m = new Matrix<double>(matrix.Rows + 1, matrix.Cols + 1);
            m.SetIdentity();
            for (int row = 0; row < matrix.Rows; row++)
            {
                for (int col = 0; col < matrix.Cols; col++)
                {
                    m[row, col] = matrix[row, col];
                }
            }
            return m;
        }

        private bool isInitialized; // true if any data has been fed

        public KalmanFilter(int variables)
        {
            variablesCount = variables;

            int measurementVariables = variables;
            int dynamicVariables = variables * 2;

            float[] state = new float[dynamicVariables];
            for (int i = 0; i < dynamicVariables; ++i)
                state[i] = 0.0f;

            Matrix<float> transitionMatrix = new Matrix<float>(dynamicVariables, dynamicVariables);
            transitionMatrix.SetZero();
            for (int i = 0; i < dynamicVariables; ++i)
            {
                transitionMatrix[i, i] = 1.0f;
                if (i >= measurementVariables)
                    transitionMatrix[i - measurementVariables, i] = 1;
            }

            Matrix<float> measurementMatrix = new Matrix<float>(measurementVariables, dynamicVariables);
            measurementMatrix.SetZero();
            for (int i = 0; i < measurementVariables; ++i)
                measurementMatrix[i, i] = 1.0f;

            Matrix<float> processNoise = new Matrix<float>(dynamicVariables, dynamicVariables);
            processNoise.SetIdentity(new MCvScalar(1));//1.0e-4));

            Matrix<float> measurementNoise = new Matrix<float>(measurementVariables, measurementVariables);
            measurementNoise.SetIdentity(new MCvScalar(4));//1.0e-1));

            Matrix<float> errorCovariancePost = new Matrix<float>(dynamicVariables, dynamicVariables);
            errorCovariancePost.SetIdentity();

            kalman = new Kalman(dynamicVariables, measurementVariables, 0);
            kalman.CorrectedState = new Matrix<float>(state);
            kalman.TransitionMatrix = transitionMatrix;
            kalman.MeasurementNoiseCovariance = measurementNoise;
            kalman.ProcessNoiseCovariance = processNoise;
            kalman.ErrorCovariancePost = errorCovariancePost;
            kalman.MeasurementMatrix = measurementMatrix;
        }

        public SyntheticData(float strengthMatrix, double processNoise, double measurementNoise)
        {
            var newStrength = strengthMatrix;
            var newProcessNoise = processNoise;
            var newMeasurementNoise = measurementNoise;

            if (strengthMatrix > 1.0f || strengthMatrix < 0.0f)
                newStrength = 0.6f;

            if (processNoise > 1.0e-1 || processNoise < 1.0e-4)
                newProcessNoise = 1.0e-2;

            if (measurementNoise > 1.0e-1 || measurementNoise < 1.0e-4)
                newMeasurementNoise = 1.0e-1;

            State = new Matrix<float>(4, 1);
            State[0, 0] = 0f;                   // x-pos
            State[1, 0] = 0f;                   // y-pos
            State[2, 0] = 0f;                   // x-velocity
            State[3, 0] = 0f;                   // y-velocity
            TransitionMatrix = new Matrix<float>(new[,]
                    {
                        {newStrength, 0, 1, 0},
                        {0, newStrength, 0, 1},
                        {0, 0, 1, 0},
                        {0, 0, 0, 1}
                    });
            MeasurementMatrix = new Matrix<float>(new float[,]
                    {
                        { 1, 0, 0, 0 },
                        { 0, 1, 0, 0 }
                    });
            MeasurementMatrix.SetIdentity();
            ProcessNoise = new Matrix<float>(4, 4);                             //Linked to the size of the transition matrix
            ProcessNoise.SetIdentity(new MCvScalar(newProcessNoise));           //The smaller the value the more resistance to noise
            MeasurementNoise = new Matrix<float>(2, 2);                         //Fixed accordiong to input data
            MeasurementNoise.SetIdentity(new MCvScalar(newMeasurementNoise));   //larger the value more resitance to noise and the less responsive to velocity
            ErrorCovariancePost = new Matrix<float>(4, 4);                      //Linked to the size of the transition matrix
            ErrorCovariancePost.SetIdentity();
        }

        /// <summary>
        /// Allocates CvKalman and all its matrices and initializes them somehow. 
        /// </summary>
        /// <param name="dynamParams">dimensionality of the state vector</param>
        /// <param name="measureParams">dimensionality of the measurement vector </param>
        /// <param name="controlParams">dimensionality of the control vector </param>
        public Kalman(int dynamParams, int measureParams, int controlParams)
        {
            _kalman.DP = dynamParams;
             _kalman.MP = measureParams;
             _kalman.CP = controlParams;

             PredictedState = new Matrix<float>(dynamParams, 1);

             CorrectedState = new Matrix<float>(dynamParams, 1);

             TransitionMatrix = new Matrix<float>(dynamParams, dynamParams);
             TransitionMatrix.SetIdentity();

             ProcessNoiseCovariance = new Matrix<float>(dynamParams, dynamParams);
             ProcessNoiseCovariance.SetIdentity();

             MeasurementMatrix = new Matrix<float>(measureParams, dynamParams);

             MeasurementNoiseCovariance = new Matrix<float>(measureParams, measureParams);
             MeasurementNoiseCovariance.SetIdentity();

             ErrorCovariancePre = new Matrix<float>(dynamParams, dynamParams);

             ErrorCovariancePost = new Matrix<float>(dynamParams, dynamParams);

             Gain = new Matrix<float>(dynamParams, measureParams);

             if (controlParams > 0)
             {
            ControlMatrix = new Matrix<float>(dynamParams, controlParams);
             }

             _temp1 = new Matrix<float>(dynamParams, dynamParams);
             _kalman.temp1 = _temp1.Ptr;
             _temp2 = new Matrix<float>(measureParams, dynamParams);
             _kalman.temp2 = _temp2.Ptr;
             _temp3 = new Matrix<float>(measureParams, measureParams);
             _kalman.temp3 = _temp3.Ptr;
             _temp4 = new Matrix<float>(measureParams, dynamParams);
             _kalman.temp4 = _temp4.Ptr;
             _temp5 = new Matrix<float>(measureParams, 1);
             _kalman.temp5 = _temp5.Ptr;

             //_kalman.Temp1 = _temp1.MCvMat.data;
             //_kalman.Temp2 = _temp2.MCvMat.data;
        }

      public void TestCudaWarpPerspective()
      {
         if (!CudaInvoke.HasCuda)
            return;
         Matrix<float> transformation = new Matrix<float>(3, 3);
         transformation.SetIdentity();

         Image<Gray, byte> image = new Image<Gray, byte>(480, 320);
         image.SetRandNormal(new MCvScalar(), new MCvScalar(255));

         using (GpuMat cudaImage = new GpuMat(image))
         using (CudaImage<Gray, Byte> resultCudaImage = new CudaImage<Gray, byte>())
         {
            CudaInvoke.WarpPerspective(cudaImage, resultCudaImage, transformation, cudaImage.Size, CvEnum.Inter.Cubic, CvEnum.BorderType.Default, new MCvScalar(), null);
         }
      }

        // this is directly from the wikipedia page on Kabsh Algorithm
        public void Recompute2()
        {
            var p = P;
            var q = Q;

            //1. subtract centroids
            for (int i = 0; i < p.Rows; i++) {
                p[i, 0] -= SourceCentroid[0, 0];
                p[i, 1] -= SourceCentroid[1, 0];
                q[i, 0] -= DestCentroid[0, 0];
                q[i, 1] -= DestCentroid[1, 0];
            }

            //2. compute covariance matrix
            var a = p.Transpose()*q;

            //3. compute rotation matrix
            /* perform svd  where A =  V S WT */
            Matrix<double> V = new Matrix<double>(2, 2);
            Matrix<double> S = new Matrix<double>(2, 2);
            Matrix<double> W = new Matrix<double>(2, 2);
            CvInvoke.cvSVD(a.Ptr, S.Ptr, V.Ptr, W.Ptr, SVD_TYPE.CV_SVD_DEFAULT);

            // Deal with reflection matrix
            Matrix<double> m = new Matrix<double>(2, 2);
            m.SetIdentity(new MCvScalar(1));
            m[1,1] = ((W*V.Transpose()).Det<0) ? -1 : 1;

            // Comput the rotation matrix
            Rotation = W*m*V.Transpose();
            //Offset = DestCentroid - (Rotation * SourceCentroid);
            Offset = DestCentroid - SourceCentroid;

            Console.WriteLine("Rotaiton Matrix - Angle ="+Angle);
            Console.WriteLine(FormatMatrix(Rotation));
        }

        public static Matrix<double> Get3DTranslationMatrix(double x, double y, double z)
        {
            Matrix<double> m = new Matrix<double>(4, 4);
            m.SetIdentity();
            m[0, 3] = x;
            m[1, 3] = y;
            m[2, 3] = z;

            return m;
        }

        public void TestStereoSGBMCorrespondence()
        {
            Image<Gray, Byte> left = new Image<Gray, byte>("left.jpg");
             Image<Gray, Byte> right = new Image<Gray, byte>("right.jpg");
             Size size = left.Size;

             Image<Gray, Int16> disparity = new Image<Gray, Int16>(size);

             StereoSGBM bm = new StereoSGBM(10, 64, 0, 0, 0, 0, 0, 0, 0, 0, false);
             Stopwatch watch = Stopwatch.StartNew();
             bm.FindStereoCorrespondence(left, right, disparity);
             watch.Stop();

             Trace.WriteLine(String.Format("Time used: {0} milliseconds", watch.ElapsedMilliseconds));

             Matrix<double> q = new Matrix<double>(4, 4);
             q.SetIdentity();
             MCvPoint3D32f[] points = PointCollection.ReprojectImageTo3D(disparity * (-16), q);

             float min = (float)1.0e10, max = 0;
             foreach (MCvPoint3D32f p in points)
             {
            if (p.z < min) min = p.z;
            else if (p.z > max) max = p.z;
             }
             Trace.WriteLine(String.Format("Min : {0}\r\nMax : {1}", min, max));
        }

         public SyntheticData()
         {
            _state = new Matrix<float>(2, 1);
            // start with random position and velocity
            //_state.SetRandNormal(new MCvScalar(0.0), new MCvScalar(1.0));
            _state[0, 0] = 0.0f;
            _state[1, 0] = 0.05f;

            _measurementNoise = new Matrix<float>(1, 1);
            _measurementNoise.SetIdentity(new MCvScalar(1.0e-2));
            _processNoise = new Matrix<float>(2, 2);
            _processNoise.SetIdentity(new MCvScalar(1.0e-5));
            _errorCovariancePost = new Matrix<float>(2, 2);
            _errorCovariancePost.SetIdentity();
            _transitionMatrix = new Matrix<float>(new float[,] { { 1, 1 }, { 0, 1 } }); // phi_t = phi_{t-1} + delta_phi
            _measurementMatrix = new Matrix<float>(new float[,] { {1, 0}});
            _measurementMatrix.SetIdentity(); //the measurement is [ phi ]
         }

      public void TestStereoSGBMCorrespondence()
      {
         Image<Gray, Byte> left = EmguAssert.LoadImage<Gray, byte>("aloeL.jpg");
         Image<Gray, Byte> right = EmguAssert.LoadImage<Gray, byte>("aloeR.jpg");
         Size size = left.Size;

         Image<Gray, Int16> disparity = new Image<Gray, Int16>(size);

         StereoSGBM bm = new StereoSGBM(10, 64, 0, 0, 0, 0, 0, 0, 0, 0, StereoSGBM.Mode.SGBM);
         Stopwatch watch = Stopwatch.StartNew();
         bm.Compute(left, right, disparity);
         watch.Stop();

         EmguAssert.WriteLine(String.Format("Time used: {0} milliseconds", watch.ElapsedMilliseconds));

         Matrix<double> q = new Matrix<double>(4, 4);
         q.SetIdentity();
         Image<Gray, Int16> disparityScaled = disparity * (-16);
         MCvPoint3D32f[] points = PointCollection.ReprojectImageTo3D(disparityScaled.Mat, q);

         float min = (float) 1.0e10, max = 0;
         foreach (MCvPoint3D32f p in points)
         {
            if (p.Z < min)
               min = p.Z;
            else if (p.Z > max)
               max = p.Z;
         }
         EmguAssert.WriteLine(String.Format("Min : {0}\r\nMax : {1}", min, max));

      }