1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/LibOVR/Src/Tracking/Tracking_SensorStateReader.cpp	Wed Jan 14 06:51:16 2015 +0200
     1.3 @@ -0,0 +1,207 @@
     1.4 +/************************************************************************************
     1.5 +
     1.6 +Filename    :   Tracking_SensorStateReader.cpp
     1.7 +Content     :   Separate reader component that is able to recover sensor pose
     1.8 +Created     :   June 4, 2014
     1.9 +Authors     :   Chris Taylor
    1.10 +
    1.11 +Copyright   :   Copyright 2014 Oculus VR, LLC All Rights reserved.
    1.12 +
    1.13 +Licensed under the Oculus VR Rift SDK License Version 3.2 (the "License"); 
    1.14 +you may not use the Oculus VR Rift SDK except in compliance with the License, 
    1.15 +which is provided at the time of installation or download, or which 
    1.16 +otherwise accompanies this software in either electronic or hard copy form.
    1.17 +
    1.18 +You may obtain a copy of the License at
    1.19 +
    1.20 +http://www.oculusvr.com/licenses/LICENSE-3.2 
    1.21 +
    1.22 +Unless required by applicable law or agreed to in writing, the Oculus VR SDK 
    1.23 +distributed under the License is distributed on an "AS IS" BASIS,
    1.24 +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    1.25 +See the License for the specific language governing permissions and
    1.26 +limitations under the License.
    1.27 +
    1.28 +*************************************************************************************/
    1.29 +
    1.30 +#include "Tracking_SensorStateReader.h"
    1.31 +#include "Tracking_PoseState.h"
    1.32 +
    1.33 +namespace OVR { namespace Tracking {
    1.34 +
    1.35 +
    1.36 +//-------------------------------------------------------------------------------------
    1.37 +
    1.38 +// This is a "perceptually tuned predictive filter", which means that it is optimized
    1.39 +// for improvements in the VR experience, rather than pure error.  In particular,
    1.40 +// jitter is more perceptible at lower speeds whereas latency is more perceptible
    1.41 +// after a high-speed motion.  Therefore, the prediction interval is dynamically
    1.42 +// adjusted based on speed.  Significant more research is needed to further improve
    1.43 +// this family of filters.
    1.44 +static Pose<double> calcPredictedPose(const PoseState<double>& poseState, double predictionDt)
    1.45 +{
    1.46 +	Pose<double> pose = poseState.ThePose;
    1.47 +	const double linearCoef = 1.0;
    1.48 +	Vector3d angularVelocity = poseState.AngularVelocity;
    1.49 +	double angularSpeed = angularVelocity.Length();
    1.50 +
    1.51 +	// This could be tuned so that linear and angular are combined with different coefficients
    1.52 +	double speed = angularSpeed + linearCoef * poseState.LinearVelocity.Length();
    1.53 +
    1.54 +	const double slope = 0.2; // The rate at which the dynamic prediction interval varies
    1.55 +	double candidateDt = slope * speed; // TODO: Replace with smoothstep function
    1.56 +
    1.57 +	double dynamicDt = predictionDt;
    1.58 +
    1.59 +	// Choose the candidate if it is shorter, to improve stability
    1.60 +	if (candidateDt < predictionDt)
    1.61 +	{
    1.62 +		dynamicDt = candidateDt;
    1.63 +	}
    1.64 +
    1.65 +	if (angularSpeed > 0.001)
    1.66 +	{
    1.67 +		pose.Rotation = pose.Rotation * Quatd(angularVelocity, angularSpeed * dynamicDt);
    1.68 +	}
    1.69 +
    1.70 +	pose.Translation += poseState.LinearVelocity * dynamicDt;
    1.71 +
    1.72 +	return pose;
    1.73 +}
    1.74 +
    1.75 +
    1.76 +//// SensorStateReader
    1.77 +
    1.78 +SensorStateReader::SensorStateReader() :
    1.79 +	Updater(NULL),
    1.80 +    LastLatWarnTime(0.)
    1.81 +{
    1.82 +}
    1.83 +
    1.84 +void SensorStateReader::SetUpdater(const CombinedSharedStateUpdater* updater)
    1.85 +{
    1.86 +	Updater = updater;
    1.87 +}
    1.88 +
    1.89 +void SensorStateReader::RecenterPose()
    1.90 +{
    1.91 +	if (!Updater)
    1.92 +	{
    1.93 +		return;
    1.94 +	}
    1.95 +
    1.96 +	/*
    1.97 +		This resets position to center in x, y, z, and resets yaw to center.
    1.98 +		Other rotation components are not affected.
    1.99 +	*/
   1.100 +
   1.101 +	const LocklessSensorState lstate = Updater->SharedSensorState.GetState();
   1.102 +
   1.103 +	Posed worldFromCpf = lstate.WorldFromImu.ThePose * lstate.ImuFromCpf;
   1.104 +	double hmdYaw, hmdPitch, hmdRoll;
   1.105 +	worldFromCpf.Rotation.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&hmdYaw, &hmdPitch, &hmdRoll);
   1.106 +
   1.107 +	Posed worldFromCentered(Quatd(Axis_Y, hmdYaw), worldFromCpf.Translation);
   1.108 +
   1.109 +	CenteredFromWorld = worldFromCentered.Inverted();
   1.110 +}
   1.111 +
   1.112 +bool SensorStateReader::GetSensorStateAtTime(double absoluteTime, TrackingState& ss) const
   1.113 +{
   1.114 +	if (!Updater)
   1.115 +	{
   1.116 +        ss.StatusFlags = 0;
   1.117 +        return false;
   1.118 +	}
   1.119 +
   1.120 +	const LocklessSensorState lstate = Updater->SharedSensorState.GetState();
   1.121 +
   1.122 +    // Update time
   1.123 +	ss.HeadPose.TimeInSeconds = absoluteTime;
   1.124 +
   1.125 +	// Update the status flags
   1.126 +	ss.StatusFlags = lstate.StatusFlags;
   1.127 +	// If no hardware is connected, override the tracking flags
   1.128 +	if (0 == (ss.StatusFlags & Status_HMDConnected))
   1.129 +	{
   1.130 +		ss.StatusFlags &= ~Status_TrackingMask;
   1.131 +	}
   1.132 +	if (0 == (ss.StatusFlags & Status_PositionConnected))
   1.133 +	{
   1.134 +		ss.StatusFlags &= ~(Status_PositionTracked | Status_CameraPoseTracked);
   1.135 +	}
   1.136 +
   1.137 +	// If tracking info is invalid,
   1.138 +    if (0 == (ss.StatusFlags & Status_TrackingMask))
   1.139 +	{
   1.140 +        return false;
   1.141 +	}
   1.142 +    
   1.143 +    // Delta time from the last available data
   1.144 +	double pdt = absoluteTime - lstate.WorldFromImu.TimeInSeconds;
   1.145 +	static const double maxPdt = 0.1;
   1.146 +
   1.147 +	// If delta went negative due to synchronization problems between processes or just a lag spike,
   1.148 +	if (pdt < 0.)
   1.149 +	{
   1.150 +		pdt = 0.;
   1.151 +	}
   1.152 +	else if (pdt > maxPdt)
   1.153 +	{
   1.154 +        if (LastLatWarnTime != lstate.WorldFromImu.TimeInSeconds)
   1.155 +        {
   1.156 +            LastLatWarnTime = lstate.WorldFromImu.TimeInSeconds;
   1.157 +            LogText("[SensorStateReader] Prediction interval too high: %f s, clamping at %f s\n", pdt, maxPdt);
   1.158 +        }
   1.159 +		pdt = maxPdt;
   1.160 +	}
   1.161 +
   1.162 +	ss.HeadPose = PoseStatef(lstate.WorldFromImu);
   1.163 +	// Do prediction logic and ImuFromCpf transformation
   1.164 +	ss.HeadPose.ThePose = Posef(CenteredFromWorld * calcPredictedPose(lstate.WorldFromImu, pdt) * lstate.ImuFromCpf);
   1.165 +
   1.166 +    ss.CameraPose = Posef(CenteredFromWorld * lstate.WorldFromCamera);
   1.167 +
   1.168 +    Posed worldFromLeveledCamera = Posed(Quatd(), lstate.WorldFromCamera.Translation);
   1.169 +    ss.LeveledCameraPose = Posef(CenteredFromWorld * worldFromLeveledCamera);
   1.170 +
   1.171 +    ss.RawSensorData = lstate.RawSensorData;
   1.172 +    ss.LastVisionProcessingTime = lstate.LastVisionProcessingTime;
   1.173 +    ss.LastVisionFrameLatency = lstate.LastVisionFrameLatency;
   1.174 +
   1.175 +	return true;
   1.176 +}
   1.177 +
   1.178 +bool SensorStateReader::GetPoseAtTime(double absoluteTime, Posef& transform) const
   1.179 +{
   1.180 +	TrackingState ss;
   1.181 +	if (!GetSensorStateAtTime(absoluteTime, ss))
   1.182 +	{
   1.183 +		return false;
   1.184 +	}
   1.185 +
   1.186 +	transform = ss.HeadPose.ThePose;
   1.187 +
   1.188 +	return true;
   1.189 +}
   1.190 +
   1.191 +uint32_t SensorStateReader::GetStatus() const
   1.192 +{
   1.193 +	if (!Updater)
   1.194 +	{
   1.195 +		return 0;
   1.196 +	}
   1.197 +
   1.198 +	const LocklessSensorState lstate = Updater->SharedSensorState.GetState();
   1.199 +
   1.200 +	// If invalid,
   1.201 +	if (0 == (lstate.StatusFlags & Status_TrackingMask))
   1.202 +	{
   1.203 +		// Return 0 indicating no orientation nor position tracking
   1.204 +		return 0;
   1.205 +	}
   1.206 +
   1.207 +	return lstate.StatusFlags;
   1.208 +}
   1.209 +
   1.210 +}} // namespace OVR::Tracking