From ab1fb8a513fed73c8005b1dc7bcc28e565033b86 Mon Sep 17 00:00:00 2001 From: "N.N." Date: Tue, 13 Jul 2010 20:07:51 +0000 Subject: fixed angle calculations svn path=/trunk/externals/pdp_opencv/; revision=13710 --- pdp_opencv_of_lk.cc | 50 ++++++++++++++++++++++++++++---------------------- 1 file changed, 28 insertions(+), 22 deletions(-) (limited to 'pdp_opencv_of_lk.cc') diff --git a/pdp_opencv_of_lk.cc b/pdp_opencv_of_lk.cc index 425d484..072cf7c 100755 --- a/pdp_opencv_of_lk.cc +++ b/pdp_opencv_of_lk.cc @@ -76,7 +76,7 @@ static void pdp_opencv_of_lk_process_rgb(t_pdp_opencv_of_lk *x) int i,j,k,im; int marked; int px,py; - double meanangle=0.0, meanx=0.0, meany=0.0, maxamp=0.0, maxangle=0.0; + double globangle=0.0, globx=0.0, globy=0.0, maxamp=0.0, maxangle=0.0; int nbblocks=0; CvPoint orig, dest; double angle=0.0; @@ -129,6 +129,9 @@ static void pdp_opencv_of_lk_process_rgb(t_pdp_opencv_of_lk *x) x->x_winsize, x->x_velx, x->x_vely ); nbblocks = 0; + globx=0; + globy=0; + globangle=0; for( py=0; pyx_velsize.height; py++ ) { for( px=0; pxx_velsize.width; px++ ) @@ -136,10 +139,10 @@ static void pdp_opencv_of_lk_process_rgb(t_pdp_opencv_of_lk *x) // post( "pdp_opencv_of_lk : (%d,%d) values (%f,%f)", px, py, velxf, velyf ); orig.x = px; orig.y = py; - dest.x = (int)(orig.x + cvGet2D(x->x_velx, py, px).val[0]); - dest.y = (int)(orig.y + cvGet2D(x->x_vely, py, px).val[0]); - angle = -atan2( (double) cvGet2D(x->x_vely, py, px).val[0], (double) cvGet2D(x->x_velx, py, px).val[0] ); - hypotenuse = sqrt( pow(orig.y - dest.y, 2) + pow(orig.x - dest.x, 2) ); + dest.x = (int)(px + cvGet2D(x->x_velx, py, px).val[0]); + dest.y = (int)(py + cvGet2D(x->x_vely, py, px).val[0]); + angle = -atan2( (double) (dest.y-orig.y), (double) (dest.x-orig.x) ); + hypotenuse = sqrt( pow(dest.y - orig.y, 2) + pow(dest.x - orig.x, 2) ); /* Now draw the tips of the arrow. I do some scaling so that the * tips look proportional to the main line of the arrow. @@ -154,8 +157,9 @@ static void pdp_opencv_of_lk_process_rgb(t_pdp_opencv_of_lk *x) orig.x = (int) (dest.x - (6) * cos(angle - M_PI / 4)); orig.y = (int) (dest.y + (6) * sin(angle - M_PI / 4)); cvLine( x->image, orig, dest, CV_RGB(0,0,255), 1, CV_AA, 0 ); - meanx = (meanx*nbblocks+cvGet2D(x->x_velx, py, px).val[0])/(nbblocks+1); - meany = (meanx*nbblocks+cvGet2D(x->x_vely, py, px).val[0])/(nbblocks+1); + + globx = globx+cvGet2D(x->x_velx, py, px).val[0]; + globy = globy+cvGet2D(x->x_vely, py, px).val[0]; if ( hypotenuse > maxamp ) { maxamp = hypotenuse; @@ -168,26 +172,26 @@ static void pdp_opencv_of_lk_process_rgb(t_pdp_opencv_of_lk *x) } } - meanangle=-atan2( meany, meanx ); - // post( "pdp_opencv_of_lk : meanangle : %f", (meanangle*180)/M_PI ); - if ( nbblocks >= x->x_minblocks ) { + globangle=-atan2( globy, globx ); + // post( "pdp_opencv_of_lk : globangle : %f from vector (%f,%f)", (globangle*180)/M_PI, globx, globy ); + orig.x = (int) (x->x_width/2); orig.y = (int) (x->x_height/2); - dest.x = (int) (orig.x+((x->x_width>x->x_height)?x->x_height/2:x->x_width/2)*cos(meanangle)); - dest.y = (int) (orig.y-((x->x_width>x->x_height)?x->x_height/2:x->x_width/2)*sin(meanangle)); + dest.x = (int) (orig.x+((x->x_width>x->x_height)?x->x_height/2:x->x_width/2)*cos(globangle)); + dest.y = (int) (orig.y-((x->x_width>x->x_height)?x->x_height/2:x->x_width/2)*sin(globangle)); cvLine( x->image, orig, dest, CV_RGB(255,255,255), 3, CV_AA, 0 ); - orig.x = (int) (dest.x - (6) * cos(meanangle + M_PI / 4)); - orig.y = (int) (dest.y + (6) * sin(meanangle + M_PI / 4)); + orig.x = (int) (dest.x - (6) * cos(globangle + M_PI / 4)); + orig.y = (int) (dest.y + (6) * sin(globangle + M_PI / 4)); cvLine( x->image, orig, dest, CV_RGB(255,255,255), 3, CV_AA, 0 ); - orig.x = (int) (dest.x - (6) * cos(meanangle - M_PI / 4)); - orig.y = (int) (dest.y + (6) * sin(meanangle - M_PI / 4)); + orig.x = (int) (dest.x - (6) * cos(globangle - M_PI / 4)); + orig.y = (int) (dest.y + (6) * sin(globangle - M_PI / 4)); cvLine( x->image, orig, dest, CV_RGB(255,255,255), 3, CV_AA, 0 ); // outputs the average angle of movement - meanangle = (meanangle*180)/M_PI; - SETFLOAT(&x->x_list[0], meanangle); + globangle = (globangle*180)/M_PI; + SETFLOAT(&x->x_list[0], globangle); outlet_list( x->x_outlet1, 0, 1, x->x_list ); // outputs the amplitude and angle of the maximum movement @@ -220,8 +224,10 @@ static void pdp_opencv_of_lk_threshold(t_pdp_opencv_of_lk *x, t_floatarg f) static void pdp_opencv_of_lk_winsize(t_pdp_opencv_of_lk *x, t_floatarg fwidth, t_floatarg fheight ) { - if (fwidth>0.0) x->x_winsize.width = (double)fwidth; - if (fheight>0.0) x->x_winsize.height = (double)fheight; + if (fwidth==1.0 || fwidth==3.0 || fwidth==5.0 || fwidth==7.0 || fwidth==9.0 || fwidth==11.0 || fwidth==13.0 || fwidth==15.0 ) x->x_winsize.width = (double)fwidth; + else post( "pdp_opencv_of_lk : wrong winsize width : must be one of (1,3,5,7,9,11,13,15)" ); + if (fheight==1.0 || fheight==3.0 || fheight==5.0 || fheight==7.0 || fheight==9.0 || fheight==11.0 || fheight==13.0 || fheight==15.0 ) x->x_winsize.height = (double)fheight; + else post( "pdp_opencv_of_lk : wrong winsize height : must be one of (1,3,5,7,9,11,13,15)" ); } static void pdp_opencv_of_lk_sendpacket(t_pdp_opencv_of_lk *x) @@ -315,8 +321,8 @@ void *pdp_opencv_of_lk_new(t_floatarg f) x->x_minblocks = 10; x->x_velsize.width = x->x_width; x->x_velsize.height = x->x_height; - x->x_winsize.width = 10; - x->x_winsize.height = 10; + x->x_winsize.width = 9; + x->x_winsize.height = 9; // initialize font cvInitFont( &x->font, CV_FONT_HERSHEY_PLAIN, 1.0, 1.0, 0, 1, 8 ); -- cgit v1.2.1