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_bm.cc | 38 ++++++++++++++++++++---------------- pdp_opencv_of_hs.cc | 37 +++++++++++++++++++---------------- pdp_opencv_of_lk-help.pd | 36 ++++++++++++++++++---------------- pdp_opencv_of_lk.cc | 50 +++++++++++++++++++++++++++--------------------- 4 files changed, 90 insertions(+), 71 deletions(-) diff --git a/pdp_opencv_of_bm.cc b/pdp_opencv_of_bm.cc index 3774617..700bb9b 100755 --- a/pdp_opencv_of_bm.cc +++ b/pdp_opencv_of_bm.cc @@ -74,7 +74,7 @@ static void pdp_opencv_of_bm_process_rgb(t_pdp_opencv_of_bm *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_bm_process_rgb(t_pdp_opencv_of_bm *x) x->x_velx, x->x_vely ); nbblocks = 0; + globangle = 0; + globx = 0; + globy = 0; for( py=0; pyx_velsize.height; py++ ) { for( px=0; pxx_velsize.width; px++ ) @@ -138,8 +141,8 @@ static void pdp_opencv_of_bm_process_rgb(t_pdp_opencv_of_bm *x) orig.y = (py*x->x_height)/x->x_velsize.height; 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) ); + 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_bm_process_rgb(t_pdp_opencv_of_bm *x) orig.x = (int) (dest.x - (x->x_shiftsize.width/4) * cos(angle - M_PI / 4)); orig.y = (int) (dest.y + (x->x_shiftsize.height/4) * sin(angle - M_PI / 4)); cvLine( x->image, orig, dest, CV_RGB(0,0,255), (int)hypotenuse/10, 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,28 @@ static void pdp_opencv_of_bm_process_rgb(t_pdp_opencv_of_bm *x) } } - meanangle=-atan2( meany, meanx ); - // post( "pdp_opencv_of_bm : meanangle : %f", (meanangle*180)/M_PI ); - if ( nbblocks >= x->x_minblocks ) { + + globangle=-atan2( globy, globx ); + // post( "pdp_opencv_of_bm : globangle : %f", (globangle*180)/M_PI ); + 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 - (x->x_shiftsize.width/2) * cos(meanangle + M_PI / 4)); - orig.y = (int) (dest.y + (x->x_shiftsize.height/2) * sin(meanangle + M_PI / 4)); + orig.x = (int) (dest.x - (x->x_shiftsize.width/2) * cos(globangle + M_PI / 4)); + orig.y = (int) (dest.y + (x->x_shiftsize.height/2) * sin(globangle + M_PI / 4)); cvLine( x->image, orig, dest, CV_RGB(255,255,255), 3, CV_AA, 0 ); - orig.x = (int) (dest.x - (x->x_shiftsize.width/2) * cos(meanangle - M_PI / 4)); - orig.y = (int) (dest.y + (x->x_shiftsize.height/2) * sin(meanangle - M_PI / 4)); + orig.x = (int) (dest.x - (x->x_shiftsize.width/2) * cos(globangle - M_PI / 4)); + orig.y = (int) (dest.y + (x->x_shiftsize.height/2) * 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 diff --git a/pdp_opencv_of_hs.cc b/pdp_opencv_of_hs.cc index 5eeb005..95fa338 100755 --- a/pdp_opencv_of_hs.cc +++ b/pdp_opencv_of_hs.cc @@ -77,7 +77,7 @@ static void pdp_opencv_of_hs_process_rgb(t_pdp_opencv_of_hs *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; @@ -132,6 +132,9 @@ static void pdp_opencv_of_hs_process_rgb(t_pdp_opencv_of_hs *x) cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03) ); nbblocks = 0; + globangle = 0; + globx = 0; + globy = 0; for( py=0; pyx_velsize.height; py++ ) { for( px=0; pxx_velsize.width; px++ ) @@ -141,8 +144,8 @@ static void pdp_opencv_of_hs_process_rgb(t_pdp_opencv_of_hs *x) 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) ); + 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. @@ -157,8 +160,9 @@ static void pdp_opencv_of_hs_process_rgb(t_pdp_opencv_of_hs *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; @@ -171,26 +175,27 @@ static void pdp_opencv_of_hs_process_rgb(t_pdp_opencv_of_hs *x) } } - meanangle=-atan2( meany, meanx ); - // post( "pdp_opencv_of_hs : meanangle : %f", (meanangle*180)/M_PI ); - if ( nbblocks >= x->x_minblocks ) { + globangle=-atan2( globy, globx ); + // post( "pdp_opencv_of_hs : globangle : %f", (globangle*180)/M_PI ); + 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 diff --git a/pdp_opencv_of_lk-help.pd b/pdp_opencv_of_lk-help.pd index dc83e46..9a030c2 100755 --- a/pdp_opencv_of_lk-help.pd +++ b/pdp_opencv_of_lk-help.pd @@ -1,4 +1,4 @@ -#N canvas 587 49 833 785 10; +#N canvas 45 49 833 785 10; #X obj -11 57 cnv 15 621 250 empty empty empty 20 12 0 14 -260097 -66577 0; #X obj 229 97 tgl 15 0 empty empty empty 17 7 0 10 -262144 -1 -1 0 @@ -7,7 +7,7 @@ #X msg 310 146 close; #X obj 249 222 pdp_v4l; #X msg 306 119 open /dev/video0; -#X obj 423 95 tgl 15 0 empty empty empty 17 7 0 10 -262144 -1 -1 0 +#X obj 423 95 tgl 15 0 empty empty empty 17 7 0 10 -262144 -1 -1 1 1; #X obj 423 122 metro 40; #X msg 500 145 close; @@ -37,7 +37,7 @@ #X obj 165 483 tgl 15 0 empty empty empty 17 7 0 10 -262144 -1 -1 0 1; #X msg 80 481 nightmode \$1; -#X floatatom 150 403 5 0 0 0 - - -; +#X floatatom 153 389 5 0 0 0 - - -; #X floatatom 154 428 5 0 0 0 - - -; #X msg 67 426 threshold \$1; #X text -10 0 with a great help from Mateu Batle & Alberto de Rodrigo @@ -69,16 +69,18 @@ a very powerful machine; #X text 127 314 you can rotate the object to check that the main direction of movement is detected; #X text -10 24 comments to ydegoyon@gmail.com; -#X obj 19 534 pdp_opencv_of_hs; #X text 198 463 ( default 10 ); -#X msg 56 401 winsize \$1 \$2; -#X text 193 400 set window size ( width height ) [ default : 10x10 -]; +#X msg 59 387 winsize \$1 \$2; #X obj 421 253 pdp_xv; #X text 195 424 threshold value for the detection of movement ( default 100 ); #X text -11 -52 pdp_opencv_of_lk : Lukas Kanade optical flow algorithm. ; +#X text 197 384 set window size ( width height ) [ default : 9x9 ] +; +#X text 199 398 window width and height must be one of (1 \, 3 \, 5 +\, 7 \, 9 \, 11 \, 13 \, 15); +#X obj 18 535 pdp_opencv_of_lk; #X connect 1 0 2 0; #X connect 2 0 4 0; #X connect 3 0 4 0; @@ -89,7 +91,7 @@ of movement is detected; #X connect 8 0 10 0; #X connect 9 0 10 0; #X connect 10 0 53 0; -#X connect 10 0 61 0; +#X connect 10 0 59 0; #X connect 11 0 10 0; #X connect 12 0 11 0; #X connect 13 0 53 0; @@ -105,20 +107,20 @@ of movement is detected; #X connect 22 0 15 0; #X connect 25 0 13 1; #X connect 28 0 29 0; -#X connect 29 0 57 0; -#X connect 30 0 59 0; +#X connect 29 0 64 0; +#X connect 30 0 58 0; #X connect 31 0 32 0; -#X connect 32 0 57 0; +#X connect 32 0 64 0; #X connect 35 0 36 0; #X connect 35 1 37 0; -#X connect 39 0 57 0; +#X connect 39 0 64 0; #X connect 40 0 39 0; #X connect 44 0 13 0; -#X connect 46 0 57 0; +#X connect 46 0 64 0; #X connect 48 0 10 0; #X connect 53 0 46 0; #X connect 54 0 53 1; -#X connect 57 0 27 0; -#X connect 57 1 34 0; -#X connect 57 2 35 0; -#X connect 59 0 57 0; +#X connect 58 0 64 0; +#X connect 64 0 27 0; +#X connect 64 1 34 0; +#X connect 64 2 35 0; 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