From 42424d32f1d7e3de78853f7c2ae395e30b8baef6 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 7 Dec 2017 16:15:56 -0800 Subject: [PATCH] Added exponential smoothing filter Tuned coefficients to maximize glitch and vibration damping while minimizing latency. --- interface/resources/controllers/vive.json | 11 ++-- .../src/controllers/impl/Filter.cpp | 14 +++-- .../controllers/impl/RouteBuilderProxy.cpp | 6 ++ .../src/controllers/impl/RouteBuilderProxy.h | 1 + .../filters/ExponentialSmoothingFilter.cpp | 62 +++++++++++++++++++ .../impl/filters/ExponentialSmoothingFilter.h | 42 +++++++++++++ 6 files changed, 124 insertions(+), 12 deletions(-) create mode 100644 libraries/controllers/src/controllers/impl/filters/ExponentialSmoothingFilter.cpp create mode 100644 libraries/controllers/src/controllers/impl/filters/ExponentialSmoothingFilter.h diff --git a/interface/resources/controllers/vive.json b/interface/resources/controllers/vive.json index 02fc09c815..8a7744efb3 100644 --- a/interface/resources/controllers/vive.json +++ b/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" }, diff --git a/libraries/controllers/src/controllers/impl/Filter.cpp b/libraries/controllers/src/controllers/impl/Filter.cpp index b6c3ed4d27..6e6dc816d0 100644 --- a/libraries/controllers/src/controllers/impl/Filter.cpp +++ b/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(), diff --git a/libraries/controllers/src/controllers/impl/RouteBuilderProxy.cpp b/libraries/controllers/src/controllers/impl/RouteBuilderProxy.cpp index bc1ef55725..048e23be1c 100644 --- a/libraries/controllers/src/controllers/impl/RouteBuilderProxy.cpp +++ b/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(rotationConstant, translationConstant)); + return this; +} + QObject* RouteBuilderProxy::constrainToInteger() { addFilter(std::make_shared()); return this; diff --git a/libraries/controllers/src/controllers/impl/RouteBuilderProxy.h b/libraries/controllers/src/controllers/impl/RouteBuilderProxy.h index 75f3747566..92a87e5e39 100644 --- a/libraries/controllers/src/controllers/impl/RouteBuilderProxy.h +++ b/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: diff --git a/libraries/controllers/src/controllers/impl/filters/ExponentialSmoothingFilter.cpp b/libraries/controllers/src/controllers/impl/filters/ExponentialSmoothingFilter.cpp new file mode 100644 index 0000000000..355c45ced9 --- /dev/null +++ b/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 +#include +#include "../../UserInputMapper.h" +#include "../../Input.h" +#include + +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(); + 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; + } + +} diff --git a/libraries/controllers/src/controllers/impl/filters/ExponentialSmoothingFilter.h b/libraries/controllers/src/controllers/impl/filters/ExponentialSmoothingFilter.h new file mode 100644 index 0000000000..d2ce96deca --- /dev/null +++ b/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