try to find the closest point

svn path=/trunk/externals/pdp_opencv/; revision=13763

4 files changed, 141 insertions, 106 deletions

diff --git a/pdp_opencv_haarcascade.cc b/pdp_opencv_haarcascade.cc
index cafc2fb..f4c9a21 100755
--- a/pdp_opencv_haarcascade.cc
+++ b/pdp_opencv_haarcascade.cc
@@ -91,7 +91,7 @@ static int pdp_opencv_haarcascade_mark(t_pdp_opencv_haarcascade *x, t_floatarg f
           x->x_xmark[i] = (int)fx;
           x->x_ymark[i] = (int)fy;
           x->x_found[i] = x->x_ftolerance;
-          post( "pdp_opencv_haarcascade : inserted point %d (%d,%d)", i, x->x_xmark[i], x->x_ymark[i] );
+          // post( "pdp_opencv_haarcascade : inserted point %d (%d,%d)", i, x->x_xmark[i], x->x_ymark[i] );
           return i;
        }
     }
@@ -236,7 +236,7 @@ static void pdp_opencv_haarcascade_process_rgb(t_pdp_opencv_haarcascade *x)
              x->x_xmark[im] = -1.0;
              x->x_ymark[im] = -1,0;
              x->x_found[im] = x->x_ftolerance;
-             post( "deleted point %d", im );
+             // post( "deleted point %d", im );
            }
         }
     }
diff --git a/pdp_opencv_lk-help.pd b/pdp_opencv_lk-help.pd
index f7c2ea2..89038be 100644
--- a/pdp_opencv_lk-help.pd
+++ b/pdp_opencv_lk-help.pd
@@ -1,4 +1,4 @@
-#N canvas 493 49 833 785 10;
+#N canvas 554 56 833 785 10;
 #X text -16 -86 written by Lluis Gomez i Bigorda ( lluisgomez@hangar.org
 ) and Yves Degoyon ( ydegoyon@gmail.com );
 #X obj -17 -55 cnv 15 621 250 empty empty empty 20 12 0 14 -260097
@@ -9,7 +9,7 @@
 #X msg 313 39 close;
 #X obj 243 110 pdp_v4l;
 #X msg 300 7 open /dev/video0;
-#X obj 412 -20 tgl 15 0 empty empty empty 17 7 0 10 -262144 -1 -1 1
+#X obj 412 -20 tgl 15 0 empty empty empty 17 7 0 10 -262144 -1 -1 0
 1;
 #X obj 412 7 metro 40;
 #X msg 493 26 close;
@@ -66,8 +66,6 @@
 #X text 357 264 set quality ( default 100 );
 #X msg 265 263 100;
 #X obj 298 263 loadbang;
-#X text 180 368 mark %x %y : mark a point to track ( max points : 10
-);
 #X obj 117 588 unpack f f;
 #X floatatom 116 606 5 0 0 0 - - -;
 #X floatatom 174 607 5 0 0 0 - - -;
@@ -86,7 +84,6 @@ window;
 ;
 #X obj 171 391 hradio 15 1 0 8 empty empty empty 0 -8 0 10 -262144
 -1 -1 0;
-#X msg 137 527 pdelaunay 1 50;
 #X text -14 -111 pdp_opencv_lk : Lukas-Kanade corner points tracking
 ;
 #X text -14 -99 Corner points detection based on Shi and Tomasi;
@@ -98,9 +95,12 @@ window;
 #X text 167 346 reset all markers;
 #X msg 118 458 ftolerance \$1;
 #X floatatom 210 458 5 0 0 0 - - -;
-#X text 232 437 max movement of a marker ( default 8 pisels );
 #X text 250 458 frame tolerance for point identification ( default
 : 5 );
+#X text 179 368 mark %x %y : mark a point to track ( max points : 500
+);
+#X msg 137 527 pdelaunay 0 50;
+#X text 232 437 max movement of a marker ( default 10 pisels );
 #X connect 2 0 3 0;
 #X connect 3 0 5 0;
 #X connect 4 0 5 0;
@@ -132,7 +132,7 @@ window;
 #X connect 29 0 30 1;
 #X connect 30 0 28 0;
 #X connect 30 0 46 0;
