From 612ff9cef0fc534e5c88ded3423d511991f6639e Mon Sep 17 00:00:00 2001 From: mdigkin Date: Sat, 7 Jan 2017 01:14:19 +0000 Subject: [PATCH 1/4] Added angular velocty to pose passed to SteamVR --- src/AngVelTools.h | 62 +++++++++++++++++++++++++++++++++++++++ src/OSVRTrackedDevice.cpp | 17 ++++++++++- 2 files changed, 78 insertions(+), 1 deletion(-) create mode 100644 src/AngVelTools.h diff --git a/src/AngVelTools.h b/src/AngVelTools.h new file mode 100644 index 0000000..3bc99c0 --- /dev/null +++ b/src/AngVelTools.h @@ -0,0 +1,62 @@ +/** @file + @brief Header + + @date 2016 + + @author + Sensics, Inc. + +*/ + +// Copyright 2016 Sensics, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef INCLUDED_AngVelTools_h_GUID_628878EF_DAFC_4486_36BC_6C47BD452AB0 +#define INCLUDED_AngVelTools_h_GUID_628878EF_DAFC_4486_36BC_6C47BD452AB0 + +// Internal Includes +// - none + +// Library/third-party includes +#include +#include + +// Standard includes +#include + +namespace osvr { +namespace vbtracker { + /// use only for derivatives - has factor of 2/0.5 in it! + // inline Eigen::Quaterniond + // angVelVecToIncRot(Eigen::Vector3d const &angVelVec, double dt) { + // return util::ei_quat_exp_map::quat_exp(angVelVec * dt * 0.5).normalized(); + // } +// + /// use only for derivatives - has factor of 2/0.5 in it! + inline Eigen::Vector3d incRotToAngVelVec(Eigen::Quaterniond const &incRot, + double dt) { +#if 0 + if (incRot.w() >= 1. || incRot.vec().isZero(1e-10)) { + return Eigen::Vector3d::Zero(); + } + auto angle = std::acos(incRot.w()); + return incRot.vec().normalized() * angle * 2. / dt; +#else + return util::ei_quat_exp_map::quat_ln(incRot) * 2. / dt; +#endif + } + +} // namespace vbtracker +} // namespace osvr +#endif // INCLUDED_AngVelTools_h_GUID_628878EF_DAFC_4486_36BC_6C47BD452AB0 diff --git a/src/OSVRTrackedDevice.cpp b/src/OSVRTrackedDevice.cpp index b450f24..b2dff35 100644 --- a/src/OSVRTrackedDevice.cpp +++ b/src/OSVRTrackedDevice.cpp @@ -34,6 +34,8 @@ #include "platform_fixes.h" // strcasecmp #include "make_unique.h" #include "OSVRDisplay.h" +#include "AngVelTools.h" + // OpenVR includes #include @@ -855,6 +857,19 @@ void OSVRTrackedDevice::HmdTrackerCallback(void* userdata, const OSVR_TimeValue* auto* self = static_cast(userdata); + // Get angular velocity in correct format for SteamVR + OSVR_TimeValue timeval2; + OSVR_AngularVelocityState state; + auto* self = static_cast(userdata); + osvrGetAngularVelocityState(self->trackerInterface_.get(), &timeval2, &state); + double dt = state.dt; + Eigen::Quaterniond r = osvr::util::fromQuat(report->pose.rotation); + Eigen::Quaterniond q = osvr::util::fromQuat(state.incrementalRotation); + q = r.inverse()*q*r; + // Convert invcremental rotation to angular velocity + Eigen::Vector3d angularvelocity = osvr::vbtracker::incRotToAngVelVec(q, dt); + + vr::DriverPose_t pose; pose.poseTimeOffset = 0; // close enough @@ -876,7 +891,7 @@ void OSVRTrackedDevice::HmdTrackerCallback(void* userdata, const OSVR_TimeValue* map(pose.qRotation) = osvr::util::fromQuat(report->pose.rotation); // Angular velocity and acceleration are not currently consistently provided - Eigen::Vector3d::Map(pose.vecAngularVelocity) = Eigen::Vector3d::Zero(); + Eigen::Vector3d::Map(pose.vecAngularVelocity) = angularvelocity; Eigen::Vector3d::Map(pose.vecAngularAcceleration) = Eigen::Vector3d::Zero(); pose.result = vr::TrackingResult_Running_OK; From 11b26dacc000b8ddb59acbfdc3df8914fa9753d8 Mon Sep 17 00:00:00 2001 From: mdigkin Date: Sat, 7 Jan 2017 01:57:50 +0000 Subject: [PATCH 2/4] Added angular velocty to pose passed to SteamVR --- src/OSVRTrackedDevice.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/OSVRTrackedDevice.cpp b/src/OSVRTrackedDevice.cpp index b2dff35..b95101e 100644 --- a/src/OSVRTrackedDevice.cpp +++ b/src/OSVRTrackedDevice.cpp @@ -860,7 +860,6 @@ void OSVRTrackedDevice::HmdTrackerCallback(void* userdata, const OSVR_TimeValue* // Get angular velocity in correct format for SteamVR OSVR_TimeValue timeval2; OSVR_AngularVelocityState state; - auto* self = static_cast(userdata); osvrGetAngularVelocityState(self->trackerInterface_.get(), &timeval2, &state); double dt = state.dt; Eigen::Quaterniond r = osvr::util::fromQuat(report->pose.rotation); From ed787953a75ba759178c326ab1fa260e03b48bdd Mon Sep 17 00:00:00 2001 From: mdigkin Date: Sat, 7 Jan 2017 10:21:15 +0000 Subject: [PATCH 3/4] Clang formatting as per project guideline --- src/OSVRTrackedDevice.cpp | 82 ++++++++++++++++++--------------------- 1 file changed, 37 insertions(+), 45 deletions(-) diff --git a/src/OSVRTrackedDevice.cpp b/src/OSVRTrackedDevice.cpp index b95101e..c2e7a6e 100644 --- a/src/OSVRTrackedDevice.cpp +++ b/src/OSVRTrackedDevice.cpp @@ -26,31 +26,30 @@ #include "OSVRTrackedDevice.h" #include "Logging.h" -#include "osvr_compiler_detection.h" +#include "AngVelTools.h" +#include "OSVRDisplay.h" +#include "ValveStrCpy.h" #include "make_unique.h" #include "matrix_cast.h" +#include "osvr_compiler_detection.h" #include "osvr_device_properties.h" -#include "ValveStrCpy.h" #include "platform_fixes.h" // strcasecmp -#include "make_unique.h" -#include "OSVRDisplay.h" -#include "AngVelTools.h" - // OpenVR includes #include // Library/third-party includes +#include #include #include +#include #include #include -#include +#include #include -#include // Standard includes -#include // for std::find +#include // for std::find #include #include #include @@ -145,7 +144,7 @@ vr::EVRInitError OSVRTrackedDevice::Activate(uint32_t object_id) try { renderManagerConfig_.parse(configString); - } catch(const std::exception& e) { + } catch (const std::exception& e) { OSVR_LOG(err) << "OSVRTrackedDevice::Activate(): Exception parsing Render Manager config: " << e.what() << "\n"; } @@ -276,7 +275,7 @@ vr::DistortionCoordinates_t OSVRTrackedDevice::ComputeDistortion(vr::EVREye eye, const auto osvr_eye = static_cast(eye); const auto distortion_parameters = distortionParameters_[osvr_eye]; - const auto in_coords = osvr::renderkit::Float2 {{u, 1.0f - v}}; // flip v-coordinate + const auto in_coords = osvr::renderkit::Float2{{u, 1.0f - v}}; // flip v-coordinate const auto interpolators = (vr::Eye_Left == eye) ? &leftEyeInterpolators_ : &rightEyeInterpolators_; @@ -722,7 +721,7 @@ vr::HmdMatrix34_t OSVRTrackedDevice::GetMatrix34TrackedDeviceProperty(vr::ETrack return default_value; } -uint32_t OSVRTrackedDevice::GetStringTrackedDeviceProperty(vr::ETrackedDeviceProperty prop, char* value, uint32_t buffer_size, vr::ETrackedPropertyError *error) +uint32_t OSVRTrackedDevice::GetStringTrackedDeviceProperty(vr::ETrackedDeviceProperty prop, char* value, uint32_t buffer_size, vr::ETrackedPropertyError* error) { uint32_t default_value = 0; @@ -748,11 +747,11 @@ uint32_t OSVRTrackedDevice::GetStringTrackedDeviceProperty(vr::ETrackedDevicePro return 0; } - // ------------------------------------ - // Private Methods - // ------------------------------------ +// ------------------------------------ +// Private Methods +// ------------------------------------ -std::string OSVRTrackedDevice::GetStringTrackedDeviceProperty(vr::ETrackedDeviceProperty prop, vr::ETrackedPropertyError *error) +std::string OSVRTrackedDevice::GetStringTrackedDeviceProperty(vr::ETrackedDeviceProperty prop, vr::ETrackedPropertyError* error) { std::string default_value = ""; @@ -839,7 +838,6 @@ std::string OSVRTrackedDevice::GetStringTrackedDeviceProperty(vr::ETrackedDevice if (error) *error = vr::TrackedProp_ValueNotProvidedByDevice; return default_value; - } #include "ignore-warning/pop" @@ -857,18 +855,17 @@ void OSVRTrackedDevice::HmdTrackerCallback(void* userdata, const OSVR_TimeValue* auto* self = static_cast(userdata); - // Get angular velocity in correct format for SteamVR - OSVR_TimeValue timeval2; - OSVR_AngularVelocityState state; - osvrGetAngularVelocityState(self->trackerInterface_.get(), &timeval2, &state); - double dt = state.dt; - Eigen::Quaterniond r = osvr::util::fromQuat(report->pose.rotation); - Eigen::Quaterniond q = osvr::util::fromQuat(state.incrementalRotation); - q = r.inverse()*q*r; - // Convert invcremental rotation to angular velocity - Eigen::Vector3d angularvelocity = osvr::vbtracker::incRotToAngVelVec(q, dt); - - + // Get angular velocity in correct format for SteamVR + OSVR_TimeValue timeval2; + OSVR_AngularVelocityState state; + osvrGetAngularVelocityState(self->trackerInterface_.get(), &timeval2, &state); + double dt = state.dt; + Eigen::Quaterniond r = osvr::util::fromQuat(report->pose.rotation); + Eigen::Quaterniond q = osvr::util::fromQuat(state.incrementalRotation); + q = r.inverse() * q * r; + // Convert invcremental rotation to angular velocity + Eigen::Vector3d angularvelocity = osvr::vbtracker::incRotToAngVelVec(q, dt); + vr::DriverPose_t pose; pose.poseTimeOffset = 0; // close enough @@ -1005,7 +1002,7 @@ void OSVRTrackedDevice::configure() display_.position.y = position_y; display_.rotation = rotation; display_.verticalRefreshRate = settings_->getSetting("verticalRefreshRate", getVerticalRefreshRate()); - display_.attachedToDesktop = false; // assuming direct mode + display_.attachedToDesktop = false; // assuming direct mode display_.edidVendorId = settings_->getSetting("edidVendorId", 0xd24e); // SVR display_.edidProductId = settings_->getSetting("edidProductId", 0x1019); @@ -1047,7 +1044,7 @@ void OSVRTrackedDevice::configureDistortionParameters() // Initialize the distortion parameters OSVR_LOG(debug) << "OSVRTrackedDevice::configureDistortionParameters(): Number of eyes: " << displayConfiguration_.getEyes().size() << "."; for (size_t i = 0; i < displayConfiguration_.getEyes().size(); ++i) { - auto distortion = osvr::renderkit::DistortionParameters { displayConfiguration_, i }; + auto distortion = osvr::renderkit::DistortionParameters{displayConfiguration_, i}; distortion.m_desiredTriangles = 200 * 64; OSVR_LOG(debug) << "OSVRTrackedDevice::configureDistortionParameters(): Adding distortion for eye " << i << "."; distortionParameters_.push_back(distortion); @@ -1091,17 +1088,13 @@ osvr::display::ScanOutOrigin OSVRTrackedDevice::parseScanOutOrigin(std::string s // Make the string lowercase std::transform(str.begin(), str.end(), str.begin(), ::tolower); - if ("lower-left" == str || "ll" == str || "lowerleft" == str || "lower left" == str - || "bottom-left" == str || "bl" == str || "bottomleft" == str || "bottom left" == str) { + if ("lower-left" == str || "ll" == str || "lowerleft" == str || "lower left" == str || "bottom-left" == str || "bl" == str || "bottomleft" == str || "bottom left" == str) { return osvr::display::ScanOutOrigin::LowerLeft; - } else if ("lower-right" == str || "lr" == str || "lowerright" == str || "lower right" == str - || "bottom-right" == str || "br" == str || "bottomright" == str || "bottom right" == str) { + } else if ("lower-right" == str || "lr" == str || "lowerright" == str || "lower right" == str || "bottom-right" == str || "br" == str || "bottomright" == str || "bottom right" == str) { return osvr::display::ScanOutOrigin::LowerRight; - } else if ("upper-left" == str || "ul" == str || "upperleft" == str || "upper left" == str - || "top-left" == str || "tl" == str || "topleft" == str || "top left" == str) { + } else if ("upper-left" == str || "ul" == str || "upperleft" == str || "upper left" == str || "top-left" == str || "tl" == str || "topleft" == str || "top left" == str) { return osvr::display::ScanOutOrigin::UpperLeft; - } else if ("upper-right" == str || "ur" == str || "upperright" == str || "upper right" == str - || "top-right" == str || "tr" == str || "topright" == str || "top right" == str) { + } else if ("upper-right" == str || "ur" == str || "upperright" == str || "upper right" == str || "top-right" == str || "tr" == str || "topright" == str || "top right" == str) { return osvr::display::ScanOutOrigin::UpperRight; } else { OSVR_LOG(err) << "The string [" + str + "] could not be parsed as a scan-out origin. Use one of: lower-left, upper-left, lower-right, upper-right."; @@ -1144,16 +1137,15 @@ std::pair OSVRTrackedDevice::rotate(float u, float v, osvr::displa // Rotates normalized coordinates counter-clockwise using R = osvr::display::Rotation; if (R::Zero == rotation) { - return { u, v }; + return {u, v}; } else if (R::Ninety == rotation) { - return { 1.0f - v, u }; + return {1.0f - v, u}; } else if (R::OneEighty == rotation) { - return { 1.0f - u, 1.0f - v }; + return {1.0f - u, 1.0f - v}; } else if (R::TwoSeventy == rotation) { - return { v, 1.0f - u }; + return {v, 1.0f - u}; } else { OSVR_LOG(err) << "Unknown rotation [" << rotation << "] Assuming 0 degrees."; - return { u, v }; + return {u, v}; } } - From 893247a8ae1e6473e52ba19e51c895953ea599ef Mon Sep 17 00:00:00 2001 From: mdigkin Date: Fri, 13 Jan 2017 03:15:51 +0000 Subject: [PATCH 4/4] Check if angular velocity is valid in VelocityState --- src/OSVRTrackedDevice.cpp | 28 ++++++++++++++++++---------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/src/OSVRTrackedDevice.cpp b/src/OSVRTrackedDevice.cpp index c2e7a6e..e335d7a 100644 --- a/src/OSVRTrackedDevice.cpp +++ b/src/OSVRTrackedDevice.cpp @@ -41,11 +41,11 @@ // Library/third-party includes #include #include +#include #include #include #include #include -#include #include // Standard includes @@ -848,7 +848,7 @@ std::string OSVRTrackedDevice::GetStringTrackedDeviceProperty(vr::ETrackedDevice return default_value; } -void OSVRTrackedDevice::HmdTrackerCallback(void* userdata, const OSVR_TimeValue*, const OSVR_PoseReport* report) +void OSVRTrackedDevice::HmdTrackerCallback(void* userdata, const OSVR_TimeValue* timeval, const OSVR_PoseReport* report) { if (!userdata) return; @@ -858,13 +858,16 @@ void OSVRTrackedDevice::HmdTrackerCallback(void* userdata, const OSVR_TimeValue* // Get angular velocity in correct format for SteamVR OSVR_TimeValue timeval2; OSVR_AngularVelocityState state; - osvrGetAngularVelocityState(self->trackerInterface_.get(), &timeval2, &state); + OSVR_VelocityState velocitystate; + + osvrGetVelocityState(self->trackerInterface_.get(), &timeval2, &velocitystate); + double dt = state.dt; - Eigen::Quaterniond r = osvr::util::fromQuat(report->pose.rotation); - Eigen::Quaterniond q = osvr::util::fromQuat(state.incrementalRotation); - q = r.inverse() * q * r; + Eigen::Quaterniond poserotation = osvr::util::fromQuat(report->pose.rotation); + Eigen::Quaterniond angvel_incrementalRotation = osvr::util::fromQuat(velocitystate.angularVelocity.incrementalRotation); + angvel_incrementalRotation = poserotation.inverse() * angvel_incrementalRotation * poserotation; // Convert invcremental rotation to angular velocity - Eigen::Vector3d angularvelocity = osvr::vbtracker::incRotToAngVelVec(q, dt); + Eigen::Vector3d angularvelocity = osvr::vbtracker::incRotToAngVelVec(angvel_incrementalRotation, dt); vr::DriverPose_t pose; pose.poseTimeOffset = 0; // close enough @@ -873,7 +876,6 @@ void OSVRTrackedDevice::HmdTrackerCallback(void* userdata, const OSVR_TimeValue* Eigen::Vector3d::Map(pose.vecDriverFromHeadTranslation) = Eigen::Vector3d::Zero(); map(pose.qWorldFromDriverRotation) = Eigen::Quaterniond::Identity(); - map(pose.qDriverFromHeadRotation) = Eigen::Quaterniond::Identity(); // Position @@ -886,10 +888,16 @@ void OSVRTrackedDevice::HmdTrackerCallback(void* userdata, const OSVR_TimeValue* // Orientation map(pose.qRotation) = osvr::util::fromQuat(report->pose.rotation); - // Angular velocity and acceleration are not currently consistently provided - Eigen::Vector3d::Map(pose.vecAngularVelocity) = angularvelocity; + // Acceleration is not currently consistently provided Eigen::Vector3d::Map(pose.vecAngularAcceleration) = Eigen::Vector3d::Zero(); + // If angular velocity is valid, pass that data to SteamVR + if (velocitystate.angularVelocityValid) { + Eigen::Vector3d::Map(pose.vecAngularVelocity) = angularvelocity; + } else { + Eigen::Vector3d::Map(pose.vecAngularVelocity) = Eigen::Vector3d::Zero(); + } + pose.result = vr::TrackingResult_Running_OK; pose.poseIsValid = true; pose.willDriftInYaw = true;