Added exponential smoothing filter

Tuned coefficients to maximize glitch and vibration damping while minimizing latency.

interface/resources/controllers/vive.json

@@ -56,29 +56,28 @@
 
         {
             "from": "Vive.LeftFoot", "to" : "Standard.LeftFoot",
-            "filters" : [{"type" :  "lowVelocity", "rotation" : 1.0, "translation": 1.0}]
+            "filters" : [{"type" :  "exponentialSmoothing", "rotation" : 0.15, "translation": 0.3}]
         },
 
         {
             "from": "Vive.RightFoot", "to" : "Standard.RightFoot",
-            "filters" : [{"type" :  "lowVelocity", "rotation" : 1.0, "translation": 1.0}]
+            "filters" : [{"type" :  "exponentialSmoothing", "rotation" : 0.15, "translation": 0.3}]
         },
 
         {
             "from": "Vive.Hips", "to" : "Standard.Hips",
-            "filters" : [{"type" :  "lowVelocity", "rotation" : 0.01, "translation": 0.01}]
+            "filters" : [{"type" :  "exponentialSmoothing", "rotation" : 0.15, "translation": 0.3}]
         },
 
         {
             "from": "Vive.Spine2", "to" : "Standard.Spine2",
-            "filters" : [{"type" :  "lowVelocity", "rotation" : 0.01, "translation": 0.01}]
+            "filters" : [{"type" :  "exponentialSmoothing", "rotation" : 0.15, "translation": 0.3}]
         },
 
         { "from": "Vive.Head", "to" : "Standard.Head"},
-
         { "from": "Vive.RightArm", "to" : "Standard.RightArm" },
         { "from": "Vive.LeftArm", "to" : "Standard.LeftArm" },
-        
+
         { "from": "Vive.TrackedObject00", "to" : "Standard.TrackedObject00" },
         { "from": "Vive.TrackedObject01", "to" : "Standard.TrackedObject01" },
         { "from": "Vive.TrackedObject02", "to" : "Standard.TrackedObject02" },

libraries/controllers/src/controllers/impl/Filter.cpp

@@ -30,6 +30,7 @@
 #include "filters/PostTransformFilter.h"
 #include "filters/RotateFilter.h"
 #include "filters/LowVelocityFilter.h"
+#include "filters/ExponentialSmoothingFilter.h"
 
 using namespace controller;
 
@@ -49,6 +50,7 @@ REGISTER_FILTER_CLASS_INSTANCE(TransformFilter, "transform")
 REGISTER_FILTER_CLASS_INSTANCE(PostTransformFilter, "postTransform")
 REGISTER_FILTER_CLASS_INSTANCE(RotateFilter, "rotate")
 REGISTER_FILTER_CLASS_INSTANCE(LowVelocityFilter, "lowVelocity")
+REGISTER_FILTER_CLASS_INSTANCE(ExponentialSmoothingFilter, "exponentialSmoothing")
 
 const QString JSON_FILTER_TYPE = QStringLiteral("type");
 const QString JSON_FILTER_PARAMS = QStringLiteral("params");
@@ -93,7 +95,7 @@ bool Filter::parseSingleFloatParameter(const QJsonValue& parameters, const QStri
             output = objectParameters[name].toDouble();
             return true;
         }
-    } 
+    }
     return false;
 }
 
@@ -117,7 +119,7 @@ bool Filter::parseVec3Parameter(const QJsonValue& parameters, glm::vec3& output)
                                objectParameters["z"].toDouble());
             return true;
         }
-    } 
+    }
     return false;
 }
 
@@ -126,7 +128,7 @@ bool Filter::parseMat4Parameter(const QJsonValue& parameters, glm::mat4& output)
         auto objectParameters = parameters.toObject();
 
 
