diff options
Diffstat (limited to 'LibOVR/Src/OVR_SensorFusion.h')
| -rw-r--r-- | LibOVR/Src/OVR_SensorFusion.h | 582 |
1 files changed, 0 insertions, 582 deletions
diff --git a/LibOVR/Src/OVR_SensorFusion.h b/LibOVR/Src/OVR_SensorFusion.h deleted file mode 100644 index 2a17920..0000000 --- a/LibOVR/Src/OVR_SensorFusion.h +++ /dev/null @@ -1,582 +0,0 @@ -/************************************************************************************ - -PublicHeader: OVR.h -Filename : OVR_SensorFusion.h -Content : Methods that determine head orientation from sensor data over time -Created : October 9, 2012 -Authors : Michael Antonov, Steve LaValle, Dov Katz, Max Katsev, Dan Gierl - -Copyright : Copyright 2014 Oculus VR, Inc. All Rights reserved. - -Licensed under the Oculus VR Rift SDK License Version 3.1 (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.1 - -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 OVR_SensorFusion_h -#define OVR_SensorFusion_h - -#include "OVR_Device.h" -#include "OVR_SensorFilter.h" -#include "Kernel/OVR_Timer.h" -#include "Kernel/OVR_Threads.h" -#include "Kernel/OVR_Lockless.h" - -// CAPI forward declarations. -typedef struct ovrPoseStatef_ ovrPoseStatef; -typedef struct ovrSensorState_ ovrSensorState; - -namespace OVR { - -struct HmdRenderInfo; - -//------------------------------------------------------------------------------------- -// ***** Sensor State - -// These values are reported as compatible with C API. - - -// 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<Transform<T> >::Type CompatibleType; - - PoseState() : TimeInSeconds(0.0) { } - // float <-> double conversion constructor. - explicit PoseState(const PoseState<typename Math<T>::OtherFloatType> &src) - : Pose(src.Pose), - 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) - : Pose(src.Pose), - 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.Pose = Pose; - result.AngularVelocity = AngularVelocity; - result.LinearVelocity = LinearVelocity; - result.AngularAcceleration = AngularAcceleration; - result.LinearAcceleration = LinearAcceleration; - result.TimeInSeconds = TimeInSeconds; - return result; - } - - - Transform<T> Pose; - 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); -}; - - - -// External API returns pose as float, but uses doubles internally for quaternion precision. -typedef PoseState<float> PoseStatef; -typedef PoseState<double> PoseStated; - - -//------------------------------------------------------------------------------------- -// ***** Sensor State - - -// Bit flags describing the current status of sensor tracking. -enum StatusBits -{ - Status_OrientationTracked = 0x0001, // Orientation is currently tracked (connected and in use). - Status_PositionTracked = 0x0002, // Position is currently tracked (false if out of range). - Status_PositionConnected = 0x0020, // Position tracking HW is conceded. - // Status_HMDConnected = 0x0080 // HMD Display is available & connected. -}; - - -// Full state of of the sensor reported by GetSensorState() at a given absolute time. -class SensorState -{ -public: - SensorState() : Temperature(0), StatusFlags(0) { } - - // C-interop support - SensorState(const ovrSensorState& s); - operator ovrSensorState () const; - - // Pose state at the time that SensorState was requested. - PoseStatef Predicted; - // Actual recorded pose configuration based on sensor sample at a - // moment closest to the requested time. - PoseStatef Recorded; - - // Calibrated magnetometer reading, in Gauss, at sample time. - Vector3f Magnetometer; - // Sensor temperature reading, in degrees Celsius, at sample time. - float Temperature; - // Sensor status described by ovrStatusBits. - unsigned int StatusFlags; -}; - - - -//------------------------------------------------------------------------------------- - -class VisionHandler -{ -public: - virtual void OnVisionSuccess(const Transform<double>& cameraFromImu, UInt32 exposureCounter) = 0; - virtual void OnVisionPreviousFrame(const Transform<double>& cameraFromImu) = 0; - virtual void OnVisionFailure() = 0; - - // Get a configuration that represents the change over a short time interval - virtual Transform<double> GetVisionPrediction(UInt32 exposureCounter) = 0; -}; - -//------------------------------------------------------------------------------------- -// ***** SensorFusion - -// SensorFusion class accumulates Sensor notification messages to keep track of -// orientation, which involves integrating the gyro and doing correction with gravity. -// Magnetometer based yaw drift correction is also supported; it is usually enabled -// automatically based on loaded magnetometer configuration. -// Orientation is reported as a quaternion, from which users can obtain either the -// rotation matrix or Euler angles. -// -// The class can operate in two ways: -// - By user manually passing MessageBodyFrame messages to the OnMessage() function. -// - By attaching SensorFusion to a SensorDevice, in which case it will -// automatically handle notifications from that device. - - -class SensorFusion : public NewOverrideBase, public VisionHandler -{ - friend class SensorFusionDebug; - - enum - { - MagMaxReferences = 1000 - }; - -public: - - // ------------------------------------------------------------------------------- - // Critical components for tiny API - - SensorFusion(SensorDevice* sensor = 0); - ~SensorFusion(); - - // Attaches this SensorFusion to the IMU sensor device, from which it will receive - // notification messages. If a sensor is attached, manual message notification - // is not necessary. Calling this function also resets SensorFusion state. - bool AttachToSensor(SensorDevice* sensor); - - // Returns true if this Sensor fusion object is attached to the IMU. - bool IsAttachedToSensor() const; - - // Sets up head-and-neck model and device-to-pupil dimensions from the user's profile and the HMD stats. - // This copes elegantly if profile is NULL. - void SetUserHeadDimensions(Profile const &profile, HmdRenderInfo const &hmdRenderInfo); - - // Get the predicted pose (orientation, position) of the center pupil frame (CPF) at a specific point in time. - Transformf GetPoseAtTime(double absoluteTime) const; - - // Get the full dynamical system state of the CPF, which includes velocities and accelerations, - // predicted at a specified absolute point in time. - SensorState GetSensorStateAtTime(double absoluteTime) const; - - // Get the sensor status (same as GetSensorStateAtTime(...).Status) - unsigned int GetStatus() const; - - // End tiny API components - // ------------------------------------------------------------------------------- - - // Resets the current orientation. - void Reset (); - - // Configuration - void EnableMotionTracking(bool enable = true) { MotionTrackingEnabled = enable; } - bool IsMotionTrackingEnabled() const { return MotionTrackingEnabled; } - - // Accelerometer/Gravity Correction Control - // Enables/disables gravity correction (on by default). - void SetGravityEnabled (bool enableGravity); - bool IsGravityEnabled () const; - - // Vision Position and Orientation Configuration - // ----------------------------------------------- - bool IsVisionPositionEnabled () const; - void SetVisionPositionEnabled (bool enableVisionPosition); - - // compensates for a tilted camera - void SetCameraTiltCorrectionEnabled(bool enable); - bool IsCameraTiltCorrectionEnabled () const; - - // Message Handling Logic - // ----------------------------------------------- - // Notifies SensorFusion object about a new BodyFrame - // message from a sensor. - // Should be called by user if not attached to sensor. - void OnMessage (const MessageBodyFrame& msg); - - - // Interaction with vision - // ----------------------------------------------- - // Handle observation from vision system (orientation, position, time) - virtual void OnVisionSuccess(const Transform<double>& cameraFromImu, UInt32 exposureCounter); - - virtual void OnVisionPreviousFrame(const Transform<double>& cameraFromImu); - virtual void OnVisionFailure(); - // Get a configuration that represents the change over a short time interval - virtual Transform<double> GetVisionPrediction(UInt32 exposureCounter); - - double GetTime () const; - double GetVisionLatency () const; - - - // Detailed head dimension control - // ----------------------------------------------- - // These are now deprecated in favour of SetUserHeadDimensions() - Vector3f GetHeadModel() const; - void SetHeadModel(const Vector3f &headModel, bool resetNeckPivot = true ); - float GetCenterPupilDepth() const; - void SetCenterPupilDepth(float centerPupilDepth); - - - // Magnetometer and Yaw Drift Section: - // --------------------------------------- - - // Enables/disables magnetometer based yaw drift correction. - // Must also have mag calibration data for this correction to work. - void SetYawCorrectionEnabled(bool enable); - // Determines if yaw correction is enabled. - bool IsYawCorrectionEnabled () const; - - // Clear the reference points associating - // mag readings with orientations - void ClearMagReferences (); - - // Sets the focus filter direction to the current HMD direction - void SetFocusDirection(); - // Sets the focus filter to a direction in the body frame. Once set, a complementary filter - // will very slowly drag the world to keep the direction of the HMD within the FOV of the focus - void SetFocusDirection(Vector3d direction); - // Sets the FOV (in radians) of the focus. When the yaw difference between the HMD's current pose - // and the focus is smaller than the FOV, the complementary filter does not act. - void SetFocusFOV(double rads); - // Turns off the focus filter (equivalent to setting the focus to 0 - void ClearFocus(); - -private: - - // ----------------------------------------------- - - class BodyFrameHandler : public NewOverrideBase, public MessageHandler - { - SensorFusion* pFusion; - public: - BodyFrameHandler(SensorFusion* fusion) - : pFusion(fusion) {} - - ~BodyFrameHandler(); - - virtual void OnMessage(const Message& msg); - virtual bool SupportsMessageType(MessageType type) const; - }; - - - // ----------------------------------------------- - - // State version stored in lockless updater "queue" and used for - // prediction by GetPoseAtTime/GetSensorStateAtTime - struct LocklessState - { - PoseState<double> State; - float Temperature; - Vector3d Magnetometer; - unsigned int StatusFlags; - - LocklessState() : Temperature(0.0), StatusFlags(0) { }; - }; - - - // ----------------------------------------------- - - // Entry describing the state of the headset at the time of an exposure as reported by the DK2 board. - // This is used in combination with the vision data for - // incremental tracking based on IMU change and for drift correction - struct ExposureRecord - { - UInt32 ExposureCounter; - double ExposureTime; - // State of the headset at the time of exposure - PoseState<double> WorldFromImu; - // Change in state since the last exposure based on IMU data only - PoseState<double> ImuOnlyDelta; - // Did we have tracking for the entire interval between exposures - bool VisionTrackingAvailable; - - ExposureRecord() : ExposureCounter(0), ExposureTime(0.0), VisionTrackingAvailable(true) { } - ExposureRecord(UInt32 exposureCounter, double exposureTime, - const PoseState<double>& worldFromImu, const PoseState<double>& imuOnlyDelta) - : ExposureCounter(exposureCounter), ExposureTime(exposureTime), - WorldFromImu(worldFromImu), ImuOnlyDelta(imuOnlyDelta), VisionTrackingAvailable(true) { } - }; - - // ----------------------------------------------- - - // Entry describing the magnetometer reference point - // Used for mag yaw correction - struct MagReferencePoint - { - Vector3d InImuFrame; - Transformd WorldFromImu; - int Score; - - MagReferencePoint() { } - MagReferencePoint(const Vector3d& inImuFrame, const Transformd& worldFromImu, int score) - : InImuFrame(inImuFrame), WorldFromImu(worldFromImu), Score(score) { } - }; - - // ----------------------------------------------- - - // The phase of the head as estimated by sensor fusion - PoseState<double> WorldFromImu; - - // State that can be read without any locks, so that high priority rendering thread - // doesn't have to worry about being blocked by a sensor/vision threads that got preempted. - LocklessUpdater<LocklessState> UpdatedState; - - // The pose we got from Vision, augmented with velocity information from numerical derivatives - PoseState<double> CameraFromImu; - // Difference between the vision and sensor fusion poses at the time of last exposure adjusted - // by all the corrections applied since then - // NB: this one is unlike all the other poses/transforms we use, since it's a difference - // between 2 WorldFromImu transforms, but is stored in the world frame, not the IMU frame - // (see computeVisionError() for details) - // For composition purposes it should be considered a WorldFromWorld transform, where the left - // side comes from vision and the right - from sensor fusion - PoseState<double> VisionError; - // Past exposure records between the last update from vision and now - // (should only be one record unless vision latency is high) - CircularBuffer<ExposureRecord> ExposureRecordHistory; - // ExposureRecord that corresponds to the last pose we got from vision - ExposureRecord LastVisionExposureRecord; - // Incomplete ExposureRecord that will go into the history buffer when - // the new MessageExposureFrame is received - ExposureRecord NextExposureRecord; - // Timings of the previous exposure, used to populate ExposureRecordHistory - MessageExposureFrame LastMessageExposureFrame; - // Time of the last vision update - double LastVisionAbsoluteTime; - - unsigned int Stage; - BodyFrameHandler *pHandler; - - Vector3d FocusDirection; - double FocusFOV; - - SensorFilterBodyFrame FAccelInImuFrame, FAccelInCameraFrame; - SensorFilterd FAngV; - - Vector3d AccelOffset; - - bool EnableGravity; - - bool EnableYawCorrection; - bool MagCalibrated; - Array<MagReferencePoint> MagRefs; - int MagRefIdx; - Quatd MagCorrectionIntegralTerm; - - bool EnableCameraTiltCorrection; - // Describes the pose of the camera in the world coordinate system - Transformd WorldFromCamera; - double WorldFromCameraConfidence; - - bool MotionTrackingEnabled; - bool VisionPositionEnabled; - - // This is a signed distance, but positive because Z increases looking inward. - // This is expressed relative to the IMU in the HMD and corresponds to the location - // of the cyclopean virtual camera focal point if both the physical and virtual - // worlds are isometrically mapped onto each other. -Steve - float CenterPupilDepth; - // Describes the position of the user eyes relative to the IMU - Transformd ImuFromCpf; - // Position of the center of the screen relative to the IMU (loaded from the headset firmware) - Transformd ImuFromScreen; - // Built-in head model for faking position using orientation only - Transformd CpfFromNeck; - // Last known base of the neck pose used for head model computations - Transformd WorldFromNeck; - - //--------------------------------------------- - - // Internal handler for messages - // bypasses error checking. - void handleMessage(const MessageBodyFrame& msg); - void handleExposure(const MessageExposureFrame& msg); - - // Compute the difference between vision and sensor fusion data - PoseStated computeVisionError(); - // Apply headset yaw correction from magnetometer - // for models without camera or when camera isn't available - void applyMagYawCorrection(Vector3d mag, double deltaT); - // Apply headset tilt correction from the accelerometer - void applyTiltCorrection(double deltaT); - // Apply headset yaw correction from the camera - void applyVisionYawCorrection(double deltaT); - // Apply headset position correction from the camera - void applyPositionCorrection(double deltaT); - // Apply camera tilt correction from the accelerometer - void applyCameraTiltCorrection(Vector3d accel, double deltaT); - // Apply camera focus correction - void applyFocusCorrection(double deltaT); - - // If you have a known-good pose, this sets the neck pivot position. - void setNeckPivotFromPose ( Transformd const &pose ); -}; - - - -//------------------------------------------------------------------------------------- -// ***** SensorFusion - Inlines - -inline bool SensorFusion::IsAttachedToSensor() const -{ - return pHandler->IsHandlerInstalled(); -} - -inline void SensorFusion::SetGravityEnabled(bool enableGravity) -{ - EnableGravity = enableGravity; -} - -inline bool SensorFusion::IsGravityEnabled() const -{ - return EnableGravity; -} - -inline void SensorFusion::SetYawCorrectionEnabled(bool enable) -{ - EnableYawCorrection = enable; -} - -inline bool SensorFusion::IsYawCorrectionEnabled() const -{ - return EnableYawCorrection; -} - -inline bool SensorFusion::IsVisionPositionEnabled() const -{ - return VisionPositionEnabled; -} - -inline void SensorFusion::SetVisionPositionEnabled(bool enableVisionPosition) -{ - VisionPositionEnabled = enableVisionPosition; -} - -inline void SensorFusion::SetCameraTiltCorrectionEnabled(bool enable) -{ - EnableCameraTiltCorrection = enable; -} - -inline bool SensorFusion::IsCameraTiltCorrectionEnabled() const -{ - return EnableCameraTiltCorrection; -} - -inline double SensorFusion::GetVisionLatency() const -{ - return LastVisionAbsoluteTime - CameraFromImu.TimeInSeconds; -} - -inline double SensorFusion::GetTime() const -{ - return Timer::GetSeconds(); -} - -inline SensorFusion::BodyFrameHandler::~BodyFrameHandler() -{ - RemoveHandlerFromDevices(); -} - -inline bool SensorFusion::BodyFrameHandler::SupportsMessageType(MessageType type) const -{ - return (type == Message_BodyFrame || type == Message_ExposureFrame); -} - - -//------------------------------------------------------------------------------------- -// ***** PoseState - Inlines - -// Stores and integrates gyro angular velocity reading for a given time step. -template<class T> -void PoseState<T>::StoreAndIntegrateGyro(Vector3d angVel, double dt) -{ - AngularVelocity = angVel; - double angle = angVel.Length() * dt; - if (angle > 0) - Pose.Rotation = Pose.Rotation * Quatd(angVel, angle); -} - -template<class T> -void PoseState<T>::StoreAndIntegrateAccelerometer(Vector3d linearAccel, double dt) -{ - LinearAcceleration = linearAccel; - Pose.Translation += LinearVelocity * dt + LinearAcceleration * (dt * dt * 0.5); - LinearVelocity += LinearAcceleration * dt; -} - -// Performs integration of state by adding next state delta to it -// to produce a combined state change -template<class T> -void PoseState<T>::AdvanceByDelta(const PoseState<T>& delta) -{ - Pose.Rotation = Pose.Rotation * delta.Pose.Rotation; - Pose.Translation += delta.Pose.Translation + LinearVelocity * delta.TimeInSeconds; - LinearVelocity += delta.LinearVelocity; - TimeInSeconds += delta.TimeInSeconds; -} - -} // namespace OVR -#endif |