public double CheckAffinity(Particle p, double anglespan = 20, double bounderDistance = 100) { /* * IMPORTANT: * The formula of the sigma computation is not correct, that is relative to the decreasing exponential, not normal distribution. * * */ // Confidence: ANGLE_AFFINITY * DISTANCE_AFFINITY double confOverall; // anglespan is the angle span at which the distribution is 0.1 double SIGMA_angle = Math.Sqrt(Math.Pow(anglespan,2)/4.6); // BounderyDistance is the distance at which the distribution is 0.1 double SIGMA_distance = Math.Sqrt(Math.Pow(bounderDistance,2) / 4.6); // check distance affinity double d = GaussianDistribution(this.PointDistance(p.Position()), SIGMA_distance); //check angle affinity double angDif = Math.Abs(this.angle - p.angle); if (angDif > 180) angDif = 360 - angDif; double a = GaussianDistribution(angDif, SIGMA_angle); // Overall Affinity confOverall= a*d; return confOverall; }
public void CopyTo(Particle pOut) { pOut.id = this.id; pOut.state = this.state; pOut.lastState = this.lastState; pOut.pAlone = new List<float>(this.pAlone); pOut.direction = this.direction; pOut.angle = this.angle; pOut.groupLabel = this.groupLabel; pOut.posList = new List<Point>(this.posList); pOut.wholePosList = new List<Point>(this.wholePosList); pOut.fnList = new List<int>(this.fnList); pOut.wholeFnList = new List<int>(this.wholeFnList); pOut.tWin = this.tWin; }
private static List<Particle> GridGFTTUpdate(Image<Gray, byte> frame, List<Particle> particleList, int dimCells, int numPerCell, double notTooCloseThan = 10, int windowLength = 20) { Size s = new Size(dimCells, dimCells); List<PointF> pointList = new List<PointF>(); for (int i = 0; i < frame.Width; i = i + dimCells) { for (int j = 0; j < frame.Height; j = j + dimCells) { // cut the ROI Image<Gray, byte> tmp = new Image<Gray, byte>(s); Rectangle roi = new Rectangle(new Point(i, j), s); frame.ROI = roi; tmp = frame.Copy(); frame.ROI = new Rectangle(); // how many points we need to create? int pointsInside = 0; foreach (Particle p in particleList) { if (roi.Contains(p.Position())) pointsInside++; } // init new points if not enough if (pointsInside < numPerCell) { int pointToInit = numPerCell - pointsInside; PointF[][] tmpPoints = tmp.GoodFeaturesToTrack(pointToInit, 0.01, 10, 3); foreach (PointF p in tmpPoints[0]) { PointF tmpPoint = p; tmpPoint.X += i; tmpPoint.Y += j; pointList.Add(tmpPoint); } } } } // convert points in new particles if (particleList.Count == 0) return particleList; int maxId = particleList.Max<Particle>(p => p.GetID()); //if (maxId > 100000) // maxId = 0; int fn = particleList[0].GetFrameNow(); foreach (PointF p in pointList) { while (particleList.Find(t => t.GetID() == maxId) != null) maxId++; Particle part = new Particle(maxId, new Point(Convert.ToInt32(p.X), Convert.ToInt32(p.Y)), fn, windowLength); particleList.Add(part); } // Delete overlapping particles for (int i = particleList.Count - 1; i != 0; i--) { Point pos = particleList[i].Position(); for (int j = 0; j < i; j++) { Point pos2 = particleList[j].Position(); if (particleList[j].PointDistance(pos) < notTooCloseThan) { particleList.RemoveAt(i); break; } } } return particleList; }
static void Main(string[] args) { /// Getting parameters /// inputValues iVal = ReadInput(); PrintParams(iVal); string videoNameFile = iVal.videoIn; if (!File.Exists(videoNameFile)) { Console.WriteLine("File {0} doesn't exist!", videoNameFile); Environment.Exit(1); } string workingDirectory = Path.GetDirectoryName(videoNameFile); //string outputFileName = string.Format("{0}_out1.avi", videoNameFile.Remove(videoNameFile.Length - 4)); int windowLength = iVal.windowLength; int nParticles = iVal.nParticles; int blockDim = iVal.blockDim; double SIGMA = iVal.SIGMA; // in/out streams Capture cap = new Capture(Path.Combine(workingDirectory, videoNameFile)); MCvFont font = new MCvFont(Emgu.CV.CvEnum.FONT.CV_FONT_HERSHEY_PLAIN, 1, 1); VideoWriter wr = new VideoWriter(iVal.videoOut, //Path.Combine(workingDirectory, outputFileName), CvInvoke.CV_FOURCC('D', 'I', 'V', 'X'), (int)cap.GetCaptureProperty(Emgu.CV.CvEnum.CAP_PROP.CV_CAP_PROP_FPS), (int)cap.GetCaptureProperty(Emgu.CV.CvEnum.CAP_PROP.CV_CAP_PROP_FRAME_WIDTH), (int)cap.GetCaptureProperty(Emgu.CV.CvEnum.CAP_PROP.CV_CAP_PROP_FRAME_HEIGHT), true); // Print Results StreamWriter rw = new StreamWriter(Path.Combine(workingDirectory, iVal.videoOut.Remove(iVal.videoOut.Length - 4) + ".txt"));//videoNameFile.Remove(videoNameFile.Length - 4) + ".txt")); // LK params MCvTermCriteria termCrit = new MCvTermCriteria(10000, 0.0001); byte[] bs; float[] fs; PointF[] prevPoints = new PointF[nParticles]; PointF[] newPoints = new PointF[nParticles]; // Video frames currentFrame = cap.QueryFrame(); Image<Bgr, byte> previousFrame = currentFrame.Clone(); Image<Gray, byte> currentFrameGray = new Image<Gray, byte>(currentFrame.Bitmap); Image<Gray, byte> previousFrameGray = currentFrameGray.Clone(); // Particle Initialization List<Particle> pList = GridGFTTinit(currentFrameGray, blockDim, nParticles, windowLength); prevPoints = ParticleToPointF(pList); newPoints = prevPoints; // State Matrix Matrix<float> stateMat = iVal.stateMat; //ReadStateMatrix2x2(); while (frameNumber < frameToProcess) { Console.WriteLine("------------Frame Number: {0} ---------", frameNumber); //if (currentFrame == null) //{ // currentFrame = cap.QueryFrame(); // frameNumber++; //} // Get Pointf[] from ParticleList List<Particle> pOld = new List<Particle>(); foreach (Particle p in pList) { Particle tmpParticle = new Particle(0, new Point(), 0); p.CopyTo(tmpParticle); pOld.Add(tmpParticle); } prevPoints = ParticleToPointF(pList); // Tracking old particles OpticalFlow.PyrLK(previousFrameGray, currentFrameGray, prevPoints, new System.Drawing.Size(15, 15), 3, termCrit, out newPoints, out bs, out fs); prevPoints = newPoints; for (int i = 0; i < newPoints.Length; i++ ) { Point p = new Point(Convert.ToInt32(newPoints[i].X), Convert.ToInt32(newPoints[i].Y)); pList[i].Update(p, frameNumber); } // Particle Thickening pList = GridGFTTUpdate(currentFrameGray, pList, blockDim, nParticles, 3, windowLength); Console.WriteLine("Particle Number: {0}.", pList.Count); //debug line if (pList.Count == 0) pList.Add(new Particle(-1, new Point(), frameNumber, windowLength)); // Particle classification and mitigation EUSIPCO INFLUENCE Matrix<float> InfluenceMatrix = CalcMatrixRdynamic(pList, SIGMA); //Matrix<float> InfluenceMatrix = CalcMatrixRhybrid(pList, SIGMA); Console.WriteLine("Matrix R Computed."); //MarkovChain(ref pList, InfluenceMatrix, stateMat); ForwardAlgorithm(ref pList, InfluenceMatrix, stateMat, iVal.condMat, iVal.hmmLength); Console.WriteLine("Influence model completed."); // to write trajectories List<int> pExpList = ExpiredParticles(pList, pOld); foreach (int i in pExpList) { Particle part = pList.First<Particle>(p => p.GetID() == i); rw.Write("{0}\t", part.wholePosList.Count); for (int pt = 0; pt < part.wholePosList.Count; pt++) { rw.Write("({0},{1},{2})", part.wholePosList[pt].X, part.wholePosList[pt].Y, part.wholeFnList[pt]); } rw.WriteLine(""); part.ClearPosList(); } rw.Flush(); ////////// Draw & display points List<Particle> finalParticles = new List<Particle>(); int count = 0; foreach (Particle p in pList) { if (p.state == 1) { currentFrame.Draw(new CircleF(p.Position(), 1), new Bgr(Color.Blue), lineStroke); finalParticles.Add(p); } else if (p.state == 2) { //Point posText = p.Position(); //posText.Y += 10; //currentFrame.Draw(new CircleF(p.Position(), 1), new Bgr(Color.Yellow), lineStroke); //currentFrame.Draw(string.Format("{0}", p.GetID()), ref font, posText, new Bgr(Color.Yellow)); Point prevpo = p.posList[p.posList.Count - 1]; bool jumpcheck = false; for (int j = 1; j < iVal.hmmLength - 1; j++) { Point actpo = p.posList[p.posList.Count - 1 - j]; if (PointDistance(actpo, prevpo) > 50) jumpcheck = true; } if (jumpcheck) continue; prevpo = p.posList[p.posList.Count - 1]; //currentFrame.Draw(p.GetID().ToString(), ref font, p.Position(), new Bgr( Color.Beige)); for (int j = 1; j < iVal.hmmLength - 1; j++) { Point actpo = p.posList[p.posList.Count - 1 - j]; currentFrame.Draw(new LineSegment2D(actpo, prevpo), new Bgr(Color.Yellow), lineStroke); prevpo = actpo; } finalParticles.Add(p); //rw.WriteLine("{0}\t{1}\t{2}\t{3}", frameNumber, p.Position().X, p.Position().Y, p.groupLabel); } else if (p.state == 0) { Console.WriteLine(string.Format("Particle {0} killed", count)); } else if (p.state == 3) { finalParticles.Add(p); } count++; } pList = finalParticles; currentFrame.Draw(string.Format("Frame: {0}", frameNumber), ref font, new Point(50, 50), new Bgr(Color.Yellow)); wr.WriteFrame<Bgr, byte>(currentFrame); previousFrame = currentFrame.Clone(); double ratio = 1; if (cap.Width > cap.Height) { ratio = 1000 / (double)cap.Width; } else { ratio = 1000 / (double)cap.Height; } CvInvoke.cvShowImage("Video", previousFrame.Resize(ratio, Emgu.CV.CvEnum.INTER.CV_INTER_CUBIC).Ptr); CvInvoke.cvWaitKey(10); if (frameNumber > windowLength - 3 ) { //currentFrame.Resize(ratio, Emgu.CV.CvEnum.INTER.CV_INTER_CUBIC).Save(string.Format(@"frames/debug_{0}.jpg", frameNumber)); } // Update frame (current, gray and previous) try { currentFrame = cap.QueryFrame(); frameNumber++; //cap.SetCaptureProperty(Emgu.CV.CvEnum.CAP_PROP.CV_CAP_PROP_POS_FRAMES, frameNumber); //currentFrame = cap.QueryFrame(); previousFrameGray = currentFrameGray.Clone(); currentFrameGray = new Image<Gray, byte>(currentFrame.Bitmap); } catch (NullReferenceException) { break; } } //rw.Flush(); rw.Close(); //List<string> emailText = new List<string>(); //emailText.Add(string.Format("Finito con file {0}, PARAMS:\nProcessed Frames: {1}", videoNameFile, frameToProcess)); //emailText.Add(string.Format("BlockDim: {0}", blockDim)); //emailText.Add(string.Format("N. particles per block: {0}", nParticles)); //emailText.Add(string.Format("Window Length: {0}", windowLength)); //emailText.Add(string.Format("SIGMA: {0}", SIGMA)); //emailText.Add(string.Format("State Matrix:\n{0} {1}\n{2} {3}", stateMat[0,0], stateMat[0,1], stateMat[1,0], stateMat[1,1])); //SendEmailAlert(emailText.ToArray()); }
private static List<Particle> GridGFTTinit(Image<Gray, byte> frame, int dimCells, int numPerCell, int windowLength = 20) { Size s = new Size(dimCells, dimCells); List<PointF> pointList = new List<PointF>(); for (int i = 0; i < frame.Width; i = i + dimCells) { for (int j = 0; j < frame.Height; j = j + dimCells) { Image<Gray, byte> tmp = new Image<Gray, byte>(s); frame.ROI = new Rectangle(new Point(i, j), s); tmp = frame.Copy(); frame.ROI = new Rectangle(); PointF[][] tmpPoints = tmp.GoodFeaturesToTrack(numPerCell, 0.01, 10, 3); foreach (PointF p in tmpPoints[0]) { PointF tmpPoint = p; tmpPoint.X += i; tmpPoint.Y += j; pointList.Add(tmpPoint); } } } int idCounter = 0; PointF[] result = pointList.ToArray(); List<Particle> pList = new List<Particle>(); for (int i = 0; i < result.Length; i++) { Particle p = new Particle(idCounter, new Point(Convert.ToInt32(result[i].X), Convert.ToInt32(result[i].Y)), 1, windowLength); pList.Add(p); idCounter++; } return pList; }