New One Euro Filter Feature

emscripten/ARBindEM.cpp

@@ -8,6 +8,8 @@ EMSCRIPTEN_BINDINGS(constant_bindings) {
     register_vector<int>("IntList");
 	register_vector<nftMarker>("nftMarkers");
 
+	value_array<std::array<std::array<int, 3>, 4>>("ArrayOfArray");
+
 	function("setup", &setup);
 	function("teardown", &teardown);
 
@@ -21,6 +23,9 @@ EMSCRIPTEN_BINDINGS(constant_bindings) {
 	function("detectNFTMarker", &detectNFTMarker);
 	function("getNFTMarker", &getNFTMarkerInfo);
 	function("getNFTData", &getNFTData);
+	function("setOEF", &setOEF);
+	function("filterOEF", &filterOEF);
+	function("filterMatOEF", &filterMatOEF, allow_raw_pointers());
 
 	/* nft marker struct */
 	value_object<nftMarker>("nftMarker")

emscripten/ARToolKitJS.cpp

@@ -14,13 +14,15 @@
 #include <emscripten.h>
 #include <string>
 #include <vector>
+#include <time.h> 
 #include <unordered_map>
 #include <AR/config.h>
 #include <AR2/tracking.h>
 #include <AR/arFilterTransMat.h>
 #include <AR/paramGL.h>
 #include <KPM/kpm.h>
 #include <WebARKit/WebARKitLog.h>
+#include <WebARKit/WebARKitOEF.h>
 #include "trackingMod.h"
 
 const int PAGES_MAX = 20;         // Maximum number of pages expected. You can change this down (to save memory) or up (to accomodate more pages.)
@@ -73,6 +75,7 @@ struct arController {
 
 	ARdouble cameraLens[16];
 	AR_PIXEL_FORMAT pixFormat = AR_PIXEL_FORMAT_RGBA;
+	OEF::OneEuroFilter f{120.0, 1.0, 1.0, 1.0};
 };
 
 std::unordered_map<int, arController> arControllers;
@@ -108,6 +111,18 @@ extern "C" {
 		}
 	}
 
+	double tick() {
+		double t = .000001;
+		while(t < 1.0) {
+			t += .000001;
+			if(t == .999999) {
+				t = .000001;
+			}
+		}
+		return t;
+	}
+
+
 	int getNFTMarkerInfo(int id, int markerIndex) {
 		if (arControllers.find(id) == arControllers.end()) { return ARCONTROLLER_NOT_FOUND; }
 		arController *arc = &(arControllers[id]);
@@ -150,6 +165,24 @@ extern "C" {
 			matrixLerp(transF, transFLerp, 0.95);
 			#endif
 
+		    ARdouble transOEF[3][4];
+
+			for (int j = 0; j < 3; j++) {
+				for (int k = 0; k < 4; k++) {
+					transOEF[j][k] = trans[j][k];
+				}
+			}
+
+			if(arc->f.filterMat2(transOEF, tick()) < 0) {
+				webarkitLOGe("Error with filterMat2!");
+			};
+
+			for (int j = 0; j < 3; j++) {
+				for (int k = 0; k < 4; k++) {
+					trans[j][k] = transOEF[j][k];
+				}
+			}	
+
 			if( trackResult < 0 ) {
 				webarkitLOGi("Tracking lost. %d", trackResult);
 				arc->detectedPage = -2;
@@ -336,6 +369,32 @@ extern "C" {
 		return arc->nftMarkers.at(index);
 	}
 
+	/****************
+	 * Set OEF filter
+     ****************/
+	int setOEF(int id, double freq, double mincutoff, double beta, double dcutoff) {
+		if (arControllers.find(id) == arControllers.end()) { return -1; }
+		arController *arc = &(arControllers[id]);
+		arc->f = {freq, mincutoff, beta, dcutoff};
+		return 0;
+	}
+
+	int filterOEF(int id, double value, double timestamp) {
+       if (arControllers.find(id) == arControllers.end()) { return -1; }
+		arController *arc = &(arControllers[id]);
+		arc->f.filter(value, timestamp);
+		return 0;
+	}
+
+	int filterMatOEF(int id, ARdouble mat[3][4], double timestamp) {
+       if (arControllers.find(id) == arControllers.end()) { return -1; }
+		arController *arc = &(arControllers[id]);
+		arc->f.filterMat(mat, timestamp);
+		return 0;
+	}
+
+
+
 	/***************
 	 * Set Log Level
 	 ****************/
@@ -383,6 +442,8 @@ extern "C" {
 			arc->videoFrameSize = 0;
 		}
 
+		arc->f = NULL;
+
 		deleteHandle(arc);
 
 		arControllers.erase(id);

js/artoolkitNFT.worker.js

@@ -59,6 +59,7 @@ function load(msg) {
       var cameraMatrix = ar.getCameraMatrix();
 
       ar.addEventListener("getNFTMarker", function (ev) {
+        console.log(ev.data.matrixGL_RH);
         markerResult = {
           type: "found",
           matrixGL_RH: JSON.stringify(ev.data.matrixGL_RH),

js/artoolkitNFT_ES6.worker.js

@@ -54,6 +54,7 @@ function load(msg) {
 
   var onLoad = function (arController) {
     ar = arController;
+    arController.setOEF(120, 0.1, 0.1, 10);
     var cameraMatrix = ar.getCameraMatrix();
 
     ar.addEventListener("getNFTMarker", function (ev) {

src/ARControllerNFT.ts

@@ -107,6 +107,9 @@ interface delegateMethods {
   detectNFTMarker: (arId: number) => void;
   getNFTMarker: (id: number, markerIndex: number) => number;
   getNFTData: (id: number, index: number) => object;
+  setOEF: (id: number, frequency: number, mincutoff: number, beta: number, dcutoff: number) => number;
+  filterOEF: (id: number, value: number, timestamp: number) => number;
+  filterMatOEF: (id: number, matrix: Array<Array<number>>, timestamp: number) => Array<Array<number>>;
   setImageProcMode: (id: number, mode: number) => number;
   getImageProcMode: (id: number) => number;
 }
@@ -426,6 +429,24 @@ export default class ARControllerNFT {
     this._bwpointer = this.getProcessingImage();
   }
 
+  /**
+   * Set the OneEuroFilter constructor.
+   */
+  setOEF(frequency: number, mincutoff: number, beta: number, dcutoff: number ) {
+    this.artoolkitNFT.setOEF(this.id, frequency, mincutoff, beta, dcutoff);
+  }
+
+  /**
+   * Filter the signal value with a timestamp, thanks to OEF.
+   */
+  filterOEF(value: number, timestamp: number) {
+    this.artoolkitNFT.filterOEF(this.id, value, timestamp);
+  }
+
+  filterMatOEF(matrix: Array<Array<number>>, timestamp: number) {
+    return this.artoolkitNFT.filterMatOEF(this.id, matrix, timestamp);
+  }
+
   /**
    * Converts the given 3x4 marker transformation matrix in the 12-element transMat array
    * into a 4x4 WebGL matrix and writes the result into the 16-element glMat array.

src/ARToolkitNFT.ts

@@ -67,6 +67,9 @@ export default class ARToolkitNFT {
   public detectNFTMarker: (id: number) => number;
   public getNFTMarker: (id: number, markerIndex: number) => number;
   public getNFTData: (id: number, index: number) => object;
+  public setOEF: (id: number, frequency: number, mincutoff: number, beta: number, dcutoff: number) => number;
+  public filterOEF: (id: number, value: number, timestamp: number) => number;
+  public filterMatOEF: (id: number, matrix: Array<Array<number>>, timestamp: number) => Array<Array<number>>;
   public setLogLevel: (mode: boolean) => number;
   public getLogLevel: () => number;
   public frameMalloc: {
@@ -158,6 +161,9 @@ export default class ARToolkitNFT {
       "detectNFTMarker",
       "getNFTMarker",
       "getNFTData",
+      "setOEF",
+      "filterOEF",
+      "filterMatOEF",
 
       "frameMalloc",
       "NFTMarkerInfo",

src/ARToolkitNFT_simd.ts

@@ -67,6 +67,9 @@ export default class ARToolkitNFT {
   public detectNFTMarker: (id: number) => number;
   public getNFTMarker: (id: number, markerIndex: number) => number;
   public getNFTData: (id: number, index: number) => object;
+  public setOEF: (id: number, frequency: number, mincutoff: number, beta: number, dcutoff: number) => number;
+  public filterOEF: (id: number, value: number, timestamp: number) => number;
+  public filterMatOEF: (id: number, matrix: Array<Array<number>>, timestamp: number) => Array<Array<number>>;
   public setLogLevel: (mode: boolean) => number;
   public getLogLevel: () => number;
   public frameMalloc: {
@@ -158,6 +161,8 @@ export default class ARToolkitNFT {
       "detectNFTMarker",
       "getNFTMarker",
       "getNFTData",
+      "setOEF",
+      "filterOEF",
 
       "frameMalloc",
       "NFTMarkerInfo",

tools/makem.js

@@ -14,7 +14,7 @@ const platform = os.platform();
 
 var NO_LIBAR = false;
 /* Filtering remote jitter, but makes the tracking swim */
-var WITH_FILTERING = 1;
+var WITH_FILTERING = 0;
 
 var arguments = process.argv;
 

types/src/ARControllerNFT.d.ts

@@ -54,6 +54,9 @@ export default class ARControllerNFT {
         data?: object;
     }): void;
     debugSetup(): void;
+    setOEF(frequency: number, mincutoff: number, beta: number, dcutoff: number): void;
+    filterOEF(value: number, timestamp: number): void;
+    filterMatOEF(matrix: Array<Array<number>>, timestamp: number): number[][];
     transMatToGLMat(transMat: Float64Array, glMat: Float64Array, scale?: number): Float64Array;
     arglCameraViewRHf(glMatrix: Float64Array, glRhMatrix?: Float64Array, scale?: number): Float64Array;
     getTransformationMatrix(): Float64Array;

types/src/ARToolkitNFT.d.ts

@@ -18,6 +18,9 @@ export default class ARToolkitNFT {
     detectNFTMarker: (id: number) => number;
     getNFTMarker: (id: number, markerIndex: number) => number;
     getNFTData: (id: number, index: number) => object;
+    setOEF: (id: number, frequency: number, mincutoff: number, beta: number, dcutoff: number) => number;
+    filterOEF: (id: number, value: number, timestamp: number) => number;
+    filterMatOEF: (id: number, matrix: Array<Array<number>>, timestamp: number) => Array<Array<number>>;
     setLogLevel: (mode: boolean) => number;
     getLogLevel: () => number;
     frameMalloc: {

types/src/ARToolkitNFT_simd.d.ts

@@ -18,6 +18,9 @@ export default class ARToolkitNFT {
     detectNFTMarker: (id: number) => number;
     getNFTMarker: (id: number, markerIndex: number) => number;
     getNFTData: (id: number, index: number) => object;
+    setOEF: (id: number, frequency: number, mincutoff: number, beta: number, dcutoff: number) => number;
+    filterOEF: (id: number, value: number, timestamp: number) => number;
+    filterMatOEF: (id: number, matrix: Array<Array<number>>, timestamp: number) => Array<Array<number>>;
     setLogLevel: (mode: boolean) => number;
     getLogLevel: () => number;
     frameMalloc: {