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