try to find the closest point

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

1 files changed, 81 insertions, 61 deletions

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;