-        if (objectParameters.contains("r0c0") && 
+        if (objectParameters.contains("r0c0") &&
             objectParameters.contains("r1c0") &&
             objectParameters.contains("r2c0") &&
             objectParameters.contains("r3c0") &&
@@ -169,9 +171,9 @@ bool Filter::parseMat4Parameter(const QJsonValue& parameters, glm::mat4& output)
 bool Filter::parseQuatParameter(const QJsonValue& parameters, glm::quat& output) {
     if (parameters.isObject()) {
         auto objectParameters = parameters.toObject();
-        if (objectParameters.contains("w") && 
-            objectParameters.contains("x") && 
-            objectParameters.contains("y") && 
+        if (objectParameters.contains("w") &&
+            objectParameters.contains("x") &&
+            objectParameters.contains("y") &&
             objectParameters.contains("z")) {
 
             output = glm::quat(objectParameters["w"].toDouble(),

libraries/controllers/src/controllers/impl/RouteBuilderProxy.cpp

@@ -32,6 +32,7 @@
 #include "filters/PostTransformFilter.h"
 #include "filters/RotateFilter.h"
 #include "filters/LowVelocityFilter.h"
+#include "filters/ExponentialSmoothingFilter.h"
 #include "conditionals/AndConditional.h"
 
 using namespace controller;
@@ -134,6 +135,11 @@ QObject* RouteBuilderProxy::lowVelocity(float rotationConstant, float translatio
     return this;
 }
 
+QObject* RouteBuilderProxy::exponentialSmoothing(float rotationConstant, float translationConstant) {
+    addFilter(std::make_shared<ExponentialSmoothingFilter>(rotationConstant, translationConstant));
+    return this;
+}
+
 QObject* RouteBuilderProxy::constrainToInteger() {
     addFilter(std::make_shared<ConstrainToIntegerFilter>());
     return this;

libraries/controllers/src/controllers/impl/RouteBuilderProxy.h

@@ -53,6 +53,7 @@ class RouteBuilderProxy : public QObject {
         Q_INVOKABLE QObject* postTransform(glm::mat4 transform);
         Q_INVOKABLE QObject* rotate(glm::quat rotation);
         Q_INVOKABLE QObject* lowVelocity(float rotationConstant, float translationConstant);
+        Q_INVOKABLE QObject* exponentialSmoothing(float rotationConstant, float translationConstant);
         Q_INVOKABLE QObject* logicalNot();
 
     private:

libraries/controllers/src/controllers/impl/filters/ExponentialSmoothingFilter.cpp

@@ -0,0 +1,62 @@
+//
+//  Created by Anthony Thibault 2017/12/07
+//  Copyright 2017 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+
+#include "ExponentialSmoothingFilter.h"
+
+#include <QtCore/QJsonObject>
+#include <QtCore/QJsonArray>
+#include "../../UserInputMapper.h"
+#include "../../Input.h"
+#include <DependencyManager.h>
+
+static const QString JSON_ROTATION = QStringLiteral("rotation");
+static const QString JSON_TRANSLATION = QStringLiteral("translation");
+namespace controller {
+
+    Pose ExponentialSmoothingFilter::apply(Pose value) const {
+
+        // to perform filtering in sensor space, we need to compute the transformations.
+        auto userInputMapper = DependencyManager::get<UserInputMapper>();
+        const InputCalibrationData calibrationData = userInputMapper->getInputCalibrationData();
+        glm::mat4 sensorToAvatarMat = glm::inverse(calibrationData.avatarMat) * calibrationData.sensorToWorldMat;
+        glm::mat4 avatarToSensorMat = glm::inverse(calibrationData.sensorToWorldMat) * calibrationData.avatarMat;
+
+        // transform pose into sensor space.
+        Pose sensorValue = value.transform(avatarToSensorMat);
+
+        if (value.isValid() && _oldSensorValue.isValid()) {
+
+            // exponential smoothing filter
+            sensorValue.translation = _translationConstant * sensorValue.getTranslation() + (1.0f - _translationConstant) * _oldSensorValue.getTranslation();
+            sensorValue.rotation = safeMix(sensorValue.getRotation(), _oldSensorValue.getRotation(), _rotationConstant);
+
+            _oldSensorValue = sensorValue;
+
+            // transform back into avatar space
+            return sensorValue.transform(sensorToAvatarMat);
+        } else {
+            _oldSensorValue = sensorValue;
+            return value;
+        }
+    }
+
+    bool ExponentialSmoothingFilter::parseParameters(const QJsonValue& parameters) {
+
+        if (parameters.isObject()) {
+            auto obj = parameters.toObject();
+            if (obj.contains(JSON_ROTATION) && obj.contains(JSON_TRANSLATION)) {
+                _rotationConstant = obj[JSON_ROTATION].toDouble();
+                _translationConstant = obj[JSON_TRANSLATION].toDouble();
+                return true;
+            }
+        }
+        return false;
+    }
+
+}

libraries/controllers/src/controllers/impl/filters/ExponentialSmoothingFilter.h

@@ -0,0 +1,42 @@
+//
+//  Created by Anthony Thibault 2017/12/17
+//  Copyright 2017 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#ifndef hifi_Controllers_Filters_Exponential_Smoothing_h
+#define hifi_Controllers_Filters_Exponential_Smoothing_h
+
+#include "../Filter.h"
+
+namespace controller {
+
+    class ExponentialSmoothingFilter : public Filter {
+        REGISTER_FILTER_CLASS(ExponentialSmoothingFilter);
+
+    public:
+        ExponentialSmoothingFilter() {}
+        ExponentialSmoothingFilter(float rotationConstant, float translationConstant) :
+            _translationConstant(translationConstant), _rotationConstant(rotationConstant) {}
+
+        float apply(float value) const override { return value; }
+        Pose apply(Pose value) const override;
+        bool parseParameters(const QJsonValue& parameters) override;
+
+    private:
+
+        // Constant between 0 and 1.
+        // 1 indicates no smoothing at all, poses are passed through unaltered.
+        // Values near 1 are less smooth with lower latency.
+        // Values near 0 are more smooth with higher latency.
+        float _translationConstant { 0.375f };
+        float _rotationConstant { 0.375f };
+
+        mutable Pose _oldSensorValue { Pose() };  // sensor space
+    };
+
+}
+
+#endif