//private methods
int ParticleTrackingAlg::generateParticleList()
{
RNG rng;
for(int i=0; i<particleNum; i++)
{
Particle p(trackObject.x + rng.gaussian(Utility::xNoise), trackObject.y + rng.gaussian(Utility::yNoise), trackObject.width, trackObject.height);
particleList.push_back(p);
}
return 0;
}
static double avgNormalRealsN(double mean, double stddev, const int N)
{
RNG r;
double sum = 0.0;
for (int i = 0 ; i < N ; ++i)
sum += r.gaussian(mean, stddev);
return sum / (double)N;
}
int ParticleTrackingAlg::resampleParticleList()
{
double *cumPdf = new double[particleNum];
std::vector<Particle> newParticleList;
std::vector<Particle>::iterator particleIter;
double sumWeight = 0.0;
int index = 0;
for(particleIter=particleList.begin(); particleIter!=particleList.end(); ++particleIter)
{
sumWeight += particleIter->GetParticleWeight();
cumPdf[index] = sumWeight;
index++;
}
double randNum = 0.0;
particleIter = particleList.begin();
RNG rng;
for(int i=0; i<particleNum; i++)
{
randNum = rng.uniform(0.0, sumWeight);
int j;
for(j=0; j<particleNum; j++)
{
if(cumPdf[j] < randNum)
{
continue;
}
else
{
break;
}
}
Particle p((particleIter+j)->GetParticleRegion(), rng.gaussian(Utility::xNoise), rng.gaussian(Utility::yNoise));
p.SetParticleWeight((particleIter+j)->GetParticleWeight());
newParticleList.push_back(p);
}
particleList = newParticleList;
return 0;
}
int ParticleGroup::generateParticleList(const Particle &originParticle, int num)
{
RNG rng;
for(int i=0; i<num; i++)
{
Particle p(originParticle.xPos, originParticle.yPos, originParticle.width, originParticle.height);
p.weight = originParticle.weight;
p.xPos = p.xPos + rng.gaussian(xNoise);
p.yPos = p.yPos + rng.gaussian(yNoise);
ParticleList.push_back(p);
}
return 0;
}
void AddGaussianNoise ( const Mat& src, Mat& dest, int amp ) {
Mat temp = src.clone();
int rows = temp.rows;
int cols = temp.cols;
RNG rng;
for ( int i = 0 ; i<rows ; i++ ) {
uchar* row_pointer = temp.ptr(i);
for ( int j=0 ; j<cols ; j++ ) {
double rnd = rng.gaussian(1);
int v = (int)row_pointer[j]+amp*rnd;
row_pointer[j] = v;
}
}
dest = temp;
}
int ParticleGroup::resampleParticle()
{
int particleNum = ParticleList.size();
double *cumPdf = new double[particleNum];
int *newParticleIndex = new int [particleNum];
std::vector<Particle>::iterator particleIter;
std::vector<Particle> newParticleList;
double sum = 0.0;
int index = 0;
for(particleIter = ParticleList.begin(); particleIter != ParticleList.end(); ++particleIter)
{
sum += particleIter->weight;
cumPdf[index] = sum;
index++;
}
double randNum = 0.0;
particleIter = ParticleList.begin();
RNG rng;
for(int i=0; i<particleNum; i++)
{
randNum = rng.uniform(0.0, sum);
int j;
for(j=0; j<particleNum; j++)
{
if(cumPdf[j] < randNum)
{
continue;
}
else
{
break;
}
}
newParticleIndex[i] = j;
Particle p((particleIter+j)->xPos, (particleIter+j)->yPos, (particleIter+j)->width, (particleIter+j)->height);
p.weight = (particleIter+j)->weight;
p.xPos = p.xPos + rng.gaussian(xNoise);
p.yPos = p.yPos + rng.gaussian(yNoise);
newParticleList.push_back(p);
}
ParticleList = newParticleList;
return 0;
}
int main(int ac, char** av)
{
#if ExampleNum==1
Scalar Colors[]={Scalar(255,0,0),Scalar(0,255,0),Scalar(0,0,255),Scalar(255,255,0),Scalar(0,255,255),Scalar(255,0,255),Scalar(255,127,255),Scalar(127,0,255),Scalar(127,0,127)};
VideoCapture capture("..\\data\\TrackingBugs.mp4");
//VideoCapture capture("..\\..\\data\\atrium.avi");
if(!capture.isOpened())
{
return 0;
}
namedWindow("Video");
Mat frame;
Mat gray;
CTracker tracker(0.2,0.5,60.0,10,10);
capture >> frame;
cv::cvtColor(frame,gray,cv::COLOR_BGR2GRAY);
CDetector* detector=new CDetector(gray);
int k=0;
vector<Point2d> centers;
while(k!=27)
{
capture >> frame;
if(frame.empty())
{
capture.set(CV_CAP_PROP_POS_FRAMES,0);
continue;
}
cv::cvtColor(frame,gray,cv::COLOR_BGR2GRAY);
centers=detector->Detect(gray);
for(int i=0; i<centers.size(); i++)
{
circle(frame,centers[i],3,Scalar(0,255,0),1,CV_AA);
}
if(centers.size()>0)
{
tracker.Update(centers);
cout << tracker.tracks.size() << endl;
for(int i=0;i<tracker.tracks.size();i++)
{
if(tracker.tracks[i]->trace.size()>1)
{
for(int j=0;j<tracker.tracks[i]->trace.size()-1;j++)
{
line(frame,tracker.tracks[i]->trace[j],tracker.tracks[i]->trace[j+1],Colors[tracker.tracks[i]->track_id%9],2,CV_AA);
}
}
}
}
imshow("Video",frame);
k=waitKey(20);
}
delete detector;
destroyAllWindows();
return 0;
#else
int k=0;
Scalar Colors[]={Scalar(255,0,0),Scalar(0,255,0),Scalar(0,0,255),Scalar(255,255,0),Scalar(0,255,255),Scalar(255,255,255)};
cv::namedWindow("Video");
Mat frame=Mat(800,800,CV_8UC3);
VideoWriter vw=VideoWriter::VideoWriter("output.mpeg", CV_FOURCC('P','I','M','1'), 20, frame.size());
// Set mouse callback
cv::setMouseCallback("Video",mv_MouseCallback,0);
CTracker tracker(0.2,0.5,60.0,25,25);
float alpha=0;
while(k!=27)
{
frame=Scalar::all(0);
// Noise addition (measurements/detections simulation )
Xmeasured=X+rng.gaussian(2.0);
Ymeasured=Y+rng.gaussian(2.0);
// Append circulating around mouse points (frequently intersecting)
vector<Point2d> pts;
pts.push_back(Point2d(Xmeasured+100.0*sin(-alpha),Ymeasured+100.0*cos(-alpha)));
pts.push_back(Point2d(Xmeasured+100.0*sin(alpha),Ymeasured+100.0*cos(alpha)));
pts.push_back(Point2d(Xmeasured+100.0*sin(alpha/2.0),Ymeasured+100.0*cos(alpha/2.0)));
pts.push_back(Point2d(Xmeasured+100.0*sin(alpha/3.0),Ymeasured+100.0*cos(alpha/1.0)));
alpha+=0.05;
for(int i=0; i<pts.size(); i++)
{
circle(frame,pts[i],3,Scalar(0,255,0),1,CV_AA);
}
tracker.Update(pts);
std::cout << tracker.tracks.size() << endl;
for(int i=0;i<tracker.tracks.size();i++)
{
if(tracker.tracks[i]->trace.size()>1)
{
for(int j=0;j<tracker.tracks[i]->trace.size()-1;j++)
{
line(frame,tracker.tracks[i]->trace[j],tracker.tracks[i]->trace[j+1],Colors[i%6],2,CV_AA);
}
}
}
imshow("Video",frame);
vw << frame;
k=waitKey(10);
}
vw.release();
cv::destroyAllWindows();
return 0;
#endif
}