-#X connect 30 1 64 0;
+#X connect 30 1 63 0;
 #X connect 32 0 33 0;
 #X connect 33 0 30 0;
 #X connect 35 0 30 0;
@@ -143,7 +143,7 @@ window;
 #X connect 42 0 41 0;
 #X connect 43 0 45 0;
 #X connect 43 0 44 0;
-#X connect 43 0 65 0;
+#X connect 43 0 64 0;
 #X connect 44 0 40 0;
 #X connect 45 0 36 0;
 #X connect 46 0 28 0;
@@ -155,15 +155,15 @@ window;
 #X connect 54 0 53 0;
 #X connect 56 0 43 0;
 #X connect 57 0 56 0;
-#X connect 59 0 60 0;
-#X connect 59 1 61 0;
-#X connect 64 0 59 0;
-#X connect 65 0 42 1;
+#X connect 58 0 59 0;
+#X connect 58 1 60 0;
+#X connect 63 0 58 0;
+#X connect 64 0 42 1;
+#X connect 68 0 30 0;
 #X connect 69 0 30 0;
-#X connect 70 0 30 0;
-#X connect 72 0 49 0;
-#X connect 73 0 30 0;
-#X connect 78 0 30 0;
-#X connect 79 0 30 0;
-#X connect 82 0 30 0;
-#X connect 83 0 82 0;
+#X connect 71 0 49 0;
+#X connect 76 0 30 0;
+#X connect 77 0 30 0;
+#X connect 80 0 30 0;
+#X connect 81 0 80 0;
+#X connect 84 0 30 0;
diff --git a/pdp_opencv_lk.cc b/pdp_opencv_lk.cc
index 432c4e7..e47d95d 100755
--- a/pdp_opencv_lk.cc
+++ b/pdp_opencv_lk.cc
@@ -93,8 +93,9 @@ static void pdp_opencv_lk_process_rgb(t_pdp_opencv_lk *x)
     short int *data   = (short int *)pdp_packet_data(x->x_packet0);
     t_pdp     *newheader = pdp_packet_header(x->x_packet1);
     short int *newdata = (short int *)pdp_packet_data(x->x_packet1); 
-    int i,j,k,im;
+    int i,j,k,im,oi;
     int marked;
+    float dist, odist;
 
     if ((x->x_width != (t_int)header->info.image.width) || 
         (x->x_height != (t_int)header->info.image.height) || (!x->image)) 
@@ -209,7 +210,7 @@ static void pdp_opencv_lk_process_rgb(t_pdp_opencv_lk *x)
                cvCalcSubdivVoronoi2D( x->x_subdiv );
             }
             // only add points included in (color-threshold)<p<(color+treshold)
