Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Passing angular velocity from OSVR into SteamVR #103

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
62 changes: 62 additions & 0 deletions src/AngVelTools.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,62 @@
/** @file
@brief Header

@date 2016

@author
Sensics, Inc.
<http://sensics.com/osvr>
*/

// 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 <osvr/Util/EigenCoreGeometry.h>
#include <osvr/Util/EigenQuatExponentialMap.h>

// Standard includes
#include <cmath>

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
84 changes: 49 additions & 35 deletions src/OSVRTrackedDevice.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,29 +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"

// OpenVR includes
#include <openvr_driver.h>

// Library/third-party includes
#include <osvr/Client/RenderManagerConfig.h>
#include <osvr/ClientKit/Display.h>
#include <osvr/ClientKit/InterfaceStateC.h>
#include <osvr/Display/DisplayEnumerator.h>
#include <osvr/RenderKit/DistortionCorrectTextureCoordinate.h>
#include <osvr/Util/EigenInterop.h>
#include <osvr/Util/PlatformConfig.h>
#include <osvr/Client/RenderManagerConfig.h>
#include <util/FixedLengthStringFunctions.h>
#include <osvr/RenderKit/DistortionCorrectTextureCoordinate.h>

// Standard includes
#include <algorithm> // for std::find
#include <algorithm> // for std::find
#include <cstring>
#include <ctime>
#include <exception>
Expand Down Expand Up @@ -143,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";
}

Expand Down Expand Up @@ -274,7 +275,7 @@ vr::DistortionCoordinates_t OSVRTrackedDevice::ComputeDistortion(vr::EVREye eye,

const auto osvr_eye = static_cast<size_t>(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_;

Expand Down Expand Up @@ -720,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;

Expand All @@ -746,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 = "";

Expand Down Expand Up @@ -837,7 +838,6 @@ std::string OSVRTrackedDevice::GetStringTrackedDeviceProperty(vr::ETrackedDevice
if (error)
*error = vr::TrackedProp_ValueNotProvidedByDevice;
return default_value;

}

#include "ignore-warning/pop"
Expand All @@ -848,21 +848,34 @@ 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;

auto* self = static_cast<OSVRTrackedDevice*>(userdata);

// Get angular velocity in correct format for SteamVR
OSVR_TimeValue timeval2;
OSVR_AngularVelocityState state;
OSVR_VelocityState velocitystate;

osvrGetVelocityState(self->trackerInterface_.get(), &timeval2, &velocitystate);

double dt = state.dt;
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;
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What is the intention behind this line? Wouldn't the poserotation.inverse() pre-rotation be eliminated by the poserotation post-rotation?

Copy link

@0x1100 0x1100 Jan 14, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

OSVR gives you the angular velocity in the same reference frame than the HMD's orientation. @mdigkin claims, and I'm willing to believe him, that SteamVR wants the angular velocity using the HMD as the reference frame. The intention here is to have the angular velocity in the reference frame SteamVR expects it to be.

They do not cancel each other because rotation composition is not commutative.

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

rotation composition is not commutative.

I realize that, but it's just strange to see that line without explanation of the intention.

// Convert invcremental rotation to angular velocity
Eigen::Vector3d angularvelocity = osvr::vbtracker::incRotToAngVelVec(angvel_incrementalRotation, dt);

vr::DriverPose_t pose;
pose.poseTimeOffset = 0; // close enough

Eigen::Vector3d::Map(pose.vecWorldFromDriverTranslation) = Eigen::Vector3d::Zero();
Eigen::Vector3d::Map(pose.vecDriverFromHeadTranslation) = Eigen::Vector3d::Zero();

map(pose.qWorldFromDriverRotation) = Eigen::Quaterniond::Identity();

map(pose.qDriverFromHeadRotation) = Eigen::Quaterniond::Identity();

// Position
Expand All @@ -875,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) = Eigen::Vector3d::Zero();
// 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;
Expand Down Expand Up @@ -991,7 +1010,7 @@ void OSVRTrackedDevice::configure()
display_.position.y = position_y;
display_.rotation = rotation;
display_.verticalRefreshRate = settings_->getSetting<double>("verticalRefreshRate", getVerticalRefreshRate());
display_.attachedToDesktop = false; // assuming direct mode
display_.attachedToDesktop = false; // assuming direct mode
display_.edidVendorId = settings_->getSetting<uint32_t>("edidVendorId", 0xd24e); // SVR
display_.edidProductId = settings_->getSetting<uint32_t>("edidProductId", 0x1019);

Expand Down Expand Up @@ -1033,7 +1052,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);
Expand Down Expand Up @@ -1077,17 +1096,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.";
Expand Down Expand Up @@ -1130,16 +1145,15 @@ std::pair<float, float> 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};
}
}