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