-            if ( ( x->x_delaunay > 0 ) && ( x->x_xmark[x->x_delaunay-1] != -1 ) ) 
+            if ( ( x->x_delaunay > 0 ) && ( x->x_xmark[x->x_delaunay] != -1 ) ) 
             {
                int px = cvPointFrom32f(x->points[1][i]).x;
                int py = cvPointFrom32f(x->points[1][i]).y;
@@ -227,9 +228,9 @@ static void pdp_opencv_lk_process_rgb(t_pdp_opencv_lk *x)
                    uchar green = ((uchar*)(x->oimage->imageData + x->oimage->widthStep*ppx))[ppy*3+1];
                    uchar blue = ((uchar*)(x->oimage->imageData + x->oimage->widthStep*ppx))[ppy*3+2];
 
-                   uchar pred = ((uchar*)(x->oimage->imageData + x->oimage->widthStep*x->x_xmark[x->x_delaunay-1]))[x->x_ymark[x->x_delaunay-1]*3];
-                   uchar pgreen = ((uchar*)(x->oimage->imageData + x->oimage->widthStep*x->x_xmark[x->x_delaunay-1]))[x->x_ymark[x->x_delaunay-1]*3+1];
-                   uchar pblue = ((uchar*)(x->oimage->imageData + x->oimage->widthStep*x->x_xmark[x->x_delaunay-1]))[x->x_ymark[x->x_delaunay-1]*3+2];
+                   uchar pred = ((uchar*)(x->oimage->imageData + x->oimage->widthStep*x->x_xmark[x->x_delaunay]))[x->x_ymark[x->x_delaunay]*3];
+                   uchar pgreen = ((uchar*)(x->oimage->imageData + x->oimage->widthStep*x->x_xmark[x->x_delaunay]))[x->x_ymark[x->x_delaunay]*3+1];
+                   uchar pblue = ((uchar*)(x->oimage->imageData + x->oimage->widthStep*x->x_xmark[x->x_delaunay]))[x->x_ymark[x->x_delaunay]*3+2];
 
                    int diff = abs(red-pred) + abs(green-pgreen) + abs(blue-pblue);
 
@@ -247,28 +248,42 @@ static void pdp_opencv_lk_process_rgb(t_pdp_opencv_lk *x)
             cvCircle( x->image, cvPointFrom32f(x->points[1][i]), 3, CV_RGB(0,255,0), -1, 8,0);
 
             marked=0;
+            oi=-1;
+            dist=(x->x_width>x->x_height)?x->x_width:x->x_height;
+
             for ( im=0; im<MAX_MARKERS; im++ )
             {
+
+              if ( x->x_xmark[im] == -1 ) continue; // i don't see the point
+
+              odist=sqrt( pow( x->points[1][i].x - x->x_xmark[im], 2 ) + pow( x->points[1][i].y - x->x_ymark[im], 2 ) );
+
               // search for this point
-              if ( x->x_xmark[im] != -1.0 )
+              if ( odist <= x->x_maxmove )
               {
-               if ( ( abs( x->points[1][i].x - x->x_xmark[im] ) <= x->x_maxmove ) && ( abs( x->points[1][i].y - x->x_ymark[im] ) <= x->x_maxmove ) )
-               {
-                  char tindex[4];
-                  sprintf( tindex, "%d", im+1 );
-                  cvPutText( x->image, tindex, cvPointFrom32f(x->points[1][i]), &x->font, CV_RGB(255,255,255));
-                  x->x_xmark[im]=x->points[1][i].x;
-                  x->x_ymark[im]=x->points[1][i].y;
-                  x->x_found[im]=x->x_ftolerance;
-                  marked=1;
-                  SETFLOAT(&x->x_list[0], im+1);
-                  SETFLOAT(&x->x_list[1], x->x_xmark[im]);
-                  SETFLOAT(&x->x_list[2], x->x_ymark[im]);
-                  outlet_list( x->x_outlet1, 0, 3, x->x_list );
-               }
+                  if ( odist < dist )
+                  {
+                    dist = odist;
+                    marked=1;
+                    oi=im;
+                  }
               }
             }
 
+            if ( oi !=-1 )
+            {      
+               char tindex[4];
+               sprintf( tindex, "%d", oi );
+               cvPutText( x->image, tindex, cvPointFrom32f(x->points[1][i]), &x->font, CV_RGB(255,255,255));
+               x->x_xmark[oi]=x->points[1][i].x;
+               x->x_ymark[oi]=x->points[1][i].y;
+               x->x_found[oi]=x->x_ftolerance;
+               SETFLOAT(&x->x_list[0], oi);
+               SETFLOAT(&x->x_list[1], x->x_xmark[oi]);
+               SETFLOAT(&x->x_list[2], x->x_ymark[oi]);
+               outlet_list( x->x_outlet1, 0, 3, x->x_list );
+            }
+
             if ( x->x_markall && !marked )
             {
               for ( im=0; im<MAX_MARKERS; im++)
@@ -515,8 +530,8 @@ static void pdp_opencv_lk_delete(t_pdp_opencv_lk *x, t_floatarg findex )
        return;
     }
 
-    x->x_xmark[(int)findex-1] = -1;
-    x->x_ymark[(int)findex-1] = -1;
+    x->x_xmark[(int)findex] = -1;
+    x->x_ymark[(int)findex] = -1;
 }
 
 static void pdp_opencv_lk_clear(t_pdp_opencv_lk *x )
@@ -637,8 +652,8 @@ void *pdp_opencv_lk_new(t_floatarg f)
   x->add_remove_pt = 0;
   x->quality = 0.1;
   x->min_distance = 10;
-  x->x_maxmove = 8;
-  x->x_ftolerance = 8;
+  x->x_maxmove = 10;
+  x->x_ftolerance = 5;
   x->x_delaunay = -1;
   x->x_threshold = -1;
 
diff --git a/pdp_opencv_surf.cc b/pdp_opencv_surf.cc
index 99c7642..1614d4c 100644
--- a/pdp_opencv_surf.cc
+++ b/pdp_opencv_surf.cc
@@ -108,6 +108,8 @@ static void pdp_opencv_surf_process_rgb(t_pdp_opencv_surf *x)
     int i,j,k,im;
     char tindex[4];
     int descsize;
+    int oi;
+    float dist, odist;
 
     if ((x->x_width != (t_int)header->info.image.width) || 
         (x->x_height != (t_int)header->info.image.height) || (!x->image)) 
@@ -184,7 +186,7 @@ static void pdp_opencv_surf_process_rgb(t_pdp_opencv_surf *x)
        }
 
        // only add points included in (color-threshold)<p<(color+treshold)
-       if ( ( x->x_delaunay > 0 ) && ( x->x_xmark[x->x_delaunay-1] != -1 ) ) 
+       if ( ( x->x_delaunay > 0 ) && ( x->x_xmark[x->x_delaunay] != -1 ) ) 
        {
          int px = cvPointFrom32f(r1->pt).x;
          int py = cvPointFrom32f(r1->pt).y;
@@ -202,9 +204,9 @@ static void pdp_opencv_surf_process_rgb(t_pdp_opencv_surf *x)
              uchar green = ((uchar*)(x->oimage->imageData + x->oimage->widthStep*ppx))[ppy*3+1];
              uchar blue = ((uchar*)(x->oimage->imageData + x->oimage->widthStep*ppx))[ppy*3+2];
 
-             uchar pred = ((uchar*)(x->oimage->imageData + x->oimage->widthStep*x->x_xmark[x->x_delaunay-1]))[x->x_ymark[x->x_delaunay-1]*3];
-             uchar pgreen = ((uchar*)(x->oimage->imageData + x->oimage->widthStep*x->x_xmark[x->x_delaunay-1]))[x->x_ymark[x->x_delaunay-1]*3+1];
-             uchar pblue = ((uchar*)(x->oimage->imageData + x->oimage->widthStep*x->x_xmark[x->x_delaunay-1]))[x->x_ymark[x->x_delaunay-1]*3+2];
+             uchar pred = ((uchar*)(x->oimage->imageData + x->oimage->widthStep*x->x_xmark[x->x_delaunay]))[x->x_ymark[x->x_delaunay]*3];
+             uchar pgreen = ((uchar*)(x->oimage->imageData + x->oimage->widthStep*x->x_xmark[x->x_delaunay]))[x->x_ymark[x->x_delaunay]*3+1];
+             uchar pblue = ((uchar*)(x->oimage->imageData + x->oimage->widthStep*x->x_xmark[x->x_delaunay]))[x->x_ymark[x->x_delaunay]*3+2];
 
              int diff = abs(red-pred) + abs(green-pgreen) + abs(blue-pblue);
 
@@ -221,6 +223,9 @@ static void pdp_opencv_surf_process_rgb(t_pdp_opencv_surf *x)
 
        cvCircle( x->image, cvPointFrom32f(r1->pt), 3, CV_RGB(0,255,0), -1, 8,0);
 
+       oi=-1;
+       dist=(x->x_width>x->x_height)?x->x_width:x->x_height;
+
        // mark the point if it is not already
        if ( x->x_markall )
        {
@@ -228,16 +233,18 @@ static void pdp_opencv_surf_process_rgb(t_pdp_opencv_surf *x)
 
          for ( im=0; im<MAX_MARKERS; im++ )
          {
-           if ( x->x_xmark[im] != -1.0 )
+
+           if ( x->x_xmark[im] == -1 ) continue; // no points
+
+           odist=sqrt( pow( r1->pt.x-x->x_xmark[im], 2 ) + pow( r1->pt.y-x->x_ymark[im], 2 ) );
+
+           if ( odist <= x->x_maxmove )
            {
-             if ( ( abs( r1->pt.x - x->x_xmark[im] ) <= x->x_maxmove ) && ( abs( r1->pt.y - x->x_ymark[im] ) <= x->x_maxmove ) )
-             {
-               marked = 1;
-               // post( "pdp_opencv_surf : point already marked" );
-               break;
-             }
+             marked = 1;
+             break;
            }
          }
+
          if ( !marked ) 
          {
             for ( i=0; i<MAX_MARKERS; i++)
@@ -260,6 +267,11 @@ static void pdp_opencv_surf_process_rgb(t_pdp_opencv_surf *x)
       int neighbour = -1;
       double d, dist1 = 1000000, dist2 = 1000000;
 
+      oi=-1;
+      dist=(x->x_width>x->x_height)?x->x_width:x->x_height;
+
+      if ( x->x_xmark[im] == -1 ) continue; // no points
+
       for( i = 0; i < x->objectKeypoints->total; i++ )
       {
         CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem( x->objectKeypoints, i );
@@ -269,58 +281,66 @@ static void pdp_opencv_surf_process_rgb(t_pdp_opencv_surf *x)
          // manually marked points
          // recognized on position
          // if ( ( x->x_xmark[im] != -1.0 ) && ( x->x_rdesc[im][0] == 0.0 ) )
-         if ( x->x_xmark[im] != -1.0 )
+
+         odist=sqrt( pow( r1->pt.x-x->x_xmark[im], 2 ) + pow( r1->pt.y-x->x_ymark[im], 2 ) );
+         
+         if ( odist <= x->x_maxmove )
          {
-           if ( ( abs( r1->pt.x - x->x_xmark[im] ) <= x->x_maxmove ) && ( abs( r1->pt.y - x->x_ymark[im] ) <= x->x_maxmove ) )
-           {
-             sprintf( tindex, "%d", im+1 );
-             cvPutText( x->image, tindex, cvPointFrom32f(r1->pt), &x->font, CV_RGB(255,255,255));
-             x->x_xmark[im]=r1->pt.x;
-             x->x_ymark[im]=r1->pt.y;
-             memcpy( (float * )x->x_rdesc[im], rdesc, descsize*sizeof(float));
-             x->x_found[im]++;
-             SETFLOAT(&x->x_list[0], im+1);
-             SETFLOAT(&x->x_list[1], x->x_xmark[im]);
-             SETFLOAT(&x->x_list[2], x->x_ymark[im]);
-             outlet_list( x->x_outlet1, 0, 3, x->x_list );
-             break;
-           }
+            if ( odist < dist )
+            {
+              oi=im;
+              x->x_xmark[oi]=r1->pt.x;
+              x->x_ymark[oi]=r1->pt.y;
+              memcpy( (float * )x->x_rdesc[oi], rdesc, descsize*sizeof(float));
+              dist = odist;
+            }
          }
-
-         // recognize points according to their SURF descriptor ( size = 128 ) 
-         // this code is desactivated because it isn't more stable than the positions
-         // if ( ( x->x_xmark[im] != -1.0 ) && ( x->x_rdesc[im][0] != 0.0 ) )
-         // {
-         //   d = pdp_opencv_surf_compare_descriptors( x->x_rdesc[im], rdesc, descsize );
-         //   if( d < dist1 )
-         //   {
-         //     dist1 = d;
-         //     neighbour = i;
-         //     // post( "pdp_opencv_surf : point %d, min distance : %d ( with %d )", i, (int)d, im );
-         //   }
-         // }
        }
 
-       // check if we found the point
-       // if ( dist1 < x->x_criteria )
-       // {
-       //   CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem( x->objectKeypoints, neighbour );
-       //   const float* rdesc = (const float*)cvGetSeqElem( x->objectDescriptors, neighbour );
-
-       //   // point identified 
-       //   sprintf( tindex, "%d", im+1 );
-       //   cvPutText( x->image, tindex, cvPointFrom32f(r1->pt), &x->font, CV_RGB(255,255,255));
-       //   x->x_xmark[im]=r1->pt.x;
-       //   x->x_ymark[im]=r1->pt.y;
-       //   memcpy( (float * )x->x_rdesc[im], rdesc, descsize*sizeof(float));
-       //   x->x_found[im]=1;
-       //   SETFLOAT(&x->x_list[0], im+1);
-       //   SETFLOAT(&x->x_list[1], x->x_xmark[im]);
-       //   SETFLOAT(&x->x_list[2], x->x_ymark[im]);
-       //   outlet_list( x->x_outlet1, 0, 3, x->x_list );
-       // }
+       if ( oi !=-1 )
+       {
+          sprintf( tindex, "%d", oi );
+          cvPutText( x->image, tindex, cvPoint(x->x_xmark[oi],x->x_ymark[oi]), &x->font, CV_RGB(255,255,255));
+          x->x_found[oi] = x->x_ftolerance;
+          SETFLOAT(&x->x_list[0], oi);
+          SETFLOAT(&x->x_list[1], x->x_xmark[oi]);
+          SETFLOAT(&x->x_list[2], x->x_ymark[oi]);
+          outlet_list( x->x_outlet1, 0, 3, x->x_list );
+        }
+
+        // recognize points according to their SURF descriptor ( size = 128 ) 
+        // this code is desactivated because it isn't more stable than the positions
+        // if ( ( x->x_xmark[im] != -1.0 ) && ( x->x_rdesc[im][0] != 0.0 ) )
+        // {
+        //   d = pdp_opencv_surf_compare_descriptors( x->x_rdesc[im], rdesc, descsize );
+        //   if( d < dist1 )
+        //   {
+        //     dist1 = d;
+        //     neighbour = i;
+        //     // post( "pdp_opencv_surf : point %d, min distance : %d ( with %d )", i, (int)d, im );
+        //   }
+        // }
     }
 
+    // check if we found the point
+    // if ( dist1 < x->x_criteria )
+    // {
+    //   CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem( x->objectKeypoints, neighbour );
+    //   const float* rdesc = (const float*)cvGetSeqElem( x->objectDescriptors, neighbour );
+
+    //   // point identified 
+    //   sprintf( tindex, "%d", im+1 );
+    //   cvPutText( x->image, tindex, cvPointFrom32f(r1->pt), &x->font, CV_RGB(255,255,255));
+    //   x->x_xmark[im]=r1->pt.x;
+    //   x->x_ymark[im]=r1->pt.y;
+    //   memcpy( (float * )x->x_rdesc[im], rdesc, descsize*sizeof(float));
+    //   x->x_found[im]=1;
+    //   SETFLOAT(&x->x_list[0], im+1);
+    //   SETFLOAT(&x->x_list[1], x->x_xmark[im]);
+    //   SETFLOAT(&x->x_list[2], x->x_ymark[im]);
+    //   outlet_list( x->x_outlet1, 0, 3, x->x_list );
+    // }
+
     // draw the delaunay
     if ( x->x_delaunay >= 0 )
     {
@@ -387,7 +407,7 @@ static void pdp_opencv_surf_process_rgb(t_pdp_opencv_surf *x)
     // suppress lost points
     for ( im=0; im<MAX_MARKERS; im++ )
     {
-        if ( (x->x_xmark[im] != -1.0 ) && !x->x_found[im] )
+        if ( (x->x_xmark[im] != -1.0 ) && (x->x_found[im]<=0) )
         {
            x->x_xmark[im]=-1.0;
            x->x_ymark[im]=-1.0;
@@ -525,8 +545,8 @@ static void pdp_opencv_surf_delete(t_pdp_opencv_surf *x, t_floatarg findex )
        return;
     }
 
-    x->x_xmark[(int)findex-1] = -1;
-    x->x_ymark[(int)findex-1] = -1;
+    x->x_xmark[(int)findex] = -1;
+    x->x_ymark[(int)findex] = -1;
 }
 
 static void pdp_opencv_surf_clear(t_pdp_opencv_surf *x )
@@ -630,7 +650,7 @@ void *pdp_opencv_surf_new(t_floatarg f)
   x->x_size   = x->x_width * x->x_height;
 
   x->night_mode = 0;
-  x->x_maxmove = 8;
+  x->x_maxmove = 10;
   x->x_delaunay = -1;
   x->x_threshold = -1;