before alphaHEADsvn2git-headexternals/pidip

svn path=/trunk/externals/pidip/; revision=14570

1 files changed, 104 insertions, 5 deletions

diff --git a/modules/pdp_artkp.cpp b/modules/pdp_artkp.cpp
index 6331562..095bbd6 100755
--- a/modules/pdp_artkp.cpp
+++ b/modules/pdp_artkp.cpp
@@ -23,14 +23,26 @@
 #include <string.h>
 #include <dirent.h>
 
+#include "ARToolKitPlus/TrackerMultiMarkerImpl.h"
+
 #include "pdp.h"
 
+class PdLogger : public ARToolKitPlus::Logger
+{
+    void artLog(const char* msg)
+    {
+        post("pdp_artkp : %s", msg);
+    }
+};
+
 typedef struct pdp_artkp
 {
     t_object x_obj;
     t_float x_f;
 
     t_outlet *x_outlet0;
+    t_outlet *x_outlet1;
+    t_outlet *x_outlet2;
     int x_packet0;
     int x_packet1;
     int x_dropped;
@@ -40,17 +52,77 @@ typedef struct pdp_artkp
     int x_height;
     int x_size;
 
+    // ARToolKitPlus data
+    ARToolKitPlus::TrackerMultiMarker *x_tracker; /// the ARToolKitPlus marker tracker
+    PdLogger *x_logger; // tracker logger
+    int x_nummarkers;   // number of detected markers
+    t_atom x_mdata[3];   // marker data ( list )
+
 } t_pdp_artkp;
 
+static void pdp_artkp_init(t_pdp_artkp *x)
+{
+   if( (x->x_width>1024) || (x->x_height>1024) )
+   {
+     post( "pdp_artkp : warning : detection might not work well with resolution > 1024");
+   } 
+
+   x->x_tracker = new ARToolKitPlus::TrackerMultiMarkerImpl<6,6,6, 1, 16>(x->x_width, x->x_height);
+
+   if( !x->x_tracker )
+   {
+     post( "pdp_artkp : FATAL... could not allocate tracker");
+     exit(-1);
+   }
+
+   x->x_logger = new PdLogger;
+   if( !x->x_logger )
+   {
+     post( "pdp_artkp : FATAL... could not allocate logger");
+     exit(-1);
+   }
+   else
+   {
+     x->x_tracker->setLogger(x->x_logger);
+   }
+
+   // setting pixel format
+   x->x_tracker->setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_RGB);
+
+   if(!x->x_tracker->init("/usr/share/ARToolKitPlus/camera.dat", "/usr/share/ARToolKitPlus/markerboard_480-499.cfg", 1.0f, 1000.0f))
+   {
+        post( "pdp_artkp : FATAL... ARToolKitPlus init() failed.");
+        post( "pdp_artkp : did you install artkp data files in /usr/share/ARToolKitPlus ?");
+        exit(-1);
+   } 
+
+   // the marker in the BCH test image has a thiner border...
+   x->x_tracker->setBorderWidth(0.125f);
+
+   // activate automatic thresholding
+   x->x_tracker->activateAutoThreshold(true);
 
+   // let's use lookup-table undistortion for high-speed
+   // note: LUT only works with images up to 1024x1024
+   x->x_tracker->setUndistortionMode(ARToolKitPlus::UNDIST_LUT);
+
+   // RPP is more robust than ARToolKit's standard pose estimator
+   x->x_tracker->setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_RPP);
+
+   // switch to simple ID based markers
+   // use the tool in tools/IdPatGen to generate markers
+   x->x_tracker->setMarkerMode(ARToolKitPlus::MARKER_ID_SIMPLE);
+
+}
 
 static void pdp_artkp_process_rgb(t_pdp_artkp *x)
 {
-  t_pdp     *header = pdp_packet_header(x->x_packet0);
-  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;
+  t_pdp         *header = pdp_packet_header(x->x_packet0);
+  unsigned char *data   = (unsigned char *)pdp_packet_data(x->x_packet0);
+  t_pdp         *newheader = pdp_packet_header(x->x_packet1);
+  unsigned char *newdata = (unsigned char *)pdp_packet_data(x->x_packet1); 
+  int im;
+  int numDetected;
 
     if ((x->x_width != (t_int)header->info.image.width) || 
         (x->x_height != (t_int)header->info.image.height)) 
@@ -61,13 +133,34 @@ static void pdp_artkp_process_rgb(t_pdp_artkp *x)
       x->x_width = header->info.image.width;
       x->x_height = header->info.image.height;
       x->x_size = x->x_width*x->x_height;
+
+      pdp_artkp_init(x);
     }
     
     newheader->info.image.encoding = header->info.image.encoding;
     newheader->info.image.width = x->x_width;
     newheader->info.image.height = x->x_height;
 
+    numDetected = x->x_tracker->calc((unsigned char*)data);
+    if ( numDetected != x->x_nummarkers )
+    {
+       x->x_nummarkers = numDetected;
+       outlet_float( x->x_outlet2, x->x_nummarkers );
+    }
+
+    for (im=0; im<numDetected; im++) 
+    {
+       ARToolKitPlus::ARMarkerInfo marker = x->x_tracker->getDetectedMarker(im);
+
+       SETFLOAT(&x->x_mdata[0], marker.id);
+       SETFLOAT(&x->x_mdata[1], marker.pos[0]);
+       SETFLOAT(&x->x_mdata[2], marker.pos[1]);
+       outlet_list( x->x_outlet1, 0, 3, x->x_mdata );
+
+    }
+
     memcpy( newdata, data, x->x_size*3 );
+
     return;
 }
 
@@ -140,6 +233,8 @@ void *pdp_artkp_new(t_floatarg f)
     t_pdp_artkp *x = (t_pdp_artkp *)pd_new(pdp_artkp_class);
 
     x->x_outlet0 = outlet_new(&x->x_obj, &s_anything); 
+    x->x_outlet1 = outlet_new(&x->x_obj, &s_anything); 
+    x->x_outlet2 = outlet_new(&x->x_obj, &s_anything); 
 
     x->x_packet0 = -1;
     x->x_packet1 = -1;
@@ -149,6 +244,10 @@ void *pdp_artkp_new(t_floatarg f)
     x->x_height = 240;
     x->x_size   = x->x_width * x->x_height;
 
+    // init the ARToolKitPlus tracker
+    pdp_artkp_init(x);
+    x->x_nummarkers = 0;
+    
     return (void *)x;
 }