mii/rust-openvr
1
0
Fork 0
mirror of https://github.com/mii443/rust-openvr.git synced 2025-09-01 15:09:21 +00:00
Code Issues Packages Projects Releases Wiki Activity

added linux makefile

Browse Source
This commit is contained in:
Colin Sherratt
2014-05-26 03:42:23 -04:00
parent cb5a46cd58
commit 6b733ba3a3
141 changed files with 53997 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

582
modules/oculus_sdk_linux/LibOVR/Src/OVR_SensorFusion.h Normal file
View File

@ -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