void * distance_thread (void * arg)
{
targ_t * targ = (targ_t*)arg;
// Calculate distances
calc_distances(targ);
return (void*)(999);
}
int lk_work(CAMOBJ * st)
{
int i, k;
float mx,my,cx,cy;
// frame = cvQueryFrame( capture );
if( !frame )
return(1);
if( !image ) // allocate all the buffers
{
image = cvCreateImage( cvGetSize(frame), 8, 3 );
image->origin = frame->origin;
grey = cvCreateImage( cvGetSize(frame), 8, 1 );
prev_grey = cvCreateImage( cvGetSize(frame), 8, 1 );
save_grey = cvCreateImage( cvGetSize(frame), 8, 1 );
pyramid = cvCreateImage( cvGetSize(frame), 8, 1 );
prev_pyramid = cvCreateImage( cvGetSize(frame), 8, 1 );
save_pyramid = cvCreateImage( cvGetSize(frame), 8, 1 );
points[0] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
points[1] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
save_points = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
status = (char*)cvAlloc(MAX_COUNT);
for (i=0;i<MAX_COUNT;i++) pt_mode[i]=0;
flags = 0;
statuscount++;
}
cvCopy( frame, image, 0 );
if (st->mode==1)
{
if (!video_writer)
video_writer = cvCreateVideoWriter(st->videofilename,-1,15,cvGetSize(image));
cvWriteFrame(video_writer,image);
}
if (st->enable_tracking)
{
cvCvtColor( image, grey, CV_BGR2GRAY );
if( night_mode )
cvZero( image );
if (need_to_init)
{
need_to_init=0;
init_flag=0;
if (st->trackface)
{
if (detect_face())
{
int x;
count=2;
cvFindCornerSubPix( grey, points[1], count,
cvSize(win_size,win_size), cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER,1,1.0));
// cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
cvCopy(grey,save_grey,0 );
cvCopy(pyramid,save_pyramid,0 );
cvCopy(grey,prev_grey,0 );
cvCopy(pyramid,prev_pyramid,0 );
for (x=0;x<count;x++)
{
save_points[x].x=points[1][x].x;
save_points[x].y=points[1][x].y;
points[0][x].x=points[1][x].x;
points[0][x].y=points[1][x].y;
save_pt_mode[x]=pt_mode[x];
}
calc_distances(1);
save_count=count;
add_remove_pt = 0;
flags = 0;
time_to_restore=0;
}
}
else
{
save_points[0].x=PT1_xpos*100;
save_points[0].y=PT1_ypos*100;
points[0][0].x=PT1_xpos*100;
points[0][0].y=PT1_ypos*100;
save_pt_mode[0]=0;
count=1;MAX_COUNT=1;
calc_distances(1);
cvFindCornerSubPix( grey, points[1], 1,
cvSize(win_size,win_size), cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER,1,1.0));
// report("hallo");
cvCopy(grey,save_grey,0 );
cvCopy(pyramid,save_pyramid,0 );
cvCopy(grey,prev_grey,0 );
cvCopy(pyramid,prev_pyramid,0 );
save_count=1;
add_remove_pt = 0;
flags = 0;
//time_to_restore=0;
}
}
if(count < MAX_COUNT) need_to_init=1;
else
{
cvCalcOpticalFlowPyrLK( prev_grey, grey, prev_pyramid, pyramid,
points[0], points[1], count, cvSize(win_size,win_size), 5, status, 0,
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), flags );
flags |= CV_LKFLOW_PYR_A_READY;
mx=0;my=0;
cx=0;cy=0;mcount=0;ccount=0;
for( i = k = 0; i < count; i++ )
{
if( add_remove_pt )
{
double dx = pt.x - points[1][i].x;
double dy = pt.y - points[1][i].y;
if( dx*dx + dy*dy <= 25 )
{
add_remove_pt = 0;
if (pt_mode[i]==1) {pt_mode[i]=0; continue;}
pt_mode[i]=1;
}
}
if( !status[i] ) { need_to_init=1; status[i]=true; }
if (pt_mode[i]==1)
{
cx+= (points[0][i].x - points[1][i].x);
cy+= (points[0][i].y - points[1][i].y);
ccount++;
}
else
{
mx += (points[0][i].x - points[1][i].x);
my += (points[0][i].y - points[1][i].y);
mcount++;
}
points[1][k] = points[1][i];
pt_mode[k++]=pt_mode[i];
if (need_to_init)
cvCircle( image, cvPointFrom32f(points[1][i]), 4, CV_RGB(255,0,0), 2, 8,0);
else if (pt_mode[i]==1)
cvCircle( image, cvPointFrom32f(points[1][i]), 4, CV_RGB(255,255,0), 2, 8,0);
else
cvCircle( image, cvPointFrom32f(points[1][i]), 4, CV_RGB(0,210,0), 2, 8,0);
}
count = k;
if (k==MAX_COUNT)
{
if (init_flag>1)
{
x_move=mx/mcount;
y_move=my/mcount;
x_click=cx/ccount;
y_click=cy/ccount;
}
if (st->trackface) calc_distances(0); else calc_distances(2);
if ((autorestore)) // && (init_flag>5))
{
if (st->trackface)
{
if ((dist_error>=dist_threshold) || (angle_error>=angle_threshold))
time_to_restore++;
else time_to_restore=0;
if (time_to_restore>threshold_time)
{ need_to_init=1; time_to_restore=0; }
}
else
{
if ((dist_error>=dist_threshold))
time_to_restore++;
else time_to_restore=0;
if (time_to_restore>threshold_time)
{ need_to_init=1; time_to_restore=0; }
}
}
}
}
if( add_remove_pt && count < MAX_COUNT )
{
points[1][count++] = cvPointTo32f(pt);
cvFindCornerSubPix( grey, points[1] + count - 1, 1,
cvSize(win_size,win_size), cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
add_remove_pt = 0;
}
}
CV_SWAP( prev_grey, grey, swap_temp );
CV_SWAP( prev_pyramid, pyramid, swap_temp );
CV_SWAP( points[0], points[1], swap_points );
if (init_flag<1000) init_flag++;
if (st->showlive) cvShowImage( "Camera", image );
return(0);
}
