From f14a321d035ae778992d826f5b9b6c1d15b657f9 Mon Sep 17 00:00:00 2001 From: samcake Date: Fri, 23 Oct 2015 15:03:04 -0700 Subject: [PATCH 1/5] Adding a amoving Average for the velocity of the hydra --- .../src/input-plugins/SixenseManager.cpp | 21 +++++++++++-- .../src/input-plugins/SixenseManager.h | 31 +++++++++++++++++++ 2 files changed, 49 insertions(+), 3 deletions(-) diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp index 29e60ca5ec..1f72d66019 100644 --- a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp +++ b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp @@ -178,6 +178,7 @@ void SixenseManager::update(float deltaTime, bool jointsCaptured) { if (sixenseGetNumActiveControllers() == 0) { _poseStateMap.clear(); + _collectedSamples.clear(); return; } @@ -234,9 +235,12 @@ void SixenseManager::update(float deltaTime, bool jointsCaptured) { } else { _poseStateMap.clear(); + _collectedSamples.clear(); } } else { - _poseStateMap[left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND] = controller::Pose(); + auto hand = left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND; + _poseStateMap[hand] = controller::Pose(); + _collectedSamples[hand].clear(); } } @@ -387,6 +391,8 @@ void SixenseManager::handleButtonEvent(unsigned int buttons, bool left) { void SixenseManager::handlePoseEvent(float deltaTime, glm::vec3 position, glm::quat rotation, bool left) { #ifdef HAVE_SIXENSE + auto hand = left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND; + // From ABOVE the sixense coordinate frame looks like this: // // | @@ -401,7 +407,7 @@ void SixenseManager::handlePoseEvent(float deltaTime, glm::vec3 position, glm::q // | // | // z - auto prevPose = _poseStateMap[left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND]; + auto prevPose = _poseStateMap[hand]; // Transform the measured position into body frame. position = _avatarRotation * (position + _avatarPosition); @@ -448,7 +454,10 @@ void SixenseManager::handlePoseEvent(float deltaTime, glm::vec3 position, glm::q glm::vec3 velocity(0.0f); glm::quat angularVelocity; + + if (prevPose.isValid() && deltaTime > std::numeric_limits::epsilon()) { + velocity = (position - prevPose.getTranslation()) / deltaTime; auto deltaRot = rotation * glm::conjugate(prevPose.getRotation()); @@ -456,9 +465,15 @@ void SixenseManager::handlePoseEvent(float deltaTime, glm::vec3 position, glm::q auto angle = glm::angle(deltaRot); angularVelocity = glm::angleAxis(angle / deltaTime, axis); + // Average + auto& samples = _collectedSamples[hand]; + samples.addSample(velocity); + velocity = samples.average; + } else if (!prevPose.isValid()) { + _collectedSamples[hand].clear(); } - _poseStateMap[left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND] = controller::Pose(position, rotation, velocity, angularVelocity); + _poseStateMap[hand] = controller::Pose(position, rotation, velocity, angularVelocity); #endif // HAVE_SIXENSE } diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.h b/libraries/input-plugins/src/input-plugins/SixenseManager.h index 368321e669..c0d908ed45 100644 --- a/libraries/input-plugins/src/input-plugins/SixenseManager.h +++ b/libraries/input-plugins/src/input-plugins/SixenseManager.h @@ -94,6 +94,36 @@ private: glm::vec3 _reachRight; float _lastDistance; bool _useSixenseFilter = true; + + template class MovingAverage { + public: + using Samples = std::list< T >; + Samples samples; + T average; + + void clear() { + samples.clear(); + } + + bool isAverageValid() const { return !samples.empty(); } + + void addSample(T sample) { + samples.push_front(sample); + int numSamples = samples.size(); + + if (numSamples < MAX_NUM_SAMPLES) { + average = (sample + average * float(numSamples - 1)) / float(numSamples); + } else { + T tail = samples.back(); + samples.pop_back(); + average = average + (sample - tail) / float(numSamples); + } + } + }; + + static const int MAX_NUM_AVERAGING_SAMPLES = 10; // At ~100 updates per seconds this means averaging over ~.1s + using MovingAverageMap = std::map< int, MovingAverage< glm::vec3, MAX_NUM_AVERAGING_SAMPLES> >; + MovingAverageMap _collectedSamples; #ifdef __APPLE__ QLibrary* _sixenseLibrary; @@ -109,3 +139,4 @@ private: }; #endif // hifi_SixenseManager_h + From be843a0035d820fbe7527ca7119acc60ba5682ac Mon Sep 17 00:00:00 2001 From: samcake Date: Fri, 23 Oct 2015 17:32:54 -0700 Subject: [PATCH 2/5] adding a rnning average on the velocity values returned by the hydra and adding a simple js to test --- examples/controllers/handPosesDebug.js | 102 ++++++++++++++++++ .../src/input-plugins/SixenseManager.h | 2 +- 2 files changed, 103 insertions(+), 1 deletion(-) create mode 100644 examples/controllers/handPosesDebug.js diff --git a/examples/controllers/handPosesDebug.js b/examples/controllers/handPosesDebug.js new file mode 100644 index 0000000000..c4e7352445 --- /dev/null +++ b/examples/controllers/handPosesDebug.js @@ -0,0 +1,102 @@ +// +// handPosesDebug.js +// examples +// +// Copyright 2015 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 +// + + + +function makeSphere(color) { + var SPHERE_SIZE = 0.05; + var sphere = Overlays.addOverlay("sphere", { + position: { x: 0, y: 0, z: 0 }, + size: SPHERE_SIZE, + color: color, + alpha: 1.0, + solid: true, + visible: true, + }); + + return sphere; +} + + +var NUM_HANDS = 2; +var NUM_SPHERES_PER_HAND = 2; +var LEFT_HAND = 0; +var RIGHT_HAND = 1; + +var COLORS = [ { red: 255, green: 0, blue: 0 }, { red: 0, green: 0, blue: 255 } ]; + +function index(handNum, indexNum) { + return handNum * NUM_HANDS + indexNum; +} + +var app = {}; + + +function setup() { + app.spheres = new Array(); + + for (var h = 0; h < NUM_HANDS; h++) { + for (var s = 0; s < NUM_SPHERES_PER_HAND; s++) { + var i = index(h, s); + app.spheres[i] = makeSphere(COLORS[h]); + print("Added Sphere num " + i + " = " + JSON.stringify(app.spheres[i])); + } + } +} + +function updateHand(handNum) { + + var pose; + var handName = "right"; + if (handNum == LEFT_HAND) { + pose = MyAvatar.getLeftHandPose(); + handName = "left"; + } else { + pose = MyAvatar.getRightHandPose(); + handName = "right"; + } + + if (pose.valid) { + //print(handName + " hand moving" + JSON.stringify(pose)); + Overlays.editOverlay(app.spheres[index(handNum, 0)], { + position: pose.translation, + visible: true, + }); + var vpos = Vec3.sum(pose.velocity, pose.translation); + Overlays.editOverlay(app.spheres[index(handNum, 1)], { + position: vpos, + visible: true, + }); + } else { + Overlays.editOverlay(app.spheres[index(handNum, 0)], { + visible: false + }); + + Overlays.editOverlay(app.spheres[index(handNum, 1)], { + visible: false + }); + } +} + +function update() { + updateHand(LEFT_HAND); + updateHand(RIGHT_HAND); +} + +function scriptEnding() { + print("Removing spheres = " + JSON.stringify(app.spheres)); + for (var i = 0; i < app.spheres.length; i++) { + Overlays.deleteOverlay(app.spheres[i]); + } +} + +setup(); +Script.update.connect(update); +Script.scriptEnding.connect(scriptEnding); diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.h b/libraries/input-plugins/src/input-plugins/SixenseManager.h index c0d908ed45..3bc82b365b 100644 --- a/libraries/input-plugins/src/input-plugins/SixenseManager.h +++ b/libraries/input-plugins/src/input-plugins/SixenseManager.h @@ -121,7 +121,7 @@ private: } }; - static const int MAX_NUM_AVERAGING_SAMPLES = 10; // At ~100 updates per seconds this means averaging over ~.1s + static const int MAX_NUM_AVERAGING_SAMPLES = 1000; // At ~100 updates per seconds this means averaging over ~.1s using MovingAverageMap = std::map< int, MovingAverage< glm::vec3, MAX_NUM_AVERAGING_SAMPLES> >; MovingAverageMap _collectedSamples; From d400c694f6d821f912002173b0c4097c54d32eaa Mon Sep 17 00:00:00 2001 From: samcake Date: Fri, 23 Oct 2015 18:09:54 -0700 Subject: [PATCH 3/5] Cleaning up for release --- examples/controllers/handPosesDebug.js | 11 +++++------ .../src/input-plugins/SixenseManager.cpp | 14 ++++++++++---- .../src/input-plugins/SixenseManager.h | 5 +++-- 3 files changed, 18 insertions(+), 12 deletions(-) diff --git a/examples/controllers/handPosesDebug.js b/examples/controllers/handPosesDebug.js index c4e7352445..6c933b2565 100644 --- a/examples/controllers/handPosesDebug.js +++ b/examples/controllers/handPosesDebug.js @@ -51,8 +51,7 @@ function setup() { } } -function updateHand(handNum) { - +function updateHand(handNum, deltaTime) { var pose; var handName = "right"; if (handNum == LEFT_HAND) { @@ -69,7 +68,7 @@ function updateHand(handNum) { position: pose.translation, visible: true, }); - var vpos = Vec3.sum(pose.velocity, pose.translation); + var vpos = Vec3.sum(Vec3.multiply(10 * deltaTime, pose.velocity), pose.translation); Overlays.editOverlay(app.spheres[index(handNum, 1)], { position: vpos, visible: true, @@ -85,9 +84,9 @@ function updateHand(handNum) { } } -function update() { - updateHand(LEFT_HAND); - updateHand(RIGHT_HAND); +function update(deltaTime) { + updateHand(LEFT_HAND, deltaTime); + updateHand(RIGHT_HAND, deltaTime); } function scriptEnding() { diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp index 1f72d66019..7ce30ec26f 100644 --- a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp +++ b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp @@ -240,7 +240,8 @@ void SixenseManager::update(float deltaTime, bool jointsCaptured) { } else { auto hand = left ? controller::StandardPoseChannel::LEFT_HAND : controller::StandardPoseChannel::RIGHT_HAND; _poseStateMap[hand] = controller::Pose(); - _collectedSamples[hand].clear(); + _collectedSamples[hand].first.clear(); + _collectedSamples[hand].second.clear(); } } @@ -467,10 +468,15 @@ void SixenseManager::handlePoseEvent(float deltaTime, glm::vec3 position, glm::q // Average auto& samples = _collectedSamples[hand]; - samples.addSample(velocity); - velocity = samples.average; + samples.first.addSample(velocity); + velocity = samples.first.average; + + // FIXME: // Not using quaternion average yet for angular velocity because it s probably wrong but keep the MovingAverage in place + //samples.second.addSample(glm::vec4(angularVelocity.x, angularVelocity.y, angularVelocity.z, angularVelocity.w)); + //angularVelocity = glm::quat(samples.second.average.w, samples.second.average.x, samples.second.average.y, samples.second.average.z); } else if (!prevPose.isValid()) { - _collectedSamples[hand].clear(); + _collectedSamples[hand].first.clear(); + _collectedSamples[hand].second.clear(); } _poseStateMap[hand] = controller::Pose(position, rotation, velocity, angularVelocity); diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.h b/libraries/input-plugins/src/input-plugins/SixenseManager.h index 3bc82b365b..b9ca9d8479 100644 --- a/libraries/input-plugins/src/input-plugins/SixenseManager.h +++ b/libraries/input-plugins/src/input-plugins/SixenseManager.h @@ -121,8 +121,9 @@ private: } }; - static const int MAX_NUM_AVERAGING_SAMPLES = 1000; // At ~100 updates per seconds this means averaging over ~.1s - using MovingAverageMap = std::map< int, MovingAverage< glm::vec3, MAX_NUM_AVERAGING_SAMPLES> >; + static const int MAX_NUM_AVERAGING_SAMPLES = 50; // At ~100 updates per seconds this means averaging over ~.5s + using Samples = std::pair< MovingAverage< glm::vec3, MAX_NUM_AVERAGING_SAMPLES>, MovingAverage< glm::vec4, MAX_NUM_AVERAGING_SAMPLES> >; + using MovingAverageMap = std::map< int, Samples >; MovingAverageMap _collectedSamples; #ifdef __APPLE__ From a8872d065b2b4aa680b49b86b99680b43e8c31a0 Mon Sep 17 00:00:00 2001 From: samcake Date: Mon, 26 Oct 2015 11:23:13 -0700 Subject: [PATCH 4/5] MOve the movingAverage class to shared next to SimpleMovingAverage --- .../src/input-plugins/SixenseManager.h | 27 ++---------------- libraries/shared/src/SimpleMovingAverage.h | 28 +++++++++++++++++++ 2 files changed, 30 insertions(+), 25 deletions(-) diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.h b/libraries/input-plugins/src/input-plugins/SixenseManager.h index b9ca9d8479..c6b938c774 100644 --- a/libraries/input-plugins/src/input-plugins/SixenseManager.h +++ b/libraries/input-plugins/src/input-plugins/SixenseManager.h @@ -24,6 +24,8 @@ #endif +#include + #include #include @@ -95,31 +97,6 @@ private: float _lastDistance; bool _useSixenseFilter = true; - template class MovingAverage { - public: - using Samples = std::list< T >; - Samples samples; - T average; - - void clear() { - samples.clear(); - } - - bool isAverageValid() const { return !samples.empty(); } - - void addSample(T sample) { - samples.push_front(sample); - int numSamples = samples.size(); - - if (numSamples < MAX_NUM_SAMPLES) { - average = (sample + average * float(numSamples - 1)) / float(numSamples); - } else { - T tail = samples.back(); - samples.pop_back(); - average = average + (sample - tail) / float(numSamples); - } - } - }; static const int MAX_NUM_AVERAGING_SAMPLES = 50; // At ~100 updates per seconds this means averaging over ~.5s using Samples = std::pair< MovingAverage< glm::vec3, MAX_NUM_AVERAGING_SAMPLES>, MovingAverage< glm::vec4, MAX_NUM_AVERAGING_SAMPLES> >; diff --git a/libraries/shared/src/SimpleMovingAverage.h b/libraries/shared/src/SimpleMovingAverage.h index 194a078194..5f4e7367fc 100644 --- a/libraries/shared/src/SimpleMovingAverage.h +++ b/libraries/shared/src/SimpleMovingAverage.h @@ -15,6 +15,7 @@ #define hifi_SimpleMovingAverage_h #include +#include class SimpleMovingAverage { public: @@ -40,4 +41,31 @@ private: float ONE_MINUS_WEIGHTING; }; + +template class MovingAverage { +public: + using Samples = std::list< T >; + Samples samples; + T average; + + void clear() { + samples.clear(); + } + + bool isAverageValid() const { return !samples.empty(); } + + void addSample(T sample) { + samples.push_front(sample); + int numSamples = samples.size(); + + if (numSamples < MAX_NUM_SAMPLES) { + average = (sample + average * (float)(numSamples - 1)) / (float)(numSamples); + } else { + T tail = samples.back(); + samples.pop_back(); + average = average + (sample - tail) / (float)(numSamples); + } + } +}; + #endif // hifi_SimpleMovingAverage_h From 990764d85577ed5f9e167b3fab91bbb6f1176407 Mon Sep 17 00:00:00 2001 From: samcake Date: Mon, 26 Oct 2015 13:29:13 -0700 Subject: [PATCH 5/5] Really fixing the MovingAverage class... --- libraries/shared/src/SimpleMovingAverage.h | 22 +++++++++------------- 1 file changed, 9 insertions(+), 13 deletions(-) diff --git a/libraries/shared/src/SimpleMovingAverage.h b/libraries/shared/src/SimpleMovingAverage.h index 5f4e7367fc..53754ae241 100644 --- a/libraries/shared/src/SimpleMovingAverage.h +++ b/libraries/shared/src/SimpleMovingAverage.h @@ -15,7 +15,6 @@ #define hifi_SimpleMovingAverage_h #include -#include class SimpleMovingAverage { public: @@ -44,27 +43,24 @@ private: template class MovingAverage { public: - using Samples = std::list< T >; - Samples samples; + const float WEIGHTING = 1.0f / (float)MAX_NUM_SAMPLES; + const float ONE_MINUS_WEIGHTING = 1.0f - WEIGHTING; + int numSamples{ 0 }; T average; void clear() { - samples.clear(); + numSamples = 0; } - bool isAverageValid() const { return !samples.empty(); } + bool isAverageValid() const { return (numSamples > 0); } void addSample(T sample) { - samples.push_front(sample); - int numSamples = samples.size(); - - if (numSamples < MAX_NUM_SAMPLES) { - average = (sample + average * (float)(numSamples - 1)) / (float)(numSamples); + if (numSamples > 0) { + average = (sample * WEIGHTING) + (average * ONE_MINUS_WEIGHTING); } else { - T tail = samples.back(); - samples.pop_back(); - average = average + (sample - tail) / (float)(numSamples); + average = sample; } + numSamples++; } };