diff options
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, 0 insertions, 639 deletions
diff --git a/LibOVR/Src/Tracking/Tracking_PoseState.h b/LibOVR/Src/Tracking/Tracking_PoseState.h deleted file mode 100644 index 19e2ee2..0000000 --- a/LibOVR/Src/Tracking/Tracking_PoseState.h +++ /dev/null @@ -1,133 +0,0 @@ -/************************************************************************************ - -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 deleted file mode 100644 index e13c3ef..0000000 --- a/LibOVR/Src/Tracking/Tracking_SensorState.h +++ /dev/null @@ -1,212 +0,0 @@ -/************************************************************************************ - -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 deleted file mode 100644 index a506b26..0000000 --- a/LibOVR/Src/Tracking/Tracking_SensorStateReader.cpp +++ /dev/null @@ -1,207 +0,0 @@ -/************************************************************************************ - -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 deleted file mode 100644 index c2922c8..0000000 --- a/LibOVR/Src/Tracking/Tracking_SensorStateReader.h +++ /dev/null @@ -1,87 +0,0 @@ -/************************************************************************************ - -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 |