diff options
Diffstat (limited to 'LibOVR/Src/OVR_SensorFusion.cpp')
| -rw-r--r-- | LibOVR/Src/OVR_SensorFusion.cpp | 904 |
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 |