mirror of
https://github.com/mii443/rust-openvr.git
synced 2025-08-24 17:19:25 +00:00
905 lines
35 KiB
C++
905 lines
35 KiB
C++
/************************************************************************************
|
|
|
|
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
|