/// <summary>
        /// Create an object recognizer using the specific tranning data and parameters
        /// </summary>
        /// <param name="images">The images used for training, each of them should be the same size. It's recommended the images are histogram normalized</param>
        /// <param name="labels">The labels corresponding to the images</param>
        /// <param name="eigenDistanceThreshold">
        /// The eigen distance threshold, (0, ~1000].
        /// The smaller the number, the more likely an examined image will be treated as unrecognized object. 
        /// If the threshold is &lt; 0, the recognizer will always treated the examined image as one of the known object. 
        /// </param>
        /// <param name="termCrit">The criteria for recognizer training</param>
        public EigenObjectRecognizer(Image<Gray, Byte>[] images, Guid[] labels, int cacheSize, double eigenDistanceThreshold, ref MCvTermCriteria termCrit)
        {
            Debug.Assert(images.Length == labels.Length, "The number of images should equals the number of labels");
               Debug.Assert(eigenDistanceThreshold >= 0.0, "Eigen-distance threshold should always >= 0.0");

               CalcEigenObjects(images, ref termCrit, out _eigenImages, out _avgImage);

               /*
               _avgImage.SerializationCompressionRatio = 9;

               foreach (Image<Gray, Single> img in _eigenImages)
               //Set the compression ration to best compression. The serialized object can therefore save spaces
               img.SerializationCompressionRatio = 9;
               */

               _eigenValues = Array.ConvertAll<Image<Gray, Byte>, Matrix<float>>(images,
               delegate(Image<Gray, Byte> img)
               {
                  return new Matrix<float>(ConstructEigenDecomposite(img, _eigenImages, _avgImage));
               });

               _labels = labels;
               _eigenDistanceThreshold = eigenDistanceThreshold;
            queueMaxCount = cacheSize;
        }

      /*
      /// <summary>
      /// Create a LevMarqSparse solver
      /// </summary>
      public LevMarqSparse()
      {
         _ptr = CvInvoke.CvCreateLevMarqSparse();
      }*/

      /// <summary>
      /// Useful function to do simple bundle adjustment tasks
      /// </summary>
      /// <param name="points">Positions of points in global coordinate system (input and output), values will be modified by bundle adjustment</param>
      /// <param name="imagePoints">Projections of 3d points for every camera</param>
      /// <param name="visibility">Visibility of 3d points for every camera</param>
      /// <param name="cameraMatrix">Intrinsic matrices of all cameras (input and output), values will be modified by bundle adjustment</param>
      /// <param name="R">rotation matrices of all cameras (input and output), values will be modified by bundle adjustment</param>
      /// <param name="T">translation vector of all cameras (input and output), values will be modified by bundle adjustment</param>
      /// <param name="distCoeffcients">distortion coefficients of all cameras (input and output), values will be modified by bundle adjustment</param>
      /// <param name="termCrit">Termination criteria, a reasonable value will be (30, 1.0e-12) </param>
      public static void BundleAdjust(
         MCvPoint3D64f[] points, MCvPoint2D64f[][] imagePoints, int[][] visibility,
         Matrix<double>[] cameraMatrix, Matrix<double>[] R, Matrix<double>[] T, Matrix<double>[] distCoeffcients, MCvTermCriteria termCrit)
      {
         using (Matrix<double> imagePointsMat = CvToolbox.GetMatrixFromPoints(imagePoints))
         using (Matrix<int> visibilityMat = CvToolbox.GetMatrixFromArrays(visibility))
         using (VectorOfMat cameraMatVec = new VectorOfMat())
         using (VectorOfMat rMatVec = new VectorOfMat())
         using (VectorOfMat tMatVec = new VectorOfMat())
         using (VectorOfMat distorMatVec = new VectorOfMat())
         {
            cameraMatVec.Push(cameraMatrix);
            rMatVec.Push(R);
            tMatVec.Push(T);
            distorMatVec.Push(distCoeffcients);


            GCHandle handlePoints = GCHandle.Alloc(points, GCHandleType.Pinned);

            CvInvoke.CvLevMarqSparseAdjustBundle(
               cameraMatrix.Length,
               points.Length, handlePoints.AddrOfPinnedObject(),
               imagePointsMat, visibilityMat, cameraMatVec, rMatVec, tMatVec, distorMatVec, ref termCrit);

            handlePoints.Free();

         }
      }

       public static List<List<Point>> Lines(List<Point> input, int lanes = 2)
        {
            float[,] samples = new float[input.Count, 2];
            int i = 0;
            foreach (var p in input)
            {
                samples[i, 0] = p.X;
                samples[i, 1] = p.Y;
                ++i;
            }

           MCvTermCriteria term = new MCvTermCriteria();

           Matrix<float> samplesMatrix = new Matrix<float>(samples);
           Matrix<Int32> labels = new Matrix<Int32>(input.Count, 1);
           CvInvoke.cvKMeans2(samplesMatrix, 2, labels, term, lanes, IntPtr.Zero, Emgu.CV.CvEnum.KMeansInitType.RandomCenters, IntPtr.Zero, IntPtr.Zero);

           List<Point> leftLane = new List<Point>(input.Count);
           List<Point> rightLane = new List<Point>(input.Count);
           for (i = 0; i < input.Count; ++i)
           {
               if (labels[i, 0] == 0)
                   leftLane.Add(input[i]);
               else
                   rightLane.Add(input[2]);
           }

           return new List<List<Point>> { leftLane, rightLane };
        }

 public Calibration(Camera mainCamera)
 {
     _mainCamera = mainCamera;
     _termCriteria = new MCvTermCriteria();
     _flags = CALIB_TYPE.CV_CALIB_USE_INTRINSIC_GUESS | CALIB_TYPE.CV_CALIB_FIX_K1 | CALIB_TYPE.CV_CALIB_FIX_K2 | CALIB_TYPE.CV_CALIB_FIX_K3 | CALIB_TYPE.CV_CALIB_FIX_K4 | CALIB_TYPE.CV_CALIB_FIX_K5 | CALIB_TYPE.CV_CALIB_ZERO_TANGENT_DIST;
     _size = new Size(Screen.width, Screen.height);
 }

 //private IntPtr[] _covsPtr;
 //private GCHandle _covsPtrHandle;
 /// <summary>
 /// Create EM parameters with default value
 /// </summary>
 public EMParams()
 {
     Nclusters = 10;
      CovMatType = Emgu.CV.ML.MlEnum.EM_COVARIAN_MATRIX_TYPE.COV_MAT_DIAGONAL;
      StartStep = Emgu.CV.ML.MlEnum.EM_INIT_STEP_TYPE.START_AUTO_STEP;
      _termCrit = new MCvTermCriteria(100, 1.0e-6);
 }

        /// <summary>
        /// Running k-means algorithm with k-means++ initial algorithm for k = 2.
        /// Can be used to find right and left lane boundary.
        /// </summary>
        /// <param name="input">Input points</param>
        /// <param name="a">First lane</param>
        /// <param name="b">Second lane</param>
        public static void Two_Means_Clustering(List<Point> input, ref List<Point> first, ref List<Point> second, int attempts = 3)
        {
            if (input.Count < 7)
                return;

            // formatting input data
            float[,] samples = new float[input.Count, 2];
            int i = 0;
            foreach (var p in input)
            {
                samples[i, 0] = p.X;
                samples[i, 1] = p.Y;
                ++i;
            }

            MCvTermCriteria term = new MCvTermCriteria();

            Matrix<float> samplesMatrix = new Matrix<float>(samples);
            Matrix<Int32> labels = new Matrix<Int32>(input.Count, 1);

            CvInvoke.cvKMeans2(samplesMatrix, 2, labels, term, attempts, IntPtr.Zero, KMeansInitType.RandomCenters, IntPtr.Zero, IntPtr.Zero);

            first.Clear();
            second.Clear();
            for (i = 0; i < input.Count; ++i)
            {
                if (labels[i, 0] == 0)
                    first.Add(input[i]);
                else
                    second.Add(input[i]);
            }
        }

 /// <summary>
 /// Iterates to find the object center given its back projection and initial position of search window. The iterations are made until the search window center moves by less than the given value and/or until the function has done the maximum number of iterations. 
 /// </summary>
 /// <param name="probImage">Back projection of object histogram</param>
 /// <param name="window">Initial search window</param>
 /// <param name="criteria">Criteria applied to determine when the window search should be finished. </param>
 /// <returns>The number of iterations made</returns>
 public static int MeanShift(
    IInputArray probImage,
    ref Rectangle window,
    MCvTermCriteria criteria)
 {
    using (InputArray iaProbImage = probImage.GetInputArray())
       return cveMeanShift(iaProbImage, ref window, ref criteria);
 }

 public static extern void cvCalcOpticalFlowHS(
         IntPtr prev,
         IntPtr curr,
         int usePrevious,
         IntPtr velx,
         IntPtr vely,
         double lambda,
         MCvTermCriteria criteria);

        //-------------------------------------------------------------------------------------//
        //<<<<<----------------FUNCTIONS USED TO TRAIN RECOGNIZER ON TRAINING SET----------->>>>
        //-------------------------------------------------------------------------------------//
        /// <summary>
        /// Trains recognizer on fetched face-label pairs and saves the trained data to recognition variables
        /// </summary>
        public void TrainRecognizer()
        {
            MCvTermCriteria termCrit = new MCvTermCriteria(iMaxItereations, dEPS);
            ImageInDatabase dbTrainingSet = new ImageInDatabase();

            // This will fill the training images array AND labels array
            dbTrainingSet.LoadCompleteTrainingSet();
            recognizer = new EigenObjectRecognizer(dbTrainingSet.m_trainingImages, dbTrainingSet.m_TrainingLabels, dDistTreshHold, ref termCrit);
        }

        //public static KDetector kdetector;
        public string Apply(int k, string fn, string outImagePath)
        {
            Image<Bgr, float> src = new Image<Bgr, float>(fn);
            Matrix<float> samples = new Matrix<float>(src.Rows * src.Cols, 1, 3);
            Matrix<int> finalClusters = new Matrix<int>(src.Rows * src.Cols, 1);

            //Convert image to a sample matrix that its rows equal to width*height of image and its
            //column equals to 3 feature (R/G/B) or (H/L/S)
            for (int y = 0; y < src.Rows; y++)
            {
                for (int x = 0; x < src.Cols; x++)
                {
                    samples.Data[y + x * src.Rows, 0] = (float)src[y, x].Blue;
                    samples.Data[y + x * src.Rows, 1] = (float)src[y, x].Green;
                    samples.Data[y + x * src.Rows, 2] = (float)src[y, x].Red;
                }
            }

            MCvTermCriteria term = new MCvTermCriteria(10000, 0.0001);
            term.type = TERMCRIT.CV_TERMCRIT_ITER | TERMCRIT.CV_TERMCRIT_EPS;

            int clusterCount = k;
            int attempts = 10;

            //center matrix after call k-means function holds the cluster value
            Matrix<float> centers;
            centers = new Matrix<float>(clusterCount, 3);

            int mm = CvInvoke.cvKMeans2(samples, clusterCount, finalClusters, term, attempts, IntPtr.Zero, KMeansInitType.PPCenters, centers, IntPtr.Zero);

            Image<Bgr, float> new_image = new Image<Bgr, float>(src.Size);

            //find color of cluster values
            Bgr[] clusterColors = new Bgr[clusterCount];
            for (int i = 0; i < clusterCount; i++)
            {
                Bgr b = new Bgr(centers[i, 0], centers[i, 1], centers[i, 2]);

                clusterColors[i] = b;
            }

            //Draw a image based on cluster color
            for (int y = 0; y < src.Rows; y++)
            {
                for (int x = 0; x < src.Cols; x++)
                {
                    PointF p = new PointF(x, y);
                    new_image.Draw(new CircleF(p, 1.0f), clusterColors[finalClusters[y + x * src.Rows, 0]], 1);
                }
            }

            new_image.Save(outImagePath);

            return outImagePath;
        }

 public static extern void cvSnakeImage(
    IntPtr image,
    IntPtr points,
    int length,
    [MarshalAs(UnmanagedType.LPArray)] float[] alpha,
    [MarshalAs(UnmanagedType.LPArray)] float[] beta,
    [MarshalAs(UnmanagedType.LPArray)] float[] gamma,
    int coeffUsage,
    Size win,
    MCvTermCriteria criteria,
    int calcGradient);

    private void InitEigenObjectRecognizer() {
      if (trainedImages.Count <= 0) { return; }
      initEigen = true;

      // TermCriteria for face recognition with numbers of trained images like maxIteration
      termCrit = new MCvTermCriteria(trainedImages.Count, 0.001);

      // Eigen face recognizer
      recognizer = new EigenObjectRecognizer(trainedImages.ToArray(), trainedLabels.ToArray(), 5000, ref termCrit);

      initEigen = false;
    }

 /// <summary>
 /// Implements CAMSHIFT object tracking algorithm ([Bradski98]). First, it finds an object center using cvMeanShift and, after that, calculates the object size and orientation. 
 /// </summary>
 /// <param name="probImage">Back projection of object histogram </param>
 /// <param name="window">Initial search window</param>
 /// <param name="criteria">Criteria applied to determine when the window search should be finished</param>
 /// <returns>Circumscribed box for the object, contains object size and orientation</returns>
 public static RotatedRect CamShift(
    IInputArray probImage,
    ref Rectangle window,
    MCvTermCriteria criteria)
 {
    RotatedRect box = new RotatedRect();
    using (InputArray iaProbImage = probImage.GetInputArray())
    {
       cveCamShift(iaProbImage, ref window, ref criteria, ref box);
    }
    return box;
 }

 /// <summary>
 /// Calculates optical flow for a sparse feature set using iterative Lucas-Kanade method in pyramids
 /// </summary>
 /// <param name="prev">First frame, at time t</param>
 /// <param name="curr">Second frame, at time t + dt </param>
 /// <param name="prevFeatures">Array of points for which the flow needs to be found</param>
 /// <param name="winSize">Size of the search window of each pyramid level</param>
 /// <param name="level">Maximal pyramid level number. If 0 , pyramids are not used (single level), if 1 , two levels are used, etc</param>
 /// <param name="criteria">Specifies when the iteration process of finding the flow for each point on each pyramid level should be stopped</param>
 /// <param name="currFeatures">Array of 2D points containing calculated new positions of input features in the second image</param>
 /// <param name="status">Array. Every element of the array is set to 1 if the flow for the corresponding feature has been found, 0 otherwise</param>
 /// <param name="trackError">Array of double numbers containing difference between patches around the original and moved points</param>
 public static void PyrLK(
    Image<Gray, Byte> prev,
    Image<Gray, Byte> curr,
    PointF[] prevFeatures,
    Size winSize,
    int level,
    MCvTermCriteria criteria,
    out PointF[] currFeatures,
    out Byte[] status,
    out float[] trackError)
 {
    PyrLK(prev, curr, null, null, prevFeatures, winSize, level, criteria, Emgu.CV.CvEnum.LKFLOW_TYPE.DEFAULT, out currFeatures, out status, out trackError);
 }

        public EigenCardDetector(string foldername)
        {
            List<FileInfo> files = new List<FileInfo>(new DirectoryInfo(foldername).GetFiles());
            List<Image<Gray, byte>> images = new List<Image<Gray, byte>>(files.Count);
            foreach (FileInfo info in files)
            {
                Bitmap bit = new Bitmap(info.FullName);
                images.Add(new Image<Gray, byte>(bit));
            }

            MCvTermCriteria crit = new MCvTermCriteria(0.05);
            recog = new EigenObjectRecognizer(images.ToArray(), ref crit);
        }

        public EigenObjectRecognizer(Image<Gray, Byte>[] images, String[] labels, double eigenDistanceThreshold, ref MCvTermCriteria termCrit)
        {
            Debug.Assert(images.Length == labels.Length, "The number of images should equals the number of labels");
             Debug.Assert(eigenDistanceThreshold >= 0.0, "Eigen-distance threshold should always >= 0.0");

             CalcEigenObjects(images, ref termCrit, out _eigenImages, out _avgImage);

             _eigenValues = Array.ConvertAll<Image<Gray, Byte>, Matrix<float>>(images,
             delegate(Image<Gray, Byte> img)
             {
                return new Matrix<float>(EigenDecomposite(img, _eigenImages, _avgImage));
             });

             _labels = labels;

             _eigenDistanceThreshold = eigenDistanceThreshold;
        }

 /// <summary>
 /// Calculates orthonormal eigen basis and the averaged object for a group of the input objects.
 /// </summary>
 /// <param name="input">Pointer to the array of IplImage input objects </param>
 /// <param name="calcLimit">Criteria that determine when to stop calculation of eigen objects. Depending on the parameter calcLimit, calculations are finished either after first calcLimit.max_iter dominating eigen objects are retrieved or if the ratio of the current eigenvalue to the largest eigenvalue comes down to calcLimit.epsilon threshold. The value calcLimit -> type must be CV_TERMCRIT_NUMB, CV_TERMCRIT_EPS, or CV_TERMCRIT_NUMB | CV_TERMCRIT_EPS . The function returns the real values calcLimit->max_iter and calcLimit->epsilon</param>
 /// <param name="avg">Averaged object</param>
 /// <param name="eigVals">Pointer to the eigenvalues array in the descending order; may be NULL</param>
 /// <param name="eigVecs">Pointer either to the array of eigen objects</param>
 /// <returns>Pointer either to the array of eigen objects or to the write callback function</returns>
 public static void cvCalcEigenObjects(
  IntPtr[] input,
  ref MCvTermCriteria calcLimit,
  IntPtr[] eigVecs,
  float[] eigVals,
  IntPtr avg)
 {
     cvCalcEigenObjects(
      input.Length,
      input,
      eigVecs,
      CvEnum.EIGOBJ_TYPE.CV_EIGOBJ_NO_CALLBACK,
      0,
      IntPtr.Zero,
      ref calcLimit,
      avg,
      eigVals);
 }

        public FacialInfo()
        {
            StreamReader SR = new StreamReader("CVConfig.txt");
            gFacedetection = new HaarCascade(@"haarcascade_frontalface_alt.xml");
            gHanddetection = new HaarCascade(@"haarcascade_hand.xml");

            gCompareBoxes = new List<Image<Gray, byte>>();
            gRecognitionBoxes = new List<Image<Gray, byte>>();


            Image<Gray, byte> Blank = new Image<Gray, byte>(128, 120, new Gray(0.5));
            for (int x = 0; x < 6; x++)
            {
                gCompareBoxes.Add(Blank);
                gRecognitionBoxes.Add(Blank);
            }

            try
            {
                ImageWidth = int.Parse(SR.ReadLine().Split(':')[1]);
                ImageHeight = int.Parse(SR.ReadLine().Split(':')[1]);
                Threshold = int.Parse(SR.ReadLine().Split(':')[1]);
                termcrit = double.Parse(SR.ReadLine().Split(':')[1]);
            }
            catch 
            {
                //default values
                ImageWidth = 128;
                ImageHeight = 120;
                termcrit = 0.001;
                Threshold = 4500;
            }
            SR.Close();

            gUP = new UserProfile();

            MCvTermCriteria termCrit = new MCvTermCriteria(gUP.LoadedUsers.Count, termcrit);
            gEOR = new EigenObjectRecognizer(
               gUP.getImages(),
               gUP.getNumbers(),
                10000,//4500
               ref termCrit);
        }

      /// <summary>
      /// Calculates optical flow for a sparse feature set using iterative Lucas-Kanade method in pyramids
      /// </summary>
      /// <param name="prev">First frame, at time t</param>
      /// <param name="curr">Second frame, at time t + dt </param>
      /// <param name="prevPyrBuffer">Buffer for the pyramid for the first frame. If it is not NULL, the buffer must have a sufficient size to store the pyramid from level 1 to level #level ; the total size of (image_width+8)*image_height/3 bytes is sufficient</param>
      /// <param name="currPyrBuffer">Similar to prev_pyr, used for the second frame</param>
      /// <param name="prevFeatures">Array of points for which the flow needs to be found</param>
      /// <param name="winSize">Size of the search window of each pyramid level</param>
      /// <param name="level">Maximal pyramid level number. If 0 , pyramids are not used (single level), if 1 , two levels are used, etc</param>
      /// <param name="criteria">Specifies when the iteration process of finding the flow for each point on each pyramid level should be stopped</param>
      /// <param name="flags">Flags</param>
      /// <param name="currFeatures">Array of 2D points containing calculated new positions of input features in the second image</param>
      /// <param name="status">Array. Every element of the array is set to 1 if the flow for the corresponding feature has been found, 0 otherwise</param>
      /// <param name="trackError">Array of double numbers containing difference between patches around the original and moved points</param>
      public static void PyrLK(
         Image<Gray, Byte> prev,
         Image<Gray, Byte> curr,
         Image<Gray, Byte> prevPyrBuffer,
         Image<Gray, Byte> currPyrBuffer,
         System.Drawing.PointF[] prevFeatures,
         System.Drawing.Size winSize,
         int level,
         MCvTermCriteria criteria,
         Emgu.CV.CvEnum.LKFLOW_TYPE flags,
         out System.Drawing.PointF[] currFeatures,
         out Byte[] status,
         out float[] trackError)
      {
         if (prevPyrBuffer == null)
         {
            prevPyrBuffer = new Image<Gray, byte>(prev.Width + 8, prev.Height / 3);
         }
         if (currPyrBuffer == null)
         {
            currPyrBuffer = prevPyrBuffer.CopyBlank();
         }

         status = new Byte[prevFeatures.Length];
         trackError = new float[prevFeatures.Length];

         currFeatures = new System.Drawing.PointF[prevFeatures.Length];

         CvInvoke.cvCalcOpticalFlowPyrLK(
            prev,
            curr,
            prevPyrBuffer,
            currPyrBuffer,
            prevFeatures,
            currFeatures,
            prevFeatures.Length,
            winSize,
            level,
            status,
            trackError,
            criteria,
            flags);
      }

        static void Main(string[] args)
        {
            Image<Gray, Byte>[] trainingImages = new Image<Gray, Byte>[2];
            trainingImages[0] = new Image<Gray, byte>("C:\\Image\\Romy.jpg");
            trainingImages[1] = new Image<Gray, byte>("C:\\Image\\Stevie.jpg");

            int[] labels = new int[] { 0 , 1 };

            MCvTermCriteria termCrit = new MCvTermCriteria(16, 0.001);

            EigenFaceRecognizer recognizer = new EigenFaceRecognizer(0,0.2);
            Image<Gray, Byte> testImage = new Image<Gray, Byte>("C:\\Image\\Stevie.jpg");

            recognizer.Train(trainingImages,labels);
            EigenFaceRecognizer.PredictionResult result = recognizer.Predict(testImage);

            Console.WriteLine(result.Label);
            Console.WriteLine(result.Label);
            Console.ReadKey();
        }