diff options
Diffstat (limited to 'LibOVR/Src/OVR_SensorFusion.h')
| -rw-r--r-- | LibOVR/Src/OVR_SensorFusion.h | 582 |
1 files changed, 582 insertions, 0 deletions
diff --git a/LibOVR/Src/OVR_SensorFusion.h b/LibOVR/Src/OVR_SensorFusion.h new file mode 100644 index 0000000..2a17920 --- /dev/null +++ b/LibOVR/Src/OVR_SensorFusion.h @@ -0,0 +1,582 @@ +/************************************************************************************ + +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 |