/************************************************************************************ 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 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(); WorldFromImu.Pose = ImuFromCpf.Inverted(); // place CPF at the origin, not the IMU CameraFromImu = PoseState(); VisionError = PoseState(); WorldFromCamera = DefaultWorldFromCamera; WorldFromCameraConfidence = -1; ExposureRecordHistory.Clear(); NextExposureRecord = ExposureRecord(); LastMessageExposureFrame = MessageExposureFrame(NULL); LastVisionAbsoluteTime = 1e9; 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& 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& 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()); // ********* 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 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 delta = PoseState(); 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 ( 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 calcPredictedPose(const PoseState& poseState, double predictionDt) { Transform 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(msg)); if (msg.Type == Message_ExposureFrame) pFusion->handleExposure(static_cast(msg)); } } // namespace OVR