From 98fe06be578b5c29c72654bab8369457dee1c944 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 2 Oct 2015 09:30:18 -0700 Subject: [PATCH 1/4] cleanup sixense/hydra calibration --- .../src/input-plugins/SixenseManager.cpp | 114 +++++------------- .../src/input-plugins/SixenseManager.h | 4 +- 2 files changed, 32 insertions(+), 86 deletions(-) diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp index 9bc3a71fd5..dbacc23f26 100644 --- a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp +++ b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp @@ -13,11 +13,12 @@ #include -#include +#include #include +#include +#include #include "NumericalConstants.h" -#include #include "SixenseManager.h" #include "UserActivityLogger.h" @@ -38,14 +39,12 @@ const unsigned int RIGHT_MASK = 1U << 1; const int CALIBRATION_STATE_IDLE = 0; const int CALIBRATION_STATE_X = 1; -const int CALIBRATION_STATE_Y = 2; -const int CALIBRATION_STATE_Z = 3; -const int CALIBRATION_STATE_COMPLETE = 4; +const int CALIBRATION_STATE_COMPLETE = 2; -// default (expected) location of neck in sixense space -const float NECK_X = 0.25f; // meters -const float NECK_Y = 0.3f; // meters -const float NECK_Z = 0.3f; // meters +// default (expected) location of neck in sixense space when sitting +const float NECK_X = -0.25f; // meters +const float NECK_Y = -0.35f; // meters +const float NECK_Z = -0.3f; // meters const float CONTROLLER_THRESHOLD = 0.35f; @@ -92,9 +91,9 @@ bool SixenseManager::isSupported() const { void SixenseManager::activate() { #ifdef HAVE_SIXENSE _calibrationState = CALIBRATION_STATE_IDLE; - // By default we assume the _neckBase (in orb frame) is as high above the orb + // By default we assume the _avatarPosition (in orb frame) is as high above the orb // as the "torso" is below it. - _neckBase = glm::vec3(NECK_X, -NECK_Y, NECK_Z); + _avatarPosition = glm::vec3(NECK_X, NECK_Y, NECK_Z); CONTAINER->addMenu(MENU_PATH); CONTAINER->addMenuItem(MENU_PATH, TOGGLE_SMOOTH, @@ -258,11 +257,13 @@ void SixenseManager::update(float deltaTime, bool jointsCaptured) { #ifdef HAVE_SIXENSE // the calibration sequence is: -// (1) press BUTTON_FWD on both hands -// (2) reach arm straight out to the side (X) -// (3) lift arms staight up above head (Y) -// (4) move arms a bit forward (Z) -// (5) release BUTTON_FWD on both hands +// (1) reach arm straight out to the sides (xAxis is to the left) +// (2) press BUTTON_FWD on both hands and hold for one second +// (3) release both BUTTON_FWDs +// +// The code will: +// (4) assume that the orb is on a flat surface (yAxis is UP) +// (5) compute the forward direction (zAxis = xAxis cross yAxis) const float MINIMUM_ARM_REACH = 0.3f; // meters const float MAXIMUM_NOISE_LEVEL = 0.05f; // meters @@ -279,21 +280,16 @@ void SixenseManager::updateCalibration(void* controllersX) { return; } switch (_calibrationState) { - case CALIBRATION_STATE_Y: - case CALIBRATION_STATE_Z: case CALIBRATION_STATE_COMPLETE: { // compute calibration results - // ATM we only handle the case where the XAxis has been measured, and we assume the rest - // (i.e. that the orb is on a level surface) - // TODO: handle COMPLETE state where all three axes have been defined. This would allow us - // to also handle the case where left and right controllers have been reversed. - _neckBase = 0.5f * (_reachLeft + _reachRight); // neck is midway between right and left reaches + _avatarPosition = - 0.5f * (_reachLeft + _reachRight); // neck is midway between right and left hands glm::vec3 xAxis = glm::normalize(_reachRight - _reachLeft); - glm::vec3 yAxis(0.0f, 1.0f, 0.0f); - glm::vec3 zAxis = glm::normalize(glm::cross(xAxis, yAxis)); - xAxis = glm::normalize(glm::cross(yAxis, zAxis)); - _orbRotation = glm::inverse(glm::quat_cast(glm::mat3(xAxis, yAxis, zAxis))); + glm::vec3 zAxis = glm::normalize(glm::cross(xAxis, Vectors::UNIT_Y)); + xAxis = glm::normalize(glm::cross(Vectors::UNIT_Y, zAxis)); + _avatarRotation = glm::inverse(glm::quat_cast(glm::mat3(xAxis, Vectors::UNIT_Y, zAxis))); + const float Y_OFFSET_CALIBRATED_HANDS_TO_AVATAR = -0.3f; + _avatarPosition.y += Y_OFFSET_CALIBRATED_HANDS_TO_AVATAR; qCDebug(inputplugins, "succeess: sixense calibration"); } break; @@ -349,54 +345,10 @@ void SixenseManager::updateCalibration(void* controllersX) { _lockExpiry = now + LOCK_DURATION; _lastDistance = 0.0f; _reachUp = 0.5f * (_reachLeft + _reachRight); - _calibrationState = CALIBRATION_STATE_Y; + _calibrationState = CALIBRATION_STATE_COMPLETE; qCDebug(inputplugins, "success: sixense calibration: left"); } } - else if (_calibrationState == CALIBRATION_STATE_Y) { - glm::vec3 torso = 0.5f * (_reachLeft + _reachRight); - glm::vec3 averagePosition = 0.5f * (_averageLeft + _averageRight); - float distance = (averagePosition - torso).y; - if (fabsf(distance) > fabsf(_lastDistance) + MAXIMUM_NOISE_LEVEL) { - // distance is increasing so acquire the data and push the expiry out - _reachUp = averagePosition; - _lastDistance = distance; - _lockExpiry = now + LOCK_DURATION; - } else if (now > _lockExpiry) { - if (_lastDistance > MINIMUM_ARM_REACH) { - // lock has expired so clamp the data and move on - _reachForward = _reachUp; - _lastDistance = 0.0f; - _lockExpiry = now + LOCK_DURATION; - _calibrationState = CALIBRATION_STATE_Z; - qCDebug(inputplugins, "success: sixense calibration: up"); - } - } - } - else if (_calibrationState == CALIBRATION_STATE_Z) { - glm::vec3 xAxis = glm::normalize(_reachRight - _reachLeft); - glm::vec3 torso = 0.5f * (_reachLeft + _reachRight); - //glm::vec3 yAxis = glm::normalize(_reachUp - torso); - glm::vec3 yAxis(0.0f, 1.0f, 0.0f); - glm::vec3 zAxis = glm::normalize(glm::cross(xAxis, yAxis)); - - glm::vec3 averagePosition = 0.5f * (_averageLeft + _averageRight); - float distance = glm::dot((averagePosition - torso), zAxis); - if (fabs(distance) > fabs(_lastDistance)) { - // distance is increasing so acquire the data and push the expiry out - _reachForward = averagePosition; - _lastDistance = distance; - _lockExpiry = now + LOCK_DURATION; - } else if (now > _lockExpiry) { - if (fabsf(_lastDistance) > 0.05f * MINIMUM_ARM_REACH) { - // lock has expired so clamp the data and move on - _calibrationState = CALIBRATION_STATE_COMPLETE; - qCDebug(inputplugins, "success: sixense calibration: forward"); - // TODO: it is theoretically possible to detect that the controllers have been - // accidentally switched (left hand is holding right controller) and to swap the order. - } - } - } } #endif // HAVE_SIXENSE @@ -456,12 +408,9 @@ void SixenseManager::handlePoseEvent(glm::vec3 position, glm::quat rotation, int // z // Transform the measured position into body frame. - glm::vec3 neck = _neckBase; - // Set y component of the "neck" to raise the measured position a little bit. - neck.y = 0.5f; - position = _orbRotation * (position - neck); + position = _avatarRotation * (position + _avatarPosition); - // From ABOVE the hand canonical axes looks like this: + // From ABOVE the hand canonical axes look like this: // // | | | | y | | | | // | | | | | | | | | @@ -480,28 +429,25 @@ void SixenseManager::handlePoseEvent(glm::vec3 position, glm::quat rotation, int // // Qsh = angleAxis(PI, zAxis) * angleAxis(-PI/2, xAxis) // - const glm::vec3 xAxis = glm::vec3(1.0f, 0.0f, 0.0f); - const glm::vec3 yAxis = glm::vec3(0.0f, 1.0f, 0.0f); - const glm::vec3 zAxis = glm::vec3(0.0f, 0.0f, 1.0f); - const glm::quat sixenseToHand = glm::angleAxis(PI, zAxis) * glm::angleAxis(-PI/2.0f, xAxis); + const glm::quat sixenseToHand = glm::angleAxis(PI, Vectors::UNIT_Z) * glm::angleAxis(-PI/2.0f, Vectors::UNIT_X); // In addition to Qsh each hand has pre-offset introduced by the shape of the sixense controllers // and how they fit into the hand in their relaxed state. This offset is a quarter turn about // the sixense's z-axis, with its direction different for the two hands: float sign = (index == 0) ? 1.0f : -1.0f; - const glm::quat preOffset = glm::angleAxis(sign * PI / 2.0f, zAxis); + const glm::quat preOffset = glm::angleAxis(sign * PI / 2.0f, Vectors::UNIT_Z); // Finally, there is a post-offset (same for both hands) to get the hand's rest orientation // (fingers forward, palm down) aligned properly in the avatar's model-frame, // and then a flip about the yAxis to get into model-frame. - const glm::quat postOffset = glm::angleAxis(PI, yAxis) * glm::angleAxis(PI / 2.0f, xAxis); + const glm::quat postOffset = glm::angleAxis(PI, Vectors::UNIT_Y) * glm::angleAxis(PI / 2.0f, Vectors::UNIT_X); // The total rotation of the hand uses the formula: // // rotation = postOffset * Qsh^ * (measuredRotation * preOffset) * Qsh // // TODO: find a shortcut with fewer rotations. - rotation = postOffset * glm::inverse(sixenseToHand) * rotation * preOffset * sixenseToHand; + rotation = _avatarRotation * postOffset * glm::inverse(sixenseToHand) * rotation * preOffset * sixenseToHand; _poseStateMap[makeInput(JointChannel(index)).getChannel()] = UserInputMapper::PoseValue(position, rotation); #endif // HAVE_SIXENSE diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.h b/libraries/input-plugins/src/input-plugins/SixenseManager.h index 03482287d9..c1cc7049af 100644 --- a/libraries/input-plugins/src/input-plugins/SixenseManager.h +++ b/libraries/input-plugins/src/input-plugins/SixenseManager.h @@ -97,8 +97,8 @@ private: int _calibrationState; // these are calibration results - glm::vec3 _neckBase; // midpoint between controllers during X-axis calibration - glm::quat _orbRotation; // rotates from orb frame into body frame + glm::vec3 _avatarPosition; // in hydra-frame + glm::quat _avatarRotation; // rotation of avatar in orb-frame float _armLength; // these are measured values used to compute the calibration results From 940b8eb529c0c7fa4f9713579bd23e2776add657 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 2 Oct 2015 09:39:13 -0700 Subject: [PATCH 2/4] improve some variable names --- .../input-plugins/src/input-plugins/SixenseManager.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp index dbacc23f26..403eee2d87 100644 --- a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp +++ b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp @@ -41,10 +41,7 @@ const int CALIBRATION_STATE_IDLE = 0; const int CALIBRATION_STATE_X = 1; const int CALIBRATION_STATE_COMPLETE = 2; -// default (expected) location of neck in sixense space when sitting -const float NECK_X = -0.25f; // meters -const float NECK_Y = -0.35f; // meters -const float NECK_Z = -0.3f; // meters +const glm::vec3 DEFAULT_AVATAR_POSITION(-0.25f, -0.35f, -0.3f); // in hydra frame const float CONTROLLER_THRESHOLD = 0.35f; @@ -93,7 +90,7 @@ void SixenseManager::activate() { _calibrationState = CALIBRATION_STATE_IDLE; // By default we assume the _avatarPosition (in orb frame) is as high above the orb // as the "torso" is below it. - _avatarPosition = glm::vec3(NECK_X, NECK_Y, NECK_Z); + _avatarPosition = DEFAULT_AVATAR_POSITION; CONTAINER->addMenu(MENU_PATH); CONTAINER->addMenuItem(MENU_PATH, TOGGLE_SMOOTH, From 50b2c8ae3796282ed80ab9924d8501e7539d084f Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 2 Oct 2015 09:39:34 -0700 Subject: [PATCH 3/4] remove warnings in linux --- libraries/shared/src/Interpolate.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/shared/src/Interpolate.cpp b/libraries/shared/src/Interpolate.cpp index 7acc0c8cbd..bef69c9a33 100644 --- a/libraries/shared/src/Interpolate.cpp +++ b/libraries/shared/src/Interpolate.cpp @@ -23,12 +23,12 @@ float Interpolate::bezierInterpolate(float y1, float y2, float y3, float u) { float Interpolate::interpolate3Points(float y1, float y2, float y3, float u) { assert(0.0f <= u && u <= 1.0f); - if (u <= 0.5f && y1 == y2 || u >= 0.5f && y2 == y3) { + if ((u <= 0.5f && y1 == y2) || (u >= 0.5f && y2 == y3)) { // Flat line. return y2; } - if (y2 >= y1 && y2 >= y3 || y2 <= y1 && y2 <= y3) { + if ((y2 >= y1 && y2 >= y3) || (y2 <= y1 && y2 <= y3)) { // U or inverted-U shape. // Make the slope at y2 = 0, which means that the control points half way between the value points have the value y2. if (u <= 0.5f) { From e9c71f38677cb4ba08d917d5e3bb5687b0d67812 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 2 Oct 2015 10:02:12 -0700 Subject: [PATCH 4/4] cleanup comments --- libraries/input-plugins/src/input-plugins/SixenseManager.cpp | 2 -- libraries/input-plugins/src/input-plugins/SixenseManager.h | 4 ++-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp index 403eee2d87..aa04c49adb 100644 --- a/libraries/input-plugins/src/input-plugins/SixenseManager.cpp +++ b/libraries/input-plugins/src/input-plugins/SixenseManager.cpp @@ -88,8 +88,6 @@ bool SixenseManager::isSupported() const { void SixenseManager::activate() { #ifdef HAVE_SIXENSE _calibrationState = CALIBRATION_STATE_IDLE; - // By default we assume the _avatarPosition (in orb frame) is as high above the orb - // as the "torso" is below it. _avatarPosition = DEFAULT_AVATAR_POSITION; CONTAINER->addMenu(MENU_PATH); diff --git a/libraries/input-plugins/src/input-plugins/SixenseManager.h b/libraries/input-plugins/src/input-plugins/SixenseManager.h index c1cc7049af..22340cfc95 100644 --- a/libraries/input-plugins/src/input-plugins/SixenseManager.h +++ b/libraries/input-plugins/src/input-plugins/SixenseManager.h @@ -97,8 +97,8 @@ private: int _calibrationState; // these are calibration results - glm::vec3 _avatarPosition; // in hydra-frame - glm::quat _avatarRotation; // rotation of avatar in orb-frame + glm::vec3 _avatarPosition; // in hydra-frame + glm::quat _avatarRotation; // in hydra-frame float _armLength; // these are measured values used to compute the calibration results