/************************************************************************************
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 = 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<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