From e490c3c7f7bb5461cfa78a214827aa534fb43a3e Mon Sep 17 00:00:00 2001
From: Sven Gothel <sgothel@jausoft.com>
Date: Sat, 21 Mar 2015 21:19:34 +0100
Subject: Bump OculusVR RIFT SDK to 0.4.4
---
LibOVR/Src/OVR_SensorFusion.h | 582 ------------------------------------------
1 file changed, 582 deletions(-)
delete mode 100644 LibOVR/Src/OVR_SensorFusion.h
(limited to 'LibOVR/Src/OVR_SensorFusion.h')
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
--
cgit v1.2.3