add ar2Tracking to fixing-nft

emscripten/ARToolKitJS.cpp

@@ -13,6 +13,7 @@
 #include <AR/paramGL.h>
 #include <AR/video.h>
 #include <KPM/kpm.h>
+#include "trackingMod.h"
 
 #define PAGES_MAX               10          // Maximum number of pages expected. You can change this down (to save memory) or up (to accomodate more pages.)
 
@@ -42,6 +43,8 @@ struct arController {
 	KpmHandle* kpmHandle;
 	AR2HandleT* ar2Handle;
 
+	int detectedPage = -2;  // -2 Tracking not inited, -1 tracking inited OK, >= 0 tracking online on page.
+
 	int surfaceSetCount = 0; // Running NFT marker id
 	AR2SurfaceSetT      *surfaceSet[PAGES_MAX];
 	std::unordered_map<int, AR2SurfaceSetT*> surfaceSets;
@@ -93,28 +96,47 @@ extern "C" {
 		KpmResult *kpmResult = NULL;
 		int kpmResultNum = -1;
 
-        kpmGetResult( arc->kpmHandle, &kpmResult, &kpmResultNum );
-
-		int i, j, k;
-        int flag = -1;
-        float err = -1;
-        float trans[3][4];
-        for( i = 0; i < kpmResultNum; i++ ) {
-            if (kpmResult[i].pageNo == markerIndex && kpmResult[i].camPoseF == 0 ) {
-	            if( flag == -1 || err > kpmResult[i].error ) { // Take the first or best result.
-	                flag = i;
-	                err = kpmResult[i].error;
-	            }
-	        }
-        }
+		float trans[3][4];
+		float err = -1;
+		if (arc->detectedPage == -2) {
+			kpmMatching( arc->kpmHandle, arc->videoLuma );
+			kpmGetResult( arc->kpmHandle, &kpmResult, &kpmResultNum );
+			int i, j, k;
+			int flag = -1;
+			for( i = 0; i < kpmResultNum; i++ ) {
+				if (kpmResult[i].pageNo == markerIndex && kpmResult[i].camPoseF == 0 ) {
+					if( flag == -1 || err > kpmResult[i].error ) { // Take the first or best result.
+						flag = i;
+						err = kpmResult[i].error;
+					}
+				}
+			}
+
+			if (flag > -1) {
+				arc->detectedPage = kpmResult[0].pageNo;
+
+				for (j = 0; j < 3; j++) {
+					for (k = 0; k < 4; k++) {
+						trans[j][k] = kpmResult[flag].camPose[j][k];
+					}
+				}
+				ar2SetInitTrans(arc->surfaceSet[arc->detectedPage], trans);
+			} else {
+				arc->detectedPage = -2;
+			}
+		}
 
-        if (flag > -1) {
-            for (j = 0; j < 3; j++) {
-            	for (k = 0; k < 4; k++) {
-            		trans[j][k] = kpmResult[flag].camPose[j][k];
-            	}
-            }
+		if (arc->detectedPage >= 0) {
+			int trackResult = ar2TrackingMod(arc->ar2Handle, arc->surfaceSet[arc->detectedPage], arc->videoFrame, trans, &err);
+			if( trackResult < 0 ) {
+				ARLOGi("Tracking lost. %d\n", trackResult);
+				arc->detectedPage = -2;
+			} else {
+				ARLOGi("Tracked page %d (max %d).\n",arc->surfaceSet[arc->detectedPage], arc->surfaceSetCount - 1);
+			}
+		}
 
+		if (arc->detectedPage >= 0) {
 			EM_ASM_({
 				var $a = arguments;
 				var i = 0;
@@ -203,10 +225,7 @@ extern "C" {
 
 		KpmResult *kpmResult = NULL;
 		int kpmResultNum = -1;
-
-        kpmMatching( arc->kpmHandle, arc->videoLuma );
-        kpmGetResult( arc->kpmHandle, &kpmResult, &kpmResultNum );
-        return kpmResultNum;
+		return kpmResultNum;
 	}
 
 	KpmHandle *createKpmHandle(ARParamLT *cparamLT) {
@@ -232,6 +251,18 @@ extern "C" {
 		arController *arc = &(arControllers[id]);
 		//arc->pixFormat = arVideoGetPixelFormat();
 
+		if ((arc->ar2Handle = ar2CreateHandleMod(arc->paramLT, arc->pixFormat)) == NULL) {
+			ARLOGe("Error: ar2CreateHandle.\n");
+			kpmDeleteHandle(&arc->kpmHandle);
+		}
+		// Settings for devices with single-core CPUs.
+		ar2SetTrackingThresh(arc->ar2Handle, 5.0);
+		ar2SetSimThresh(arc->ar2Handle, 0.50);
+		ar2SetSearchFeatureNum(arc->ar2Handle, 16);
+		ar2SetSearchSize(arc->ar2Handle, 6);
+		ar2SetTemplateSize1(arc->ar2Handle, 6);
+		ar2SetTemplateSize2(arc->ar2Handle, 6);
+
 		arc->kpmHandle = createKpmHandle(arc->paramLT);
 
 		return 0;

emscripten/tracking2d.c

@@ -0,0 +1,215 @@
+/*
+ *  AR2/tracking2d.c
+ *  ARToolKit5
+ *
+ *  This file is part of ARToolKit.
+ *
+ *  ARToolKit is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU Lesser General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  ARToolKit is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public License
+ *  along with ARToolKit.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ *  As a special exception, the copyright holders of this library give you
+ *  permission to link this library with independent modules to produce an
+ *  executable, regardless of the license terms of these independent modules, and to
+ *  copy and distribute the resulting executable under terms of your choice,
+ *  provided that you also meet, for each linked independent module, the terms and
+ *  conditions of the license of that module. An independent module is a module
+ *  which is neither derived from nor based on this library. If you modify this
+ *  library, you may extend this exception to your version of the library, but you
+ *  are not obligated to do so. If you do not wish to do so, delete this exception
+ *  statement from your version.
+ *
+ *  Copyright 2015 Daqri, LLC.
+ *  Copyright 2006-2015 ARToolworks, Inc.
+ *
+ *  Author(s): Hirokazu Kato, Philip Lamb
+ *
+ */
+
+#include <AR/ar.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <AR2/config.h>
+#include <AR2/featureSet.h>
+#include <AR2/template.h>
+#include <AR2/searchPoint.h>
+#include <AR2/tracking.h>
+
+#if AR2_CAPABLE_ADAPTIVE_TEMPLATE
+int ar2Tracking2dSub ( AR2HandleT *handle, AR2SurfaceSetT *surfaceSet, AR2TemplateCandidateT *candidate,
+                              ARUint8 *dataPtr, ARUint8 *mfImage, AR2TemplateT **templ,
+                              AR2Template2T **templ2, AR2Tracking2DResultT *result );
+#else
+int ar2Tracking2dSub ( AR2HandleT *handle, AR2SurfaceSetT *surfaceSet, AR2TemplateCandidateT *candidate,
+                              ARUint8 *dataPtr, ARUint8 *mfImage, AR2TemplateT **templ,
+                              AR2Tracking2DResultT *result );
+#endif
+
+#if AR2_CAPABLE_ADAPTIVE_TEMPLATE
+int ar2Tracking2dSub ( AR2HandleT *handle, AR2SurfaceSetT *surfaceSet, AR2TemplateCandidateT *candidate,
+                              ARUint8 *dataPtr, ARUint8 *mfImage, AR2TemplateT **templ,
+                              AR2Template2T **templ2, AR2Tracking2DResultT *result )
+#else
+int ar2Tracking2dSub ( AR2HandleT *handle, AR2SurfaceSetT *surfaceSet, AR2TemplateCandidateT *candidate,
+                              ARUint8 *dataPtr, ARUint8 *mfImage, AR2TemplateT **templ,
+                              AR2Tracking2DResultT *result )
+#endif
+{
+#if AR2_CAPABLE_ADAPTIVE_TEMPLATE
+    AR2Template2T        *templ2;
+#endif
+    int                   snum, level, fnum;
+    int                   search[3][2];
+    int                   bx, by;
+
+    snum  = candidate->snum;
+    level = candidate->level;
+    fnum  = candidate->num;
+
+    if( *templ == NULL )  *templ = ar2GenTemplate( handle->templateSize1, handle->templateSize2 );
+#if AR2_CAPABLE_ADAPTIVE_TEMPLATE
+    if( *templ2 == NULL ) *templ2 = ar2GenTemplate2( handle->templateSize1, handle->templateSize2 );
+#endif
+
+#if AR2_CAPABLE_ADAPTIVE_TEMPLATE
+    if( handle->blurMethod == AR2_CONSTANT_BLUR ) {
+        if( ar2SetTemplateSub( handle->cparamLT,
+                               (const float (*)[4])handle->wtrans1[snum],
+                               surfaceSet->surface[snum].imageSet,
+                             &(surfaceSet->surface[snum].featureSet->list[level]),
+                               fnum,
+                               handle->blurLevel,
+                              *templ ) < 0 ) return -1;
+
+        if( (*templ)->vlen * (*templ)->vlen
+              < ((*templ)->xts1+(*templ)->xts2+1) * ((*templ)->yts1+(*templ)->yts2+1)
+               * AR2_DEFAULT_TRACKING_SD_THRESH * AR2_DEFAULT_TRACKING_SD_THRESH ) {
+            return -1;
+        }
+    }
+    else {
+        if( ar2SetTemplate2Sub( handle->cparamLT,
+                                (const float (*)[4])handle->wtrans1[snum],
+                                surfaceSet->surface[snum].imageSet,
+                              &(surfaceSet->surface[snum].featureSet->list[level]),
+                                fnum,
+                                handle->blurLevel,
+                               *templ2 ) < 0 ) return -1;
+
+        if( (*templ2)->vlen[1] * (*templ2)->vlen[1]
+              < ((*templ2)->xts1+(*templ2)->xts2+1) * ((*templ2)->yts1+(*templ2)->yts2+1)
+               * AR2_DEFAULT_TRACKING_SD_THRESH * AR2_DEFAULT_TRACKING_SD_THRESH ) {
+            return -1;
+        }
+    }
+#else
+    if( ar2SetTemplateSub( handle->cparamLT,
+                           (const float (*)[4])handle->wtrans1[snum],
+                           surfaceSet->surface[snum].imageSet,
+                         &(surfaceSet->surface[snum].featureSet->list[level]),
+                           fnum,
+                          *templ ) < 0 ) return -1;
+
+    if( (*templ)->vlen * (*templ)->vlen
+          < ((*templ)->xts1 + (*templ)->xts2 + 1) * ((*templ)->yts1 + (*templ)->yts2 + 1)
+           * AR2_DEFAULT_TRACKING_SD_THRESH * AR2_DEFAULT_TRACKING_SD_THRESH ) {
+        return -1;
+    }
+#endif
+
+    // Get the screen coordinates for up to three previous positions of this feature into search[][].
+    if( surfaceSet->contNum == 1 ) {
+        ar2GetSearchPoint( handle->cparamLT,
+                           (const float (*)[4])handle->wtrans1[snum], NULL, NULL,
+                         &(surfaceSet->surface[snum].featureSet->list[level].coord[fnum]),
+                           search );
+    }
+    else if( surfaceSet->contNum == 2 ) {
+        ar2GetSearchPoint( handle->cparamLT,
+                           (const float (*)[4])handle->wtrans1[snum],
+                           (const float (*)[4])handle->wtrans2[snum], NULL,
+                         &(surfaceSet->surface[snum].featureSet->list[level].coord[fnum]),
+                           search );
+    }
+    else {
+        ar2GetSearchPoint( handle->cparamLT,
+                           (const float (*)[4])handle->wtrans1[snum],
+                           (const float (*)[4])handle->wtrans2[snum],
+                           (const float (*)[4])handle->wtrans3[snum],
+                         &(surfaceSet->surface[snum].featureSet->list[level].coord[fnum]),
+                           search );
+    }
+
+#if AR2_CAPABLE_ADAPTIVE_TEMPLATE
+    if( handle->blurMethod == AR2_CONSTANT_BLUR ) {
+        if( ar2GetBestMatching( dataPtr,
+                                mfImage,
+                                handle->xsize,
+                                handle->ysize,
+                                handle->pixFormat,
+                               *templ,
+                                handle->searchSize,
+                                handle->searchSize,
+                                search,
+                                &bx, &by,
+                              &(result->sim)) < 0 ) {
+            return -1;
+        }
+        result->blurLevel = handle->blurLevel;
+    }
+    else {
+        if( ar2GetBestMatching2( dataPtr,
+                                 mfImage,
+                                 handle->xsize,
+                                 handle->ysize,
+                                 handle->pixFormat,
+                                *templ2,
+                                 handle->searchSize,
+                                 handle->searchSize,
+                                 search,
+                                 &bx, &by,
+                               &(result->sim),
+                               &(result->blurLevel)) < 0 ) {
+            return -1;
+        }
+    }
+#else
+    if( ar2GetBestMatching( dataPtr,
+                            mfImage,
+                            handle->xsize,
+                            handle->ysize,
+                            handle->pixFormat,
+                           *templ,
+                            handle->searchSize,
+                            handle->searchSize,
+                            search,
+                            &bx, &by,
+                          &(result->sim)) < 0 ) {
+        return -1;
+    }
+#endif
+
+    result->pos2d[0] = (float)bx;
+    result->pos2d[1] = (float)by;
+    result->pos3d[0] = surfaceSet->surface[snum].trans[0][0] * surfaceSet->surface[snum].featureSet->list[level].coord[fnum].mx
+                     + surfaceSet->surface[snum].trans[0][1] * surfaceSet->surface[snum].featureSet->list[level].coord[fnum].my
+                     + surfaceSet->surface[snum].trans[0][3];
+    result->pos3d[1] = surfaceSet->surface[snum].trans[1][0] * surfaceSet->surface[snum].featureSet->list[level].coord[fnum].mx
+                     + surfaceSet->surface[snum].trans[1][1] * surfaceSet->surface[snum].featureSet->list[level].coord[fnum].my
+                     + surfaceSet->surface[snum].trans[1][3];
+    result->pos3d[2] = surfaceSet->surface[snum].trans[2][0] * surfaceSet->surface[snum].featureSet->list[level].coord[fnum].mx
+                     + surfaceSet->surface[snum].trans[2][1] * surfaceSet->surface[snum].featureSet->list[level].coord[fnum].my
+                     + surfaceSet->surface[snum].trans[2][3];
+
+    return 0;
+}