Add OculusSDK 0.3.2 Linux Source Code w/o Samples, docs or binaries (libs or tools)

1 files changed, 904 insertions, 0 deletions

diff --git a/LibOVR/Src/OVR_SensorFusion.cpp b/LibOVR/Src/OVR_SensorFusion.cpp
new file mode 100644
index 0000000..5c21178
--- /dev/null
+++ b/LibOVR/Src/OVR_SensorFusion.cpp
@@ -0,0 +1,904 @@
+/************************************************************************************
+
+Filename    :   OVR_SensorFusion.cpp
+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.
+
+*************************************************************************************/
+
+#include "OVR_SensorFusion.h"
+#include "Kernel/OVR_Log.h"
+#include "Kernel/OVR_System.h"
+#include "OVR_JSON.h"
+#include "OVR_Profile.h"
+#include "OVR_Stereo.h"
+#include "OVR_Recording.h"
+
+// Temporary for debugging
+bool Global_Flag_1 = true;
+
+//Convenient global variables to temporarily extract this data.
+float  TPH_CameraPoseOrientationWxyz[4];
+double TPH_CameraPoseConfidence;
+double TPH_CameraPoseConfidenceThresholdOverrideIfNonZero = 0;
+bool   TPH_IsPositionTracked = false;
+
+
+namespace OVR {
+
+const Transformd DefaultWorldFromCamera(Quatd(), Vector3d(0, 0, -1));
+
+//-------------------------------------------------------------------------------------
+// ***** Sensor Fusion
+
+SensorFusion::SensorFusion(SensorDevice* sensor)
+  : ExposureRecordHistory(100), LastMessageExposureFrame(NULL),
+    FocusDirection(Vector3d(0, 0, 0)), FocusFOV(0.0),
+    FAccelInImuFrame(1000), FAccelInCameraFrame(1000), FAngV(20),
+    EnableGravity(true), EnableYawCorrection(true), MagCalibrated(false),
+    EnableCameraTiltCorrection(true),
+    MotionTrackingEnabled(true), VisionPositionEnabled(true),
+    CenterPupilDepth(0.0)
+{
+   pHandler = new BodyFrameHandler(this);
+
+   // And the clock is running...
+   LogText("*** SensorFusion Startup: TimeSeconds = %f\n", Timer::GetSeconds());
+
+   if (sensor)
+       AttachToSensor(sensor);
+   
+   Reset();
+}
+
+SensorFusion::~SensorFusion()
+{   
+    delete(pHandler);
+}
+
+bool SensorFusion::AttachToSensor(SensorDevice* sensor)
+{
+    pHandler->RemoveHandlerFromDevices();
+    Reset();
+
+    if (sensor != NULL)
+    {
+        // cache mag calibration state
+        MagCalibrated = sensor->IsMagCalibrated();
+        
+        // Load IMU position
+        Array<PositionCalibrationReport> reports;
+        bool result = sensor->GetAllPositionCalibrationReports(&reports);
+        if (result)
+        {
+            PositionCalibrationReport imu = reports[reports.GetSize() - 1];
+            OVR_ASSERT(imu.PositionType == PositionCalibrationReport::PositionType_IMU);
+            // convert from vision to the world frame
+            // TBD convert rotation as necessary?
+            imu.Position.x *= -1.0;
+            imu.Position.z *= -1.0;
+
+            ImuFromScreen = Transformd(Quatd(imu.Normal, imu.Angle), imu.Position).Inverted();
+
+            Recording::GetRecorder().RecordLedPositions(reports);
+            Recording::GetRecorder().RecordDeviceIfcVersion(sensor->GetDeviceInterfaceVersion());
+		}
+        
+        // Repopulate CPFOrigin
+        SetCenterPupilDepth(CenterPupilDepth);
+
+        // Subscribe to sensor updates
+        sensor->AddMessageHandler(pHandler);
+
+        // Initialize the sensor state
+        // TBD: This is a hack to avoid a race condition if sensor status is checked immediately 
+        // after sensor creation but before any data has flowed through.  We should probably
+        // not depend strictly on data flow to determine capabilities like orientation and position
+        // tracking, or else use some sort of synchronous method to wait for data
+        LocklessState init;
+        init.StatusFlags = Status_OrientationTracked;   
+        UpdatedState.SetState(init);
+    }
+
+    return true;
+}
+
+// Resets the current orientation
+void SensorFusion::Reset()
+{
+    Lock::Locker lockScope(pHandler->GetHandlerLock());
+
+    UpdatedState.SetState(LocklessState());
+    WorldFromImu                        = PoseState<double>();
+    WorldFromImu.Pose                   = ImuFromCpf.Inverted(); // place CPF at the origin, not the IMU
+    CameraFromImu                       = PoseState<double>();
+    VisionError                         = PoseState<double>();
+    WorldFromCamera                     = DefaultWorldFromCamera;
+    WorldFromCameraConfidence           = -1;
+
+    ExposureRecordHistory.Clear();
+    NextExposureRecord                  = ExposureRecord();
+    LastMessageExposureFrame            = MessageExposureFrame(NULL);
+    LastVisionAbsoluteTime              = 0;
+    Stage                               = 0;
+    
+    MagRefs.Clear();
+    MagRefIdx                           = -1;
+    MagCorrectionIntegralTerm           = Quatd();
+    AccelOffset                         = Vector3d();
+
+    FAccelInCameraFrame.Clear();
+    FAccelInImuFrame.Clear();
+    FAngV.Clear();
+
+    setNeckPivotFromPose ( WorldFromImu.Pose );
+}
+
+//-------------------------------------------------------------------------------------
+//  Vision & message processing
+
+void SensorFusion::OnVisionFailure()
+{
+    // do nothing
+    Recording::GetRecorder().RecordVisionSuccess(false);
+}
+
+void SensorFusion::OnVisionPreviousFrame(const Transform<double>& cameraFromImu)
+{
+    // simply save the observation for use in the next OnVisionSuccess call;
+    // this should not have unintended side-effects for position filtering, 
+    // since the vision time is not updated and the system keeps thinking we don't have vision yet
+    CameraFromImu.Pose = cameraFromImu;
+}
+
+void SensorFusion::OnVisionSuccess(const Transform<double>& cameraFromImu, UInt32 exposureCounter)
+{
+    Lock::Locker lockScope(pHandler->GetHandlerLock());
+
+    Recording::GetRecorder().RecordVisionSuccess(true);
+
+    LastVisionAbsoluteTime = GetTime();
+
+    // ********* LastVisionExposureRecord *********
+
+    // Skip old data and use the record that matches the exposure counter
+    while (!ExposureRecordHistory.IsEmpty() &&
+           (ExposureRecordHistory.PeekFront().ExposureCounter <= exposureCounter))
+    {
+        LastVisionExposureRecord = ExposureRecordHistory.PopFront();
+    }
+
+    // Use current values if we don't have historical data
+    // Right now, this will happen if we get first frame after prediction failure,
+    // and this exposure wasn't in the buffer.  (TBD: Unlikely.. unless IMU message wasn't sent?)
+    if (LastVisionExposureRecord.ExposureCounter != exposureCounter)
+        LastVisionExposureRecord = ExposureRecord(exposureCounter, GetTime(), WorldFromImu, PoseState<double>());
+
+    // ********* CameraFromImu *********
+    
+    // This is stored in the camera frame, so need to be careful when combining with the IMU data,
+    // which is in the world frame
+
+    Transformd cameraFromImuPrev = CameraFromImu.Pose;
+    CameraFromImu.Pose = cameraFromImu; 
+    CameraFromImu.TimeInSeconds = LastVisionExposureRecord.ExposureTime;
+
+    // Check LastVisionExposureRecord.Delta.TimeInSeconds to avoid divide by zero, which we could (rarely)
+    // get if we didn't have exposures delta for history (skipped exposure counters
+    // due to video mode change that stalls USB, etc).
+    if (LastVisionExposureRecord.ImuOnlyDelta.TimeInSeconds > 0.001)
+    {
+        Vector3d visionVelocityInImuFrame = (cameraFromImu.Translation - cameraFromImuPrev.Translation) /
+                                            LastVisionExposureRecord.ImuOnlyDelta.TimeInSeconds;
+        // Use the accel data to estimate the velocity at the exposure time
+        // (as opposed to the average velocity between exposures)
+        Vector3d imuVelocityInWorldFrame = LastVisionExposureRecord.ImuOnlyDelta.LinearVelocity -
+            LastVisionExposureRecord.ImuOnlyDelta.Pose.Translation / LastVisionExposureRecord.ImuOnlyDelta.TimeInSeconds;
+        CameraFromImu.LinearVelocity = visionVelocityInImuFrame + 
+            WorldFromCamera.Inverted().Rotate(imuVelocityInWorldFrame);
+    }
+    else
+    {
+        CameraFromImu.LinearVelocity = Vector3d(0,0,0);
+    }
+}
+
+PoseStated SensorFusion::computeVisionError()
+{
+    PoseStated worldFromImuVision = WorldFromCamera * CameraFromImu;
+    // Here we need to compute the difference between worldFromImuVision and WorldFromImu.
+    // However this difference needs to be represented in the World frame, not IMU frame.
+    // Therefore the computation is different from simply worldFromImuVision.Pose * WorldFromImu.Pose.Inverted().
+    PoseStated err;
+    err.Pose.Rotation = worldFromImuVision.Pose.Rotation * 
+        LastVisionExposureRecord.WorldFromImu.Pose.Rotation.Inverted();
+    err.Pose.Translation = worldFromImuVision.Pose.Translation - 
+        LastVisionExposureRecord.WorldFromImu.Pose.Translation;
+    err.LinearVelocity = worldFromImuVision.LinearVelocity - 
+        LastVisionExposureRecord.WorldFromImu.LinearVelocity;
+    return err;
+}
+
+Transform<double> SensorFusion::GetVisionPrediction(UInt32 exposureCounter)
+{
+    Lock::Locker lockScope(pHandler->GetHandlerLock());
+
+    // Combine the small deltas together
+    // Should only be one iteration, unless we are skipping camera frames
+    ExposureRecord record;
+    PoseState<double> delta = PoseState<double>();
+
+    while (!ExposureRecordHistory.IsEmpty() && 
+           (ExposureRecordHistory.PeekFront().ExposureCounter <= exposureCounter))
+    {
+        record = ExposureRecordHistory.PopFront();
+        delta.AdvanceByDelta(record.ImuOnlyDelta);
+    }
+    // Put the combine exposure record back in the history, for use in HandleVisionSuccess(...)
+    record.ImuOnlyDelta = delta;
+    ExposureRecordHistory.PushFront(record);
+
+    Transformd result;
+    if (record.VisionTrackingAvailable)
+    {
+        // if the tracking is working normally, use the change in the main state (SFusion output)
+        // to compute the prediction
+        result = CameraFromImu.Pose *
+            LastVisionExposureRecord.WorldFromImu.Pose.Inverted() * record.WorldFromImu.Pose;
+    }
+    else
+    {
+        // if we just acquired vision, the main state probably doesn't have the correct position,
+        // so can't rely on it for prediction
+
+        // solution: use the accelerometer and vision velocity to propagate the previous sample forward
+        // (don't forget to transform IMU to the camera frame)
+        result = Transform<double>
+            (
+                CameraFromImu.Pose.Rotation * delta.Pose.Rotation,
+                CameraFromImu.Pose.Translation + CameraFromImu.LinearVelocity * delta.TimeInSeconds +
+                WorldFromCamera.Inverted().Rotate(delta.Pose.Translation)
+            );
+    }
+
+    return result;
+}
+
+void SensorFusion::handleMessage(const MessageBodyFrame& msg)
+{
+    if (msg.Type != Message_BodyFrame || !IsMotionTrackingEnabled())
+        return;
+
+    // Put the sensor readings into convenient local variables
+    Vector3d gyro(msg.RotationRate); 
+    Vector3d accel(msg.Acceleration); 
+    Vector3d mag(msg.MagneticField);
+    double DeltaT = msg.TimeDelta;
+
+    // Keep track of time
+    WorldFromImu.TimeInSeconds = msg.AbsoluteTimeSeconds;
+    // We got an update in the last 60ms and the data is not very old
+    bool visionIsRecent = (GetTime() - LastVisionAbsoluteTime < 0.07) && (GetVisionLatency() < 0.25);
+    Stage++;
+
+    // Insert current sensor data into filter history
+    FAngV.PushBack(gyro);
+    FAccelInImuFrame.Update(accel, DeltaT, Quatd(gyro, gyro.Length() * DeltaT));
+
+    // Process raw inputs
+    // in the future the gravity offset can be calibrated using vision feedback
+    Vector3d accelInWorldFrame = WorldFromImu.Pose.Rotate(accel) - Vector3d(0, 9.8, 0);
+
+    // Recompute the vision error to account for all the corrections and the new data
+    VisionError = computeVisionError();
+
+    // Update headset orientation   
+    WorldFromImu.StoreAndIntegrateGyro(gyro, DeltaT);
+    // Tilt correction based on accelerometer
+    if (EnableGravity)
+        applyTiltCorrection(DeltaT);
+    // Yaw correction based on camera
+    if (EnableYawCorrection && visionIsRecent)
+        applyVisionYawCorrection(DeltaT);
+    // Yaw correction based on magnetometer
+	if (EnableYawCorrection && MagCalibrated) // MagCalibrated is always false for DK2 for now
+		applyMagYawCorrection(mag, DeltaT);
+	// Focus Correction
+	if ((FocusDirection.x != 0.0f || FocusDirection.z != 0.0f) && FocusFOV < Mathf::Pi)
+		applyFocusCorrection(DeltaT);
+
+    // Update camera orientation
+    if (EnableCameraTiltCorrection && visionIsRecent)
+        applyCameraTiltCorrection(accel, DeltaT);
+
+    // The quaternion magnitude may slowly drift due to numerical error,
+    // so it is periodically normalized.
+    if ((Stage & 0xFF) == 0)
+    {
+        WorldFromImu.Pose.Rotation.Normalize();
+        WorldFromCamera.Rotation.Normalize();
+    }
+
+    // Update headset position
+    if (VisionPositionEnabled && visionIsRecent)
+    {
+        // Integrate UMI and velocity here up to a fixed amount of time after vision. 
+        WorldFromImu.StoreAndIntegrateAccelerometer(accelInWorldFrame + AccelOffset, DeltaT);
+        // Position correction based on camera
+        applyPositionCorrection(DeltaT); 
+        // Compute where the neck pivot would be.
+        setNeckPivotFromPose(WorldFromImu.Pose);
+    }
+    else
+    {
+        // Fall back onto internal head model
+        // Use the last-known neck pivot position to figure out the expected IMU position.
+        // (should be the opposite of SensorFusion::setNeckPivotFromPose)
+        WorldFromNeck.Rotation = WorldFromImu.Pose.Rotation;
+        WorldFromImu.Pose = WorldFromNeck * (ImuFromCpf * CpfFromNeck).Inverted();
+
+        // We can't trust velocity past this point.
+        WorldFromImu.LinearVelocity = Vector3d(0,0,0);
+        WorldFromImu.LinearAcceleration = accelInWorldFrame;
+    }
+
+    // Compute the angular acceleration
+    WorldFromImu.AngularAcceleration = (FAngV.GetSize() >= 12 && DeltaT > 0) ? 
+        (FAngV.SavitzkyGolayDerivative12() / DeltaT) : Vector3d();
+
+    // Update the dead reckoning state used for incremental vision tracking
+    NextExposureRecord.ImuOnlyDelta.StoreAndIntegrateGyro(gyro, DeltaT);
+    NextExposureRecord.ImuOnlyDelta.StoreAndIntegrateAccelerometer(accelInWorldFrame, DeltaT);
+    NextExposureRecord.ImuOnlyDelta.TimeInSeconds = WorldFromImu.TimeInSeconds - LastMessageExposureFrame.CameraTimeSeconds;
+    NextExposureRecord.VisionTrackingAvailable &= (VisionPositionEnabled && visionIsRecent);
+
+	Recording::GetRecorder().LogData("sfTimeSeconds", WorldFromImu.TimeInSeconds);
+    Recording::GetRecorder().LogData("sfStage", (double)Stage);
+	Recording::GetRecorder().LogData("sfPose", WorldFromImu.Pose);
+	//Recorder::LogData("sfAngAcc", State.AngularAcceleration);
+	//Recorder::LogData("sfAngVel", State.AngularVelocity);
+	//Recorder::LogData("sfLinAcc", State.LinearAcceleration);
+	//Recorder::LogData("sfLinVel", State.LinearVelocity);
+
+    // Store the lockless state.    
+    LocklessState lstate;
+    lstate.StatusFlags       = Status_OrientationTracked;
+    if (VisionPositionEnabled)
+        lstate.StatusFlags  |= Status_PositionConnected;
+    if (VisionPositionEnabled && visionIsRecent)
+        lstate.StatusFlags  |= Status_PositionTracked;
+
+	//A convenient means to temporarily extract this flag
+	TPH_IsPositionTracked = visionIsRecent;
+
+    lstate.State        = WorldFromImu;
+    lstate.Temperature  = msg.Temperature;
+    lstate.Magnetometer = mag;    
+    UpdatedState.SetState(lstate);
+}
+
+void SensorFusion::handleExposure(const MessageExposureFrame& msg)
+{
+    NextExposureRecord.ExposureCounter = msg.CameraFrameCount;
+    NextExposureRecord.ExposureTime = msg.CameraTimeSeconds;
+    NextExposureRecord.WorldFromImu = WorldFromImu;
+    NextExposureRecord.ImuOnlyDelta.TimeInSeconds = msg.CameraTimeSeconds - LastMessageExposureFrame.CameraTimeSeconds;
+    ExposureRecordHistory.PushBack(NextExposureRecord);
+
+    // Every new exposure starts from zero
+    NextExposureRecord = ExposureRecord(); 
+    LastMessageExposureFrame = msg;
+}
+
+// If you have a known-good pose, this sets the neck pivot position.
+void SensorFusion::setNeckPivotFromPose(Transformd const &worldFromImu)
+{
+    WorldFromNeck = worldFromImu * ImuFromCpf * CpfFromNeck;
+}
+
+// These two functions need to be moved into Quat class
+// Compute a rotation required to transform "from" into "to". 
+Quatd vectorAlignmentRotation(const Vector3d &from, const Vector3d &to)
+{
+    Vector3d axis = from.Cross(to);
+    if (axis.LengthSq() == 0)
+        // this handles both collinear and zero-length input cases
+        return Quatd();
+    double angle = from.Angle(to);
+    return Quatd(axis, angle);
+}
+
+// Compute the part of the quaternion that rotates around Y axis
+Quatd extractYawRotation(const Quatd &error)
+{
+    if (error.y == 0)
+        return Quatd();
+    double phi = atan2(error.w, error.y);
+    double alpha = Mathd::Pi - 2 * phi;
+    return Quatd(Axis_Y, alpha);
+}
+
+void SensorFusion::applyPositionCorrection(double deltaT)
+{
+    // Each component of gainPos is equivalent to a Kalman gain of (sigma_process / sigma_observation)
+	const Vector3d gainPos = Vector3d(10, 10, 8);
+    const Vector3d gainVel = gainPos.EntrywiseMultiply(gainPos) * 0.5;
+    const Vector3d gainAccel = gainVel * 0.5;
+    const double snapThreshold = 0.1; // Large value (previously 0.01, which caused frequent jumping)
+
+    Vector3d correctionPos, correctionVel;
+    if (VisionError.Pose.Translation.LengthSq() > (snapThreshold * snapThreshold) ||
+        !(UpdatedState.GetState().StatusFlags & Status_PositionTracked))
+    {
+        // high error or just reacquired position from vision - apply full correction
+
+        // to know where we are right now, take the vision pose (which is slightly old)
+        // and update it using the imu data since then
+        PoseStated worldFromImuVision = WorldFromCamera * CameraFromImu;
+        for (unsigned int i = 0; i < ExposureRecordHistory.GetSize(); i++)
+            worldFromImuVision.AdvanceByDelta(ExposureRecordHistory.PeekFront(i).ImuOnlyDelta);
+        worldFromImuVision.AdvanceByDelta(NextExposureRecord.ImuOnlyDelta);
+
+        correctionPos = worldFromImuVision.Pose.Translation - WorldFromImu.Pose.Translation;
+        correctionVel = worldFromImuVision.LinearVelocity - WorldFromImu.LinearVelocity;
+        AccelOffset   = Vector3d();
+    }
+    else
+    {
+        correctionPos = VisionError.Pose.Translation.EntrywiseMultiply(gainPos) * deltaT;
+        correctionVel = VisionError.Pose.Translation.EntrywiseMultiply(gainVel) * deltaT;
+        AccelOffset  += VisionError.Pose.Translation.EntrywiseMultiply(gainAccel) * deltaT;
+    }
+
+    WorldFromImu.Pose.Translation += correctionPos;
+    WorldFromImu.LinearVelocity += correctionVel;
+
+    // Update the exposure records so that we don't apply the same correction twice
+    LastVisionExposureRecord.WorldFromImu.Pose.Translation += correctionPos;
+    LastVisionExposureRecord.WorldFromImu.LinearVelocity += correctionVel;
+    for (unsigned int i = 0; i < ExposureRecordHistory.GetSize(); i++)
+    {
+        PoseStated& state = ExposureRecordHistory.PeekBack(i).WorldFromImu;
+        state.Pose.Translation += correctionPos;
+        state.LinearVelocity += correctionVel;
+    }
+}
+
+void SensorFusion::applyVisionYawCorrection(double deltaT)
+{
+    const double gain = 0.25;
+    const double snapThreshold = 0.1;
+
+    Quatd yawError = extractYawRotation(VisionError.Pose.Rotation);
+
+    Quatd correction;
+    if (Alg::Abs(yawError.w) < cos(snapThreshold / 2)) // angle(yawError) > snapThreshold
+        // high error, jump to the vision position
+        correction = yawError;
+    else
+        correction = yawError.Nlerp(Quatd(), gain * deltaT);
+
+    WorldFromImu.Pose.Rotation = correction * WorldFromImu.Pose.Rotation;
+
+    // Update the exposure records so that we don't apply the same correction twice
+    LastVisionExposureRecord.WorldFromImu.Pose.Rotation = correction * LastVisionExposureRecord.WorldFromImu.Pose.Rotation;
+    for (unsigned int i = 0; i < ExposureRecordHistory.GetSize(); i++)
+    {
+        PoseStated& state = ExposureRecordHistory.PeekBack(i).WorldFromImu;
+        state.Pose.Rotation = correction * state.Pose.Rotation;
+    }
+}
+
+void SensorFusion::applyMagYawCorrection(Vector3d mag, double deltaT)
+{
+    const double minMagLengthSq   = Mathd::Tolerance; // need to use a real value to discard very weak fields
+    const double maxMagRefDist    = 0.1;
+    const double maxTiltError     = 0.05;
+    const double proportionalGain = 0.01;
+    const double integralGain     = 0.0005;
+
+    Vector3d magInWorldFrame = WorldFromImu.Pose.Rotate(mag);
+    // verify that the horizontal component is sufficient
+    if (magInWorldFrame.x * magInWorldFrame.x + magInWorldFrame.z * magInWorldFrame.z < minMagLengthSq)
+        return;
+    magInWorldFrame.Normalize();
+
+    // Delete a bad point
+    if (MagRefIdx >= 0 && MagRefs[MagRefIdx].Score < 0)
+    {
+        MagRefs.RemoveAtUnordered(MagRefIdx);
+        MagRefIdx = -1;
+    }
+
+    // Update the reference point if needed
+    if (MagRefIdx < 0 || mag.Distance(MagRefs[MagRefIdx].InImuFrame) > maxMagRefDist)
+    {
+        // Find a new one
+        MagRefIdx = -1;
+        double bestDist = maxMagRefDist;
+        for (unsigned int i = 0; i < MagRefs.GetSize(); i++)
+        {
+            double dist = mag.Distance(MagRefs[i].InImuFrame);
+            if (bestDist > dist)
+            {
+                bestDist = dist;
+                MagRefIdx = i;
+            }
+        }
+
+        // Create one if needed
+        if (MagRefIdx < 0 && MagRefs.GetSize() < MagMaxReferences)
+		{
+            MagRefs.PushBack(MagReferencePoint(mag, WorldFromImu.Pose, 1000));
+		}
+    }
+
+    if (MagRefIdx >= 0)
+    {
+        Vector3d magRefInWorldFrame = MagRefs[MagRefIdx].WorldFromImu.Rotate(MagRefs[MagRefIdx].InImuFrame);
+        magRefInWorldFrame.Normalize();
+
+        // If the vertical angle is wrong, decrease the score and do nothing
+        if (Alg::Abs(magRefInWorldFrame.y - magInWorldFrame.y) > maxTiltError)
+        {
+            MagRefs[MagRefIdx].Score -= 1;
+            return;
+        }
+
+        MagRefs[MagRefIdx].Score += 2;
+#if 0
+        // this doesn't seem to work properly, need to investigate
+        Quatd error = vectorAlignmentRotation(magW, magRefW);
+        Quatd yawError = extractYawRotation(error);
+#else
+        // Correction is computed in the horizontal plane
+        magInWorldFrame.y = magRefInWorldFrame.y = 0;
+        Quatd yawError = vectorAlignmentRotation(magInWorldFrame, magRefInWorldFrame);
+#endif                                                 
+        Quatd correction = yawError.Nlerp(Quatd(), proportionalGain * deltaT) *
+                           MagCorrectionIntegralTerm.Nlerp(Quatd(), deltaT);
+        MagCorrectionIntegralTerm = MagCorrectionIntegralTerm * yawError.Nlerp(Quatd(), integralGain * deltaT);
+
+        WorldFromImu.Pose.Rotation = correction * WorldFromImu.Pose.Rotation;
+    }
+}
+
+void SensorFusion::applyTiltCorrection(double deltaT)
+{
+    const double gain = 0.25;
+    const double snapThreshold = 0.1;
+    const Vector3d up(0, 1, 0);
+
+    Vector3d accelInWorldFrame = WorldFromImu.Pose.Rotate(FAccelInImuFrame.GetFilteredValue());
+    Quatd error = vectorAlignmentRotation(accelInWorldFrame, up);
+
+    Quatd correction;
+    if (FAccelInImuFrame.GetSize() == 1 || 
+        ((Alg::Abs(error.w) < cos(snapThreshold / 2) && FAccelInImuFrame.Confidence() > 0.75)))
+        // full correction for start-up
+        // or large error with high confidence
+        correction = error;
+    else if (FAccelInImuFrame.Confidence() > 0.5)
+        correction = error.Nlerp(Quatd(), gain * deltaT);
+    else
+        // accelerometer is unreliable due to movement
+        return;
+
+    WorldFromImu.Pose.Rotation = correction * WorldFromImu.Pose.Rotation;
+}
+
+void SensorFusion::applyCameraTiltCorrection(Vector3d accel, double deltaT)
+{
+    const double snapThreshold = 0.02; // in radians
+    const double maxCameraPositionOffset = 0.2;
+    const Vector3d up(0, 1, 0), forward(0, 0, -1);
+
+    // for startup use filtered value instead of instantaneous for stability
+    if (FAccelInCameraFrame.IsEmpty())
+        accel = FAccelInImuFrame.GetFilteredValue();
+
+    Transformd cameraFromImu = WorldFromCamera.Inverted() * VisionError.Pose * WorldFromImu.Pose;
+    // this is what the hypothetical camera-mounted accelerometer would show
+    Vector3d accelInCameraFrame = cameraFromImu.Rotate(accel);
+    FAccelInCameraFrame.Update(accelInCameraFrame, deltaT);
+    Vector3d cameraAccelInWorldFrame = WorldFromCamera.Rotate(FAccelInCameraFrame.GetFilteredValue());
+   
+    Quatd error1 = vectorAlignmentRotation(cameraAccelInWorldFrame, up);
+    // cancel out yaw rotation
+    Vector3d forwardCamera = (error1 * WorldFromCamera.Rotation).Rotate(forward);
+    forwardCamera.y = 0;
+    Quatd error2 = vectorAlignmentRotation(forwardCamera, forward);
+    // combined error
+    Quatd error = error2 * error1;
+
+    double confidence = FAccelInCameraFrame.Confidence();
+    // penalize the confidence if looking away from the camera
+    // TODO: smooth fall-off
+    if (CameraFromImu.Pose.Rotate(forward).Angle(forward) > 1)
+        confidence *= 0.5;
+
+	//Convenient global variable to temporarily extract this data.
+	TPH_CameraPoseConfidence = confidence;
+	//Allow override of confidence threshold
+	double confidenceThreshold = 0.75f;
+	if (TPH_CameraPoseConfidenceThresholdOverrideIfNonZero)
+    {
+		confidenceThreshold = TPH_CameraPoseConfidenceThresholdOverrideIfNonZero;
+	}
+
+    Quatd correction;
+    if (FAccelInCameraFrame.GetSize() == 1 ||
+        confidence > WorldFromCameraConfidence + 0.2 ||
+        // disabled due to false positives when moving side to side
+//        (Alg::Abs(error.w) < cos(5 * snapThreshold / 2) && confidence > 0.55) ||    
+        (Alg::Abs(error.w) < cos(snapThreshold / 2) && confidence > confidenceThreshold))
+    {
+        // large error with high confidence
+        correction = error;
+        // update the confidence level
+        WorldFromCameraConfidence = confidence;
+    }
+    else
+    {
+        // accelerometer is unreliable due to movement
+        return;
+    }
+
+    Transformd newWorldFromCamera(correction * WorldFromCamera.Rotation, Vector3d());
+
+    // compute a camera position change that together with the camera rotation would result in zero player movement
+    newWorldFromCamera.Translation += (WorldFromCamera * CameraFromImu.Pose).Translation -
+        (newWorldFromCamera * CameraFromImu.Pose).Translation;
+    // if the new position is too far, reset to default 
+    // (can't hide the rotation, might as well use it to reset the position)
+    if (newWorldFromCamera.Translation.DistanceSq(DefaultWorldFromCamera.Translation) > maxCameraPositionOffset * maxCameraPositionOffset)
+        newWorldFromCamera.Translation = DefaultWorldFromCamera.Translation;
+
+    WorldFromCamera = newWorldFromCamera;
+
+	//Convenient global variable to temporarily extract this data.
+	TPH_CameraPoseOrientationWxyz[0] = (float) WorldFromCamera.Rotation.w;
+	TPH_CameraPoseOrientationWxyz[1] = (float) WorldFromCamera.Rotation.x;
+	TPH_CameraPoseOrientationWxyz[2] = (float) WorldFromCamera.Rotation.y;
+	TPH_CameraPoseOrientationWxyz[3] = (float) WorldFromCamera.Rotation.z;
+}
+
+void SensorFusion::applyFocusCorrection(double deltaT)
+{
+	Vector3d up = Vector3d(0, 1, 0);
+	double gain = 0.01;
+	Vector3d currentDir = WorldFromImu.Pose.Rotate(Vector3d(0, 0, 1));
+
+	Vector3d focusYawComponent = FocusDirection.ProjectToPlane(up);
+	Vector3d currentYawComponent = currentDir.ProjectToPlane(up);
+
+	double angle = focusYawComponent.Angle(currentYawComponent);
+
+	if( angle > FocusFOV )
+	{
+		Quatd yawError;
+		if ( FocusFOV != 0.0f)
+		{
+			Vector3d lFocus = Quatd(up, -FocusFOV).Rotate(focusYawComponent);
+			Vector3d rFocus = Quatd(up, FocusFOV).Rotate(focusYawComponent);
+			double lAngle = lFocus.Angle(currentYawComponent);
+			double rAngle = rFocus.Angle(currentYawComponent);
+			if(lAngle < rAngle)
+			{
+				yawError = vectorAlignmentRotation(currentDir, lFocus);
+			} 
+            else
+			{
+				yawError = vectorAlignmentRotation(currentDir, rFocus);
+			}
+		} 
+        else
+		{
+			yawError = vectorAlignmentRotation(currentYawComponent, focusYawComponent);
+		}
+
+		Quatd correction = yawError.Nlerp(Quatd(), gain * deltaT);
+        WorldFromImu.Pose.Rotation = correction * WorldFromImu.Pose.Rotation;
+	}
+}
+
+//------------------------------------------------------------------------------------
+// Focus filter setting functions
+
+void SensorFusion::SetFocusDirection()
+{
+	SetFocusDirection(WorldFromImu.Pose.Rotate(Vector3d(0.0, 0.0, 1.0)));
+}
+
+void SensorFusion::SetFocusDirection(Vector3d direction)
+{
+	FocusDirection = direction;
+}
+
+void SensorFusion::SetFocusFOV(double fov)
+{
+	OVR_ASSERT(fov >= 0.0);
+	FocusFOV = fov;
+}
+
+void SensorFusion::ClearFocus()
+{
+	FocusDirection = Vector3d(0.0, 0.0, 0.0);
+	FocusFOV = 0.0f;
+}
+
+//-------------------------------------------------------------------------------------
+// Head model functions.
+
+// Sets up head-and-neck model and device-to-pupil dimensions from the user's profile.
+void SensorFusion::SetUserHeadDimensions(Profile const &profile, HmdRenderInfo const &hmdRenderInfo)
+{
+    float neckeye[2];
+    int count = profile.GetFloatValues(OVR_KEY_NECK_TO_EYE_DISTANCE, neckeye, 2);
+    // Make sure these are vaguely sensible values.
+    if (count == 2)
+    {
+        OVR_ASSERT ( ( neckeye[0] > 0.05f ) && ( neckeye[0] < 0.5f ) );
+        OVR_ASSERT ( ( neckeye[1] > 0.05f ) && ( neckeye[1] < 0.5f ) );
+        SetHeadModel ( Vector3f ( 0.0, neckeye[1], -neckeye[0] ) );
+    }
+
+    // Find the distance from the center of the screen to the "center eye"
+    // This center eye is used by systems like rendering & audio to represent the player,
+    // and they will handle the offsets needed from there to each actual eye.
+
+    // HACK HACK HACK
+    // We know for DK1 the screen->lens surface distance is roughly 0.049f, and that the faceplate->lens is 0.02357f.
+    // We're going to assume(!!!!) that all HMDs have the same screen->faceplate distance.
+    // Crystal Cove was measured to be roughly 0.025 screen->faceplate which agrees with this assumption.
+    // TODO: do this properly!  Update:  Measured this at 0.02733 with a CC prototype, CES era (PT7), on 2/19/14 -Steve
+    float screenCenterToMidplate = 0.02733f;
+    float centerEyeRelief = hmdRenderInfo.GetEyeCenter().ReliefInMeters;
+    float centerPupilDepth = screenCenterToMidplate + hmdRenderInfo.LensSurfaceToMidplateInMeters + centerEyeRelief;
+    SetCenterPupilDepth ( centerPupilDepth );
+
+    Recording::GetRecorder().RecordUserParams(GetHeadModel(), GetCenterPupilDepth());
+}
+
+Vector3f SensorFusion::GetHeadModel() const
+{
+    return (Vector3f)CpfFromNeck.Inverted().Translation;
+}
+
+void SensorFusion::SetHeadModel(const Vector3f &headModel, bool resetNeckPivot /*= true*/ )
+{
+    Lock::Locker lockScope(pHandler->GetHandlerLock());
+    // The head model should look something like (0, 0.12, -0.12), so
+    // these asserts are to try to prevent sign problems, as
+    // they can be subtle but nauseating!
+    OVR_ASSERT ( headModel.y > 0.0f );
+    OVR_ASSERT ( headModel.z < 0.0f );
+    CpfFromNeck = Transformd(Quatd(), (Vector3d)headModel).Inverted();
+    if ( resetNeckPivot )
+    {
+        setNeckPivotFromPose ( WorldFromImu.Pose );
+    }
+}
+
+float SensorFusion::GetCenterPupilDepth() const
+{
+    return CenterPupilDepth;
+}
+
+void SensorFusion::SetCenterPupilDepth(float centerPupilDepth)
+{
+    CenterPupilDepth = centerPupilDepth;
+
+    Transformd screenFromCpf(Quatd(), Vector3d(0, 0, centerPupilDepth));
+    ImuFromCpf = ImuFromScreen * screenFromCpf;
+	
+    setNeckPivotFromPose ( WorldFromImu.Pose );
+}
+
+//-------------------------------------------------------------------------------------
+
+// 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 Transform<double> calcPredictedPose(const PoseState<double>& poseState, double predictionDt)
+{
+    Transform<double> pose   = poseState.Pose;
+	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;
+}
+
+
+Transformf SensorFusion::GetPoseAtTime(double absoluteTime) const
+{
+    SensorState ss = GetSensorStateAtTime ( absoluteTime );
+    return ss.Predicted.Pose;
+}
+
+
+SensorState SensorFusion::GetSensorStateAtTime(double absoluteTime) const
+{          
+     const LocklessState lstate = UpdatedState.GetState();
+     // Delta time from the last available data
+     const double pdt = absoluteTime - lstate.State.TimeInSeconds;
+     
+     SensorState ss;
+     ss.Recorded     = PoseStatef(lstate.State);
+     ss.Temperature  = lstate.Temperature;
+     ss.Magnetometer = Vector3f(lstate.Magnetometer);
+     ss.StatusFlags  = lstate.StatusFlags;
+
+     ss.Predicted               = ss.Recorded;
+     ss.Predicted.TimeInSeconds = absoluteTime;
+    
+     // Do prediction logic and ImuFromCpf transformation
+     ss.Recorded.Pose  = Transformf(lstate.State.Pose * ImuFromCpf);
+     ss.Predicted.Pose = Transformf(calcPredictedPose(lstate.State, pdt) * ImuFromCpf);
+     return ss;
+}
+
+unsigned SensorFusion::GetStatus() const
+{
+    return UpdatedState.GetState().StatusFlags;
+}
+
+//-------------------------------------------------------------------------------------
+
+void SensorFusion::OnMessage(const MessageBodyFrame& msg)
+{
+    OVR_ASSERT(!IsAttachedToSensor());
+    handleMessage(msg);
+}
+
+//-------------------------------------------------------------------------------------
+
+void SensorFusion::BodyFrameHandler::OnMessage(const Message& msg)
+{
+	Recording::GetRecorder().RecordMessage(msg);
+    if (msg.Type == Message_BodyFrame)
+        pFusion->handleMessage(static_cast<const MessageBodyFrame&>(msg));
+    if (msg.Type == Message_ExposureFrame)
+        pFusion->handleExposure(static_cast<const MessageExposureFrame&>(msg));
+}
+
+} // namespace OVR