Skip to content

Instantly share code, notes, and snippets.

@ritsz

ritsz/haar_mft_condensation.cpp

Created July 23, 2013 16:48
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save ritsz/6064012 to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save ritsz/6064012 to your computer and use it in GitHub Desktop.
Download ZIP
Median flow tracker with Applied condensation algorithm.
Raw
haar_mft_condensation.cpp
#include <cv.h>
#include <highgui.h>
#include <ml.h>
#include <math.h>
#include <algorithm>
#include <vector>
#include <stdio.h>
#include <iostream>
#include <cxcore.h>
#include <cvaux.h>
#include <cmath>
#include <pthread.h>
using namespace cv;
using namespace std;
vector<CvPoint> trail;
float median;
bool fast = false;
const int winHeight=600;
const int winWidth=800;
static CvMemStorage* storage = 0; // Dynamically growing memory for calculations
static CvHaarClassifierCascade* cascade = 0;
const char* cascade_name = "/usr/local/share/opencv/haarcascades/haarcascade_frontalface_alt.xml";
inline double euclid_dist(const CvPoint2D32f* point1, const CvPoint2D32f* point2) {
//this function calculates the euclidean distance between 2 points
double distance, xvec, yvec;
xvec = point2->x - point1->x;
yvec = point2->y - point1->y;
distance = sqrt((xvec * xvec) + (yvec * yvec));
return distance;
}
void pairwise_dist(const CvPoint2D32f* features, double *edist, int npoin) {
//calculate m x n euclidean pairwise distance matrix.
for (int i = 0; i < npoin; i++) {
for (int j = 0; j < npoin; j++) {
int ind = npoin*i + j;
edist[ind] = euclid_dist(&features[i],&features[j]);
}
}
}
void ncc_filter(IplImage *frame1, IplImage *frame2, CvPoint2D32f *prev_feat, CvPoint2D32f *curr_feat,
int npoin, int method, IplImage *rec0, IplImage *rec1, IplImage *res, int *ncc_pass) {
int filt = npoin/2;
vector<float> ncc_err (npoin,0.0);
for (int i = 0; i < npoin; i++) {
cvGetRectSubPix( frame1, rec0, prev_feat[i] );
cvGetRectSubPix( frame2, rec1, curr_feat[i] );
cvMatchTemplate( rec0,rec1, res, method );
ncc_err[i] = ((float *)(res->imageData))[0];
}
vector<float> err_copy (ncc_err);
sort(ncc_err.begin(), ncc_err.end());
double median = (ncc_err[filt]+ncc_err[filt-1])/2.;
for(int i = 0; i < npoin; i++) {
if (err_copy[i] >= median) {
ncc_pass[i] = 1;
}
else {
ncc_pass[i] = 0;
}
}
}
void fb_filter(const CvPoint2D32f* prev_features, const CvPoint2D32f* backward_features,
const CvPoint2D32f* curr_feat, int *fb_pass, const int npoin) {
//this function implements forward-backward error filtering
vector<double> euclidean_dist (npoin,0.0);
int filt = npoin/2;
for(int i = 0; i < npoin; i++) {
euclidean_dist[i] = euclid_dist(&prev_features[i], &backward_features[i]);
}
vector<double> err_copy (euclidean_dist);
//use the STL sort algorithm to filter results
sort(euclidean_dist.begin(), euclidean_dist.end());
median = (euclidean_dist[filt]+euclidean_dist[filt-1])/2.;
for(int i = 0; i < npoin; i++) {
if (err_copy[i] <= median) {
fb_pass[i] = 1;
}
else {
fb_pass[i] = 0;
}
}
}
void bbox_move(const CvPoint2D32f* prev_feat, const CvPoint2D32f* curr_feat, const int npoin,
double &xmean, double &ymean) {
/*Calculate bounding box motion. */
vector<double> xvec (npoin,0.0);
vector<double> yvec (npoin,0.0);
for (int i = 0; i < npoin; i++) {
xvec[i] = curr_feat[i].x - prev_feat[i].x;
yvec[i] = curr_feat[i].y - prev_feat[i].y;
}
sort(xvec.begin(), xvec.end());
sort(yvec.begin(), yvec.end());
xmean = xvec[npoin/2];
ymean = yvec[npoin/2];
}
int main( int argc,char** argv)
{
const int stateNum = 4;
const int measureNum = 2;
const int sampleNum = 2000;
CvConDensation* condens = cvCreateConDensation(stateNum,measureNum,sampleNum);
CvMat* lowerBound;
CvMat* upperBound;
lowerBound = cvCreateMat(stateNum, 1, CV_32F);
upperBound = cvCreateMat(stateNum, 1, CV_32F);
cvmSet(lowerBound,0,0,0.0 );
cvmSet(upperBound,0,0,winWidth );
cvmSet(lowerBound,1,0,0.0 );
cvmSet(upperBound,1,0,winHeight );
cvmSet(lowerBound,2,0,0.0 );
cvmSet(upperBound,2,0,0.0 );
cvmSet(lowerBound,3,0,0.0 );
cvmSet(upperBound,3,0,0.0 );
float A[stateNum][stateNum] ={
1,0,1,0,
0,1,0,1,
0,0,1,0,
0,0,0,1
};
memcpy(condens->DynamMatr,A,sizeof(A));
cvConDensInitSampleSet(condens, lowerBound, upperBound);
CvCapture* capture = cvCreateCameraCapture(0);
cascade = (CvHaarClassifierCascade*)cvLoad(cascade_name,0,0,0);
if(!cascade){ fprintf(stderr,"ERROR: Cascade not loaded!! \nCheck the path given for it on line 17 of code \n"); return -1; } // Check whether cascades have been loaded
if(!capture){ fprintf(stderr,"ERROR: Camera not loaded \n"); return -1;} // Check whether camera has been loaded
storage = cvCreateMemStorage(0);
// Loop to get the Bounding Box from haar cascades
IplImage* frame = 0;
IplImage* displayed_image = 0;
CvPoint pt1,pt2;
CvPoint pt;
int i;
detect:cvClearMemStorage(storage);
cvNamedWindow("Results",CV_WINDOW_AUTOSIZE);
cvMoveWindow("Results",700,0);
int pcount_prev = 1;
while(1)
{
frame = cvQueryFrame(capture);
if(!frame) { fprintf(stderr,"Camera doesn't return frames \n"); return -1;}
displayed_image = cvCreateImage(cvSize(frame->width,frame->height),IPL_DEPTH_8U,3);
cvCopy(frame,displayed_image,NULL);
CvSeq* faces = cvHaarDetectObjects(frame,cascade,storage,1.1,2,CV_HAAR_DO_CANNY_PRUNING,cvSize(40,40));
for(i = 0;i<(faces? faces->total:0); i++)
{
CvRect* r = (CvRect*)cvGetSeqElem(faces,i);
pt1.x = r->x;
pt2.x = r->x + r->width;
pt1.y = r->y;
pt2.y = r->y + r->height;
cvRectangle(displayed_image,pt1,pt2,CV_RGB(255,0,0),3,8,0);
}
if(fast) { fast = false ;break; }
else cvShowImage("Results",displayed_image);
char bar = cvWaitKey(33);
if(bar == 32) break;
}
static IplImage *frame1_1C = NULL, *frame2_1C = NULL, *pyramid1 = NULL, *pyramid2 = NULL;
frame1_1C = cvCreateImage(cvSize(frame->width,frame->height),IPL_DEPTH_8U,1);
frame2_1C = cvCreateImage(cvSize(frame->width,frame->height),IPL_DEPTH_8U,1);
int npoints = 400;
int pcount;
int winsize = 30;
IplImage *rec0 = cvCreateImage( cvSize(winsize, winsize), 8, 1 );
IplImage *rec1 = cvCreateImage( cvSize(winsize, winsize), 8, 1 );
IplImage *res = cvCreateImage( cvSize( 1, 1 ), IPL_DEPTH_32F, 1 );
/* This array will contain the locations of the points from frame 1 in frame 2. */
vector<CvPoint2D32f> frame1_features(npoints);
vector<CvPoint2D32f> frame2_features(npoints);
vector<CvPoint2D32f> FB_features(npoints);
/* The i-th element of this array will be non-zero if and only if the i-th feature of
* frame 1 was found in frame 2.
*/
char optical_flow_found_feature[npoints];
char optical_flow_found_feature2[npoints];
float optical_flow_feature_error[npoints];
vector<int> fb_pass(npoints);
vector<int> ncc_pass(npoints);
double bbox_x = pt1.x;
double bbox_y = pt1.y;
double bbox_width = (pt2.x - pt1.x);
double bbox_height = (pt2.y - pt1.y );
printf("\n\tRectangle made at (%g,%g) and size (%g,%g)\n\tWe have got a Bounding Box\n\n",bbox_x,bbox_y,bbox_width,bbox_height);
bool isPredictOnly=false;
//Main Loop for tracking
while(true)
{
cvConvertImage(frame,frame1_1C,0);
//Making 400 new points for PyrLK to track
for(i = 0;i<20;i++)
{
for(int j = 0;j<20;j++)
{
int l = i*20 + j;
frame1_features[l].x = bbox_x + (bbox_width/20)*i + (bbox_width/40);
frame1_features[l].y = bbox_y + (bbox_height/20)*j + (bbox_height/40);
}
}
// 400 new feature points made
//Second Frame Capture, converted to grey scale and displayed
frame = cvQueryFrame(capture);
cvConvertImage(frame,frame2_1C,0);
cvCopy(frame,displayed_image,NULL);
// Pyr Lucas kanade Optical Flow
CvTermCriteria term = cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.3);
pyramid1 = cvCreateImage(cvSize(frame->width,frame->height),IPL_DEPTH_8U,1);
pyramid2 = cvCreateImage(cvSize(frame->width,frame->height),IPL_DEPTH_8U,1);
cvCalcOpticalFlowPyrLK(frame1_1C,frame2_1C,pyramid1,pyramid2,&frame1_features[0],&frame2_features[0],npoints,cvSize(3,3),5,optical_flow_found_feature,optical_flow_feature_error,term,0);
cvCalcOpticalFlowPyrLK(frame2_1C,frame1_1C,pyramid1,pyramid2,&frame2_features[0],&FB_features[0] ,npoints,cvSize(3,3),5,optical_flow_found_feature2,optical_flow_feature_error,term,0);
double xmean = 0;
double ymean = 0;
//filter the cascade
fb_filter(&frame1_features[0], &FB_features[0], &frame2_features[0], &fb_pass[0], npoints);
ncc_filter(frame1_1C,frame2_1C,&frame1_features[0],&frame2_features[0],
npoints,CV_TM_CCOEFF_NORMED, rec0, rec1, res, &ncc_pass[0]);
pcount = 0;
for(int i = 0; i<npoints;i++)
{
if(fb_pass[i] && ncc_pass[i])
{
pcount++;
}
}
float scaled = 1;
if( (pcount/pcount_prev < 2) && (pcount/pcount_prev > 0) )
{
scaled = pcount/pcount_prev;
}
pcount_prev = pcount;
fprintf(stderr,"Passed Points %d ### median %f \n",pcount , median );
if(pcount == 0) { fprintf(stderr," No point tracked currently: EXITING"); return -1; }
vector<CvPoint2D32f> curr_features2(pcount),prev_features2(pcount);
int j = 0;
for( i = 0; i< npoints; i++)
{
if(fb_pass[i] && ncc_pass[i])
{
curr_features2[j] = frame2_features[i];
prev_features2[j] = frame1_features[i];
j++;
}
}
int n2 = pcount*pcount;
vector<double> pdist_prev(n2),pdist_curr(n2),pdiv(n2);
pairwise_dist(&prev_features2[0],&pdist_prev[0],pcount);
pairwise_dist(&curr_features2[0],&pdist_curr[0],pcount);
for (int i = 0; i < n2; i++) {
if (pdist_prev[i] > 0.0) {
pdiv[i] = pdist_curr[i]/pdist_prev[i];
}
}
sort(pdiv.begin(),pdiv.end());
//fprintf(stderr, "box_scale: %3.4f", pdiv[n2/2]);
double box_scale;
box_scale = pdiv[n2/2];
double track_x,track_y;
if(median > 100.0){fast = true; goto detect;}
bbox_move(&prev_features2[0],&curr_features2[0],pcount,xmean,ymean);
bbox_x = bbox_x + (xmean) - bbox_width*(box_scale - 1.)/2.;
bbox_y = bbox_y + (ymean) - bbox_height*(box_scale - 1.)/2.;
bbox_width = bbox_width * (box_scale);
bbox_height = bbox_height * (box_scale);
track_x = bbox_x + (bbox_width)/2;
track_y = bbox_y + (bbox_height)/2;
pt.x = track_x;
pt.y = track_y;
trail.push_back(cvPoint(track_x,track_y));
if(trail.size() >= 30)
{
trail.erase(trail.begin());
}
for( int b = 0; b<trail.size() - 1; b++)
{
cvLine(displayed_image,trail[i],trail[i+1],cvScalar(0,0,255),2);
}
CvPoint predict_pt=cvPoint((int)condens->State[0],(int)condens->State[1]);
float variance[measureNum]={0};
//get variance/standard deviation of each state
for (i=0;i<measureNum;i++) {
//sum
float sumState=0;
for (int j=0;j<condens->SamplesNum;j++) {
sumState+=condens->flSamples[i][j];
}
//average
sumState/=sampleNum;
//variance
for (int j=0;j<condens->SamplesNum;j++) {
variance[i]+=(condens->flSamples[i][j]-sumState)*
(condens->flSamples[i][j]-sumState);
}
variance[i]/=sampleNum-1;
}
//3.update particals confidence
if (isPredictOnly) {
pt=predict_pt;
}else{
pt=cvPoint(track_x,track_y);
}
for (int i=0;i<condens->SamplesNum;i++) {
float probX=(float)exp(-1*(pt.x-condens->flSamples[i][0])
*(pt.x-condens->flSamples[i][0])/(2*variance[0]));
float probY=(float)exp(-1*(pt.y-condens->flSamples[i][1])
*(pt.y-condens->flSamples[i][1])/(2*variance[1]));
condens->flConfidence[i]=probX*probY;
}
//4.update condensation
cvConDensUpdateByTime(condens);
cvRectangle(displayed_image, cvPoint(bbox_x, bbox_y),
cvPoint(bbox_x+bbox_width,bbox_y+bbox_height),
cvScalar(0xff,0x00,0x00) );
for (int i = 0; i < pcount; i++) {
cvCircle(displayed_image, cvPoint(curr_features2[i].x, curr_features2[i].y), 1, cvScalar(255,255,255));
}
//cvCircle(displayed_image,cvPoint(track_x,track_y),5,cvScalar(0,0,255),5);
char online_dist[] = "0.0";
CvFont bfont;
double hscale = 0.5;
double vscale = 0.5;
cvInitFont(&bfont, CV_FONT_HERSHEY_SIMPLEX, hscale, vscale,0,1);
cvPutText(displayed_image, online_dist, cvPoint(bbox_x+bbox_width + 20,bbox_y), &bfont, cvScalar(0,0,255));
if (isPredictOnly) {
pt=predict_pt;
}else{
pt=cvPoint(track_x,track_y);
}
for (int i=0;i<condens->SamplesNum;i++) {
float probX=(float)exp(-1*(pt.x-condens->flSamples[i][0])
*(pt.x-condens->flSamples[i][0])/(2*variance[0]));
float probY=(float)exp(-1*(pt.y-condens->flSamples[i][1])
*(pt.y-condens->flSamples[i][1])/(2*variance[1]));
condens->flConfidence[i]=probX*probY;
}
if(track_x >600 || track_x<20) track_x = pt.x;
trail.push_back(pt);
if(trail.size() == 10)
{
trail.erase(trail.begin());
}
for( int b = 0; b<trail.size() - 1; b++)
{
cvLine(displayed_image,trail[i],trail[i+1],cvScalar(255,0,0),2);
}
cvCircle(displayed_image,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with green
cvCircle(displayed_image,cvPoint(track_x,track_y),5,CV_RGB(0,0,255),3);
cvShowImage("Results",displayed_image);
char esc = cvWaitKey(33);
if(esc == 27) break;
if(esc == 32) goto detect;
} // End of Main While Loop
cvReleaseImage(&frame);
cvDestroyWindow("Results");
cvReleaseImage(&frame1_1C);
cvReleaseImage(&frame2_1C);
cvReleaseImage(&pyramid1);
cvReleaseImage(&pyramid2);
cvReleaseCapture(&capture);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment