diff options
author | Sven Gothel <[email protected]> | 2015-03-21 21:19:34 +0100 |
---|---|---|
committer | Sven Gothel <[email protected]> | 2015-03-21 21:19:34 +0100 |
commit | e490c3c7f7bb5461cfa78a214827aa534fb43a3e (patch) | |
tree | b86b0291ef529ec6b75cc548d73599fa9c283cd6 /LibOVR/Src/Tracking | |
parent | 05bb4364bfd9930fb1902efec86446ef035ee07a (diff) |
Bump OculusVR RIFT SDK to 0.4.4
Diffstat (limited to 'LibOVR/Src/Tracking')
-rw-r--r-- | LibOVR/Src/Tracking/Tracking_PoseState.h | 133 | ||||
-rw-r--r-- | LibOVR/Src/Tracking/Tracking_SensorState.h | 212 | ||||
-rw-r--r-- | LibOVR/Src/Tracking/Tracking_SensorStateReader.cpp | 207 | ||||
-rw-r--r-- | LibOVR/Src/Tracking/Tracking_SensorStateReader.h | 87 |
4 files changed, 639 insertions, 0 deletions
diff --git a/LibOVR/Src/Tracking/Tracking_PoseState.h b/LibOVR/Src/Tracking/Tracking_PoseState.h new file mode 100644 index 0000000..19e2ee2 --- /dev/null +++ b/LibOVR/Src/Tracking/Tracking_PoseState.h @@ -0,0 +1,133 @@ +/************************************************************************************ + +Filename : Tracking_PoseState.h +Content : Describes the complete pose at a point in time, including derivatives +Created : May 13, 2014 +Authors : Dov Katz + +Copyright : Copyright 2014 Oculus VR, LLC All Rights reserved. + +Licensed under the Oculus VR Rift SDK License Version 3.2 (the "License"); +you may not use the Oculus VR Rift SDK except in compliance with the License, +which is provided at the time of installation or download, or which +otherwise accompanies this software in either electronic or hard copy form. + +You may obtain a copy of the License at + +http://www.oculusvr.com/licenses/LICENSE-3.2 + +Unless required by applicable law or agreed to in writing, the Oculus VR SDK +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 Tracking_PoseState_h +#define Tracking_PoseState_h + +#include "../Kernel/OVR_Math.h" + +namespace OVR { + +// PoseState describes the complete pose, or a rigid body configuration, at a +// point in time, including first and second derivatives. It is used to specify +// instantaneous location and movement of the headset. +// SensorState is returned as a part of the sensor state. + +template<class T> +class PoseState +{ +public: + typedef typename CompatibleTypes<Pose<T> >::Type CompatibleType; + + PoseState() : TimeInSeconds(0.0) { } + PoseState(Pose<T> pose, double time) : ThePose(pose), TimeInSeconds(time) { } + + // float <-> double conversion constructor. + explicit PoseState(const PoseState<typename Math<T>::OtherFloatType> &src) + : ThePose(src.ThePose), + AngularVelocity(src.AngularVelocity), LinearVelocity(src.LinearVelocity), + AngularAcceleration(src.AngularAcceleration), LinearAcceleration(src.LinearAcceleration), + TimeInSeconds(src.TimeInSeconds) + { } + + // C-interop support: PoseStatef <-> ovrPoseStatef + PoseState(const typename CompatibleTypes<PoseState<T> >::Type& src) + : ThePose(src.ThePose), + AngularVelocity(src.AngularVelocity), LinearVelocity(src.LinearVelocity), + AngularAcceleration(src.AngularAcceleration), LinearAcceleration(src.LinearAcceleration), + TimeInSeconds(src.TimeInSeconds) + { } + + operator typename CompatibleTypes<PoseState<T> >::Type() const + { + typename CompatibleTypes<PoseState<T> >::Type result; + result.ThePose = ThePose; + result.AngularVelocity = AngularVelocity; + result.LinearVelocity = LinearVelocity; + result.AngularAcceleration = AngularAcceleration; + result.LinearAcceleration = LinearAcceleration; + result.TimeInSeconds = TimeInSeconds; + return result; + } + + Pose<T> ThePose; + Vector3<T> AngularVelocity; + Vector3<T> LinearVelocity; + Vector3<T> AngularAcceleration; + Vector3<T> LinearAcceleration; + // Absolute time of this state sample; always a double measured in seconds. + double TimeInSeconds; + + // ***** Helpers for Pose integration + + // Stores and integrates gyro angular velocity reading for a given time step. + void StoreAndIntegrateGyro(Vector3d angVel, double dt); + // Stores and integrates position/velocity from accelerometer reading for a given time step. + void StoreAndIntegrateAccelerometer(Vector3d linearAccel, double dt); + + // Performs integration of state by adding next state delta to it + // to produce a combined state change + void AdvanceByDelta(const PoseState<T>& delta); +}; + + +template<class T> +PoseState<T> operator*(const OVR::Pose<T>& trans, const PoseState<T>& poseState) +{ + PoseState<T> result; + result.ThePose = trans * poseState.ThePose; + result.LinearVelocity = trans.Rotate(poseState.LinearVelocity); + result.LinearAcceleration = trans.Rotate(poseState.LinearAcceleration); + result.AngularVelocity = trans.Rotate(poseState.AngularVelocity); + result.AngularAcceleration = trans.Rotate(poseState.AngularAcceleration); + return result; +} + + +// External API returns pose as float, but uses doubles internally for quaternion precision. +typedef PoseState<float> PoseStatef; +typedef PoseState<double> PoseStated; + + +} // namespace OVR::Vision + + +namespace OVR { + + template<> struct CompatibleTypes<OVR::PoseState<float> > { typedef ovrPoseStatef Type; }; + template<> struct CompatibleTypes<OVR::PoseState<double> > { typedef ovrPoseStated Type; }; + + static_assert((sizeof(PoseState<double>) == sizeof(Pose<double>) + 4*sizeof(Vector3<double>) + sizeof(double)), "sizeof(PoseState<double>) failure"); +#ifdef OVR_CPU_X86_64 + static_assert((sizeof(PoseState<float>) == sizeof(Pose<float>) + 4*sizeof(Vector3<float>) + sizeof(uint32_t) + sizeof(double)), "sizeof(PoseState<float>) failure"); //TODO: Manually pad template. +#elif defined(OVR_OS_WIN32) // The Windows 32 bit ABI aligns 64 bit values on 64 bit boundaries + static_assert((sizeof(PoseState<float>) == sizeof(Pose<float>) + 4*sizeof(Vector3<float>) + sizeof(uint32_t) + sizeof(double)), "sizeof(PoseState<float>) failure"); +#else // Else Unix/Apple 32 bit ABI, which aligns 64 bit values on 32 bit boundaries. + static_assert((sizeof(PoseState<float>) == sizeof(Pose<float>) + 4*sizeof(Vector3<float>) + sizeof(double)), "sizeof(PoseState<float>) failure"); +#endif +} + +#endif // Tracking_PoseState_h diff --git a/LibOVR/Src/Tracking/Tracking_SensorState.h b/LibOVR/Src/Tracking/Tracking_SensorState.h new file mode 100644 index 0000000..e13c3ef --- /dev/null +++ b/LibOVR/Src/Tracking/Tracking_SensorState.h @@ -0,0 +1,212 @@ +/************************************************************************************ + +Filename : Tracking_SensorState.h +Content : Sensor state information shared by tracking system with games +Created : May 13, 2014 +Authors : Dov Katz, Chris Taylor + +Copyright : Copyright 2014 Oculus VR, LLC All Rights reserved. + +Licensed under the Oculus VR Rift SDK License Version 3.2 (the "License"); +you may not use the Oculus VR Rift SDK except in compliance with the License, +which is provided at the time of installation or download, or which +otherwise accompanies this software in either electronic or hard copy form. + +You may obtain a copy of the License at + +http://www.oculusvr.com/licenses/LICENSE-3.2 + +Unless required by applicable law or agreed to in writing, the Oculus VR SDK +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 Tracking_SensorState_h +#define Tracking_SensorState_h + +#include "Tracking_PoseState.h" +#include "../Kernel/OVR_SharedMemory.h" +#include "../Kernel/OVR_Lockless.h" +#include "../Kernel/OVR_String.h" +#include "../Util/Util_LatencyTest2State.h" +#include "../Sensors/OVR_DeviceConstants.h" + +// CAPI forward declarations. +struct ovrTrackingState_; +typedef struct ovrTrackingState_ ovrTrackingState; +struct ovrPoseStatef_; +typedef struct ovrPoseStatef_ ovrPoseStatef; + +namespace OVR { namespace Tracking { + + +//------------------------------------------------------------------------------------- +// ***** Sensor State +// These values are reported as compatible with C API. + +// Bit flags describing the current status of sensor tracking. +enum StatusBits +{ + // Tracked bits: Toggled by SensorFusion + Status_OrientationTracked = 0x0001, // Orientation is currently tracked (connected and in use) + Status_PositionTracked = 0x0002, // Position is currently tracked (false if out of range) + Status_CameraPoseTracked = 0x0004, // Camera pose is currently tracked + + // Connected bits: Toggled by TrackingManager + Status_PositionConnected = 0x0020, // Position tracking HW is connected + Status_HMDConnected = 0x0080, // HMD is available & connected + + // Masks + Status_AllMask = 0xffff, + Status_TrackingMask = Status_PositionTracked | Status_OrientationTracked | Status_CameraPoseTracked, + Status_ConnectedMask = Status_PositionConnected | Status_HMDConnected, +}; + + +// Full state of of the sensor reported by GetSensorState() at a given absolute time. +class TrackingState +{ +public: + TrackingState() : HeadPose(), CameraPose(), LeveledCameraPose(), RawSensorData(), StatusFlags(0), LastVisionProcessingTime(0.0) { } + + // C-interop support + TrackingState(const ovrTrackingState& s); + operator ovrTrackingState () const; + + // HMD pose information for the requested time. + PoseStatef HeadPose; + + // Orientation and position of the external camera, if present. + Posef CameraPose; + // Orientation and position of the camera after alignment with gravity + Posef LeveledCameraPose; + + // Most recent sensor data received from the HMD + SensorDataType RawSensorData; + + // Sensor status described by ovrStatusBits. + uint32_t StatusFlags; + + //// 0.4.1 + + // Measures the time from receiving the camera frame until vision CPU processing completes. + double LastVisionProcessingTime; + + //// 0.4.3 + + // Measures the time from exposure until the pose is available for the frame, including processing time. + double LastVisionFrameLatency; + + // Tag the vision processing results to a certain frame counter number. + uint32_t LastCameraFrameCounter; +}; + + +// ----------------------------------------------- + +#pragma pack(push, 8) + +struct LocklessSensorStatePadding; + +// State version stored in lockless updater "queue" and used for +// prediction by GetPoseAtTime/GetSensorStateAtTime +struct LocklessSensorState +{ + PoseState<double> WorldFromImu; + SensorDataType RawSensorData; + Pose<double> WorldFromCamera; + uint32_t StatusFlags; + uint32_t _PAD_0_; + + // ImuFromCpf for HMD pose tracking + Posed ImuFromCpf; + + // Performance logging + double LastVisionProcessingTime; + double LastVisionFrameLatency; + uint32_t LastCameraFrameCounter; + uint32_t _PAD_1_; + + // Initialized to invalid state + LocklessSensorState() : + WorldFromImu() + , RawSensorData() + , WorldFromCamera() + , StatusFlags(0) + , _PAD_0_(0) // This assignment should be irrelevant, but it quells static/runtime analysis complaints. + , ImuFromCpf() + , LastVisionProcessingTime(0.0) + , LastVisionFrameLatency(0.0) + , LastCameraFrameCounter(0) + , _PAD_1_(0) // " + { + } + + LocklessSensorState& operator = (const LocklessSensorStatePadding& rhs); +}; + +static_assert((sizeof(LocklessSensorState) == sizeof(PoseState<double>) + sizeof(SensorDataType) + sizeof(Pose<double>) + 2*sizeof(uint32_t) + sizeof(Posed) + sizeof(double)*2 + sizeof(uint32_t)*2), "sizeof(LocklessSensorState) failure"); + +// Padded out version stored in the updater slots +// Designed to be a larger fixed size to allow the data to grow in the future +// without breaking older compiled code. +struct LocklessSensorStatePadding +{ + uint64_t words[64]; + + static const int DataWords = (sizeof(LocklessSensorState) + sizeof(uint64_t) - 1) / sizeof(uint64_t); + + // Just copy the low data words + inline LocklessSensorStatePadding& operator=(const LocklessSensorState& rhs) + { + const uint64_t* src = (const uint64_t*)&rhs; + + // if this fires off, then increase words' array size + OVR_ASSERT(sizeof(words) > sizeof(LocklessSensorState)); + + for (int i = 0; i < DataWords; ++i) + { + words[i] = src[i]; + } + + return *this; + } +}; + +// Just copy the low data words +inline LocklessSensorState& LocklessSensorState::operator = (const LocklessSensorStatePadding& rhs) +{ + uint64_t* dest = (uint64_t*)this; + + for (int i = 0; i < LocklessSensorStatePadding::DataWords; ++i) + { + dest[i] = rhs.words[i]; + } + + return *this; +} + +#pragma pack(pop) + +// A lockless updater for sensor state +typedef LocklessUpdater<LocklessSensorState, LocklessSensorStatePadding> SensorStateUpdater; + + +//// Combined state + +struct CombinedSharedStateUpdater +{ + SensorStateUpdater SharedSensorState; + Util::LockessRecordUpdater SharedLatencyTestState; +}; + +typedef SharedObjectWriter< CombinedSharedStateUpdater > CombinedSharedStateWriter; +typedef SharedObjectReader< CombinedSharedStateUpdater > CombinedSharedStateReader; + + +}} // namespace OVR::Tracking + +#endif diff --git a/LibOVR/Src/Tracking/Tracking_SensorStateReader.cpp b/LibOVR/Src/Tracking/Tracking_SensorStateReader.cpp new file mode 100644 index 0000000..a506b26 --- /dev/null +++ b/LibOVR/Src/Tracking/Tracking_SensorStateReader.cpp @@ -0,0 +1,207 @@ +/************************************************************************************ + +Filename : Tracking_SensorStateReader.cpp +Content : Separate reader component that is able to recover sensor pose +Created : June 4, 2014 +Authors : Chris Taylor + +Copyright : Copyright 2014 Oculus VR, LLC All Rights reserved. + +Licensed under the Oculus VR Rift SDK License Version 3.2 (the "License"); +you may not use the Oculus VR Rift SDK except in compliance with the License, +which is provided at the time of installation or download, or which +otherwise accompanies this software in either electronic or hard copy form. + +You may obtain a copy of the License at + +http://www.oculusvr.com/licenses/LICENSE-3.2 + +Unless required by applicable law or agreed to in writing, the Oculus VR SDK +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. + +*************************************************************************************/ + +#include "Tracking_SensorStateReader.h" +#include "Tracking_PoseState.h" + +namespace OVR { namespace Tracking { + + +//------------------------------------------------------------------------------------- + +// This is a "perceptually tuned predictive filter", which means that it is optimized +// for improvements in the VR experience, rather than pure error. In particular, +// jitter is more perceptible at lower speeds whereas latency is more perceptible +// after a high-speed motion. Therefore, the prediction interval is dynamically +// adjusted based on speed. Significant more research is needed to further improve +// this family of filters. +static Pose<double> calcPredictedPose(const PoseState<double>& poseState, double predictionDt) +{ + Pose<double> pose = poseState.ThePose; + const double linearCoef = 1.0; + Vector3d angularVelocity = poseState.AngularVelocity; + double angularSpeed = angularVelocity.Length(); + + // This could be tuned so that linear and angular are combined with different coefficients + double speed = angularSpeed + linearCoef * poseState.LinearVelocity.Length(); + + const double slope = 0.2; // The rate at which the dynamic prediction interval varies + double candidateDt = slope * speed; // TODO: Replace with smoothstep function + + double dynamicDt = predictionDt; + + // Choose the candidate if it is shorter, to improve stability + if (candidateDt < predictionDt) + { + dynamicDt = candidateDt; + } + + if (angularSpeed > 0.001) + { + pose.Rotation = pose.Rotation * Quatd(angularVelocity, angularSpeed * dynamicDt); + } + + pose.Translation += poseState.LinearVelocity * dynamicDt; + + return pose; +} + + +//// SensorStateReader + +SensorStateReader::SensorStateReader() : + Updater(NULL), + LastLatWarnTime(0.) +{ +} + +void SensorStateReader::SetUpdater(const CombinedSharedStateUpdater* updater) +{ + Updater = updater; +} + +void SensorStateReader::RecenterPose() +{ + if (!Updater) + { + return; + } + + /* + This resets position to center in x, y, z, and resets yaw to center. + Other rotation components are not affected. + */ + + const LocklessSensorState lstate = Updater->SharedSensorState.GetState(); + + Posed worldFromCpf = lstate.WorldFromImu.ThePose * lstate.ImuFromCpf; + double hmdYaw, hmdPitch, hmdRoll; + worldFromCpf.Rotation.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&hmdYaw, &hmdPitch, &hmdRoll); + + Posed worldFromCentered(Quatd(Axis_Y, hmdYaw), worldFromCpf.Translation); + + CenteredFromWorld = worldFromCentered.Inverted(); +} + +bool SensorStateReader::GetSensorStateAtTime(double absoluteTime, TrackingState& ss) const +{ + if (!Updater) + { + ss.StatusFlags = 0; + return false; + } + + const LocklessSensorState lstate = Updater->SharedSensorState.GetState(); + + // Update time + ss.HeadPose.TimeInSeconds = absoluteTime; + + // Update the status flags + ss.StatusFlags = lstate.StatusFlags; + // If no hardware is connected, override the tracking flags + if (0 == (ss.StatusFlags & Status_HMDConnected)) + { + ss.StatusFlags &= ~Status_TrackingMask; + } + if (0 == (ss.StatusFlags & Status_PositionConnected)) + { + ss.StatusFlags &= ~(Status_PositionTracked | Status_CameraPoseTracked); + } + + // If tracking info is invalid, + if (0 == (ss.StatusFlags & Status_TrackingMask)) + { + return false; + } + + // Delta time from the last available data + double pdt = absoluteTime - lstate.WorldFromImu.TimeInSeconds; + static const double maxPdt = 0.1; + + // If delta went negative due to synchronization problems between processes or just a lag spike, + if (pdt < 0.) + { + pdt = 0.; + } + else if (pdt > maxPdt) + { + if (LastLatWarnTime != lstate.WorldFromImu.TimeInSeconds) + { + LastLatWarnTime = lstate.WorldFromImu.TimeInSeconds; + LogText("[SensorStateReader] Prediction interval too high: %f s, clamping at %f s\n", pdt, maxPdt); + } + pdt = maxPdt; + } + + ss.HeadPose = PoseStatef(lstate.WorldFromImu); + // Do prediction logic and ImuFromCpf transformation + ss.HeadPose.ThePose = Posef(CenteredFromWorld * calcPredictedPose(lstate.WorldFromImu, pdt) * lstate.ImuFromCpf); + + ss.CameraPose = Posef(CenteredFromWorld * lstate.WorldFromCamera); + + Posed worldFromLeveledCamera = Posed(Quatd(), lstate.WorldFromCamera.Translation); + ss.LeveledCameraPose = Posef(CenteredFromWorld * worldFromLeveledCamera); + + ss.RawSensorData = lstate.RawSensorData; + ss.LastVisionProcessingTime = lstate.LastVisionProcessingTime; + ss.LastVisionFrameLatency = lstate.LastVisionFrameLatency; + + return true; +} + +bool SensorStateReader::GetPoseAtTime(double absoluteTime, Posef& transform) const +{ + TrackingState ss; + if (!GetSensorStateAtTime(absoluteTime, ss)) + { + return false; + } + + transform = ss.HeadPose.ThePose; + + return true; +} + +uint32_t SensorStateReader::GetStatus() const +{ + if (!Updater) + { + return 0; + } + + const LocklessSensorState lstate = Updater->SharedSensorState.GetState(); + + // If invalid, + if (0 == (lstate.StatusFlags & Status_TrackingMask)) + { + // Return 0 indicating no orientation nor position tracking + return 0; + } + + return lstate.StatusFlags; +} + +}} // namespace OVR::Tracking diff --git a/LibOVR/Src/Tracking/Tracking_SensorStateReader.h b/LibOVR/Src/Tracking/Tracking_SensorStateReader.h new file mode 100644 index 0000000..c2922c8 --- /dev/null +++ b/LibOVR/Src/Tracking/Tracking_SensorStateReader.h @@ -0,0 +1,87 @@ +/************************************************************************************ + +Filename : Tracking_SensorStateReader.h +Content : Separate reader component that is able to recover sensor pose +Created : June 4, 2014 +Authors : Chris Taylor + +Copyright : Copyright 2014 Oculus VR, LLC All Rights reserved. + +Licensed under the Oculus VR Rift SDK License Version 3.2 (the "License"); +you may not use the Oculus VR Rift SDK except in compliance with the License, +which is provided at the time of installation or download, or which +otherwise accompanies this software in either electronic or hard copy form. + +You may obtain a copy of the License at + +http://www.oculusvr.com/licenses/LICENSE-3.2 + +Unless required by applicable law or agreed to in writing, the Oculus VR SDK +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 Tracking_SensorStateReader_h +#define Tracking_SensorStateReader_h + +#include "../Kernel/OVR_Lockless.h" +#include "Tracking_SensorState.h" + +#include "../OVR_Profile.h" + +namespace OVR { namespace Tracking { + + +//----------------------------------------------------------------------------- +// SensorStateReader + +// User interface to retrieve pose from the sensor fusion subsystem +class SensorStateReader : public NewOverrideBase +{ +protected: + const CombinedSharedStateUpdater *Updater; + + + // Last latency warning time + mutable double LastLatWarnTime; + + // Transform from real-world coordinates to centered coordinates + Posed CenteredFromWorld; + +public: + SensorStateReader(); + + // Initialize the updater + void SetUpdater(const CombinedSharedStateUpdater *updater); + + // Re-centers on the current yaw (optionally pitch) and translation + void RecenterPose(); + + // Get the full dynamical system state of the CPF, which includes velocities and accelerations, + // predicted at a specified absolute point in time. + bool GetSensorStateAtTime(double absoluteTime, Tracking::TrackingState& state) const; + + // Get the predicted pose (orientation, position) of the center pupil frame (CPF) at a specific point in time. + bool GetPoseAtTime(double absoluteTime, Posef& transform) const; + + // Get the sensor status (same as GetSensorStateAtTime(...).Status) + uint32_t GetStatus() const; + + const Posed getCenteredFromWorld() + { + return CenteredFromWorld; + } + + void setCenteredFromWorld(const Posed _CenteredFromWorld) + { + CenteredFromWorld = _CenteredFromWorld; + } +}; + + +}} // namespace OVR::Tracking + +#endif // Tracking_SensorStateReader_h |