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