[pose-detection]Improve one euro filter and low pass filter. (#632)

FEATURE

Author: lina128

pose-detection/src/calculators/interfaces/config_interfaces.ts

@@ -53,4 +53,11 @@ export interface OneEuroFilterConfig {
       number;  // Cutoff frequency for derivate. It is set to 1Hz in the
                // original algorithm, but can be turned to further smooth the
                // speed (i.e. derivate) on the object.
+  thresholdCutOff?:
+      number;  // The outlier threshold offset, will lead to a generally more
+               // reactive filter that will be less likely to discount outliers.
+  thresholdBeta?: number;  // The outlier threshold slope, will lead to a filter
+                           // that will more aggressively react whenever the
+                           // keypoint speed increases by being less likely to
+                           // consider that an observation is an outlier.
 }

pose-detection/src/calculators/low_pass_filter.ts

@@ -31,10 +31,26 @@ export class LowPassFilter {
 
   constructor(private alpha: number) {}
 
-  apply(value: number): number {
+  apply(value: number, threshold?: number): number {
     let result;
     if (this.initialized) {
-      result = this.alpha * value + (1 - this.alpha) * this.storedValue;
+      if (threshold == null) {
+        // Regular lowpass filter.
+        // result = this.alpha * value + (1 - this.alpha) * this.storedValue;
+        result = this.storedValue + this.alpha * (value - this.storedValue);
+      } else {
+        // We need to reformat the formula to be able to conveniently apply
+        // another optional non-linear function to the
+        // (value - this.storedValue) part.
+        // Add additional non-linearity to cap extreme value.
+        // More specifically, assume x = (value - this.storedValue), when x is
+        // close zero, the derived x is close to x, when x is several magnitudes
+        // larger, the drived x grows much slower then x. It behaves like
+        // sign(x)log(abs(x)).
+        result = this.storedValue +
+            this.alpha * threshold *
+                Math.asinh((value - this.storedValue) / threshold);
+      }
     } else {
       result = value;
       this.initialized = true;
@@ -45,9 +61,9 @@ export class LowPassFilter {
     return result;
   }
 
-  applyWithAlpha(value: number, alpha: number): number {
+  applyWithAlpha(value: number, alpha: number, threshold?: number): number {
     this.alpha = alpha;
-    return this.apply(value);
+    return this.apply(value, threshold);
   }
 
   hasLastRawValue(): boolean {

pose-detection/src/calculators/one_euro_filter.ts

@@ -22,6 +22,8 @@ import {LowPassFilter} from './low_pass_filter';
  */
 // ref:
 // https://github.com/google/mediapipe/blob/master/mediapipe/util/filtering/one_euro_filter.cc
+// Also ref original paper:
+// https://cristal.univ-lille.fr/~casiez/1euro/
 export class OneEuroFilter {
   private frequency: number;
   private minCutOff: number;
@@ -30,6 +32,8 @@ export class OneEuroFilter {
   private x: LowPassFilter;
   private dx: LowPassFilter;
   private lastTimestamp: number;
+  private thresholdCutOff: number;
+  private thresholdBeta: number;
   /**
    * Constructor of `OneEuroFilter` class.
    * @param config See documentation of `OneEuroFilterConfig`.
@@ -38,6 +42,8 @@ export class OneEuroFilter {
     this.frequency = config.frequency;
     this.minCutOff = config.minCutOff;
     this.beta = config.beta;
+    this.thresholdCutOff = config.thresholdCutOff;
+    this.thresholdBeta = config.thresholdBeta;
     this.derivateCutOff = config.derivateCutOff;
     this.x = new LowPassFilter(this.getAlpha(this.minCutOff));
     this.dx = new LowPassFilter(this.getAlpha(this.derivateCutOff));
@@ -77,8 +83,12 @@ export class OneEuroFilter {
         this.dx.applyWithAlpha(dValue, this.getAlpha(this.derivateCutOff));
     const cutOff = this.minCutOff + this.beta * Math.abs(edValue);
 
+    const threshold = this.thresholdCutOff != null ?
+        this.thresholdCutOff + this.thresholdBeta * Math.abs(edValue) :
+        null;
+
     // filter the given value.
-    return this.x.applyWithAlpha(value, this.getAlpha(cutOff));
+    return this.x.applyWithAlpha(value, this.getAlpha(cutOff), threshold);
   }
 
   private getAlpha(cutoff: number): number {