nuclear@0: /************************************************************************************ nuclear@0: nuclear@0: Filename : Tracking_SensorStateReader.cpp nuclear@0: Content : Separate reader component that is able to recover sensor pose nuclear@0: Created : June 4, 2014 nuclear@0: Authors : Chris Taylor nuclear@0: nuclear@0: Copyright : Copyright 2014 Oculus VR, LLC All Rights reserved. nuclear@0: nuclear@0: Licensed under the Oculus VR Rift SDK License Version 3.2 (the "License"); nuclear@0: you may not use the Oculus VR Rift SDK except in compliance with the License, nuclear@0: which is provided at the time of installation or download, or which nuclear@0: otherwise accompanies this software in either electronic or hard copy form. nuclear@0: nuclear@0: You may obtain a copy of the License at nuclear@0: nuclear@0: http://www.oculusvr.com/licenses/LICENSE-3.2 nuclear@0: nuclear@0: Unless required by applicable law or agreed to in writing, the Oculus VR SDK nuclear@0: distributed under the License is distributed on an "AS IS" BASIS, nuclear@0: WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. nuclear@0: See the License for the specific language governing permissions and nuclear@0: limitations under the License. nuclear@0: nuclear@0: *************************************************************************************/ nuclear@0: nuclear@0: #include "Tracking_SensorStateReader.h" nuclear@0: #include "Tracking_PoseState.h" nuclear@0: nuclear@0: namespace OVR { namespace Tracking { nuclear@0: nuclear@0: nuclear@0: //------------------------------------------------------------------------------------- nuclear@0: nuclear@0: // This is a "perceptually tuned predictive filter", which means that it is optimized nuclear@0: // for improvements in the VR experience, rather than pure error. In particular, nuclear@0: // jitter is more perceptible at lower speeds whereas latency is more perceptible nuclear@0: // after a high-speed motion. Therefore, the prediction interval is dynamically nuclear@0: // adjusted based on speed. Significant more research is needed to further improve nuclear@0: // this family of filters. nuclear@0: static Pose calcPredictedPose(const PoseState& poseState, double predictionDt) nuclear@0: { nuclear@0: Pose pose = poseState.ThePose; nuclear@0: const double linearCoef = 1.0; nuclear@0: Vector3d angularVelocity = poseState.AngularVelocity; nuclear@0: double angularSpeed = angularVelocity.Length(); nuclear@0: nuclear@0: // This could be tuned so that linear and angular are combined with different coefficients nuclear@0: double speed = angularSpeed + linearCoef * poseState.LinearVelocity.Length(); nuclear@0: nuclear@0: const double slope = 0.2; // The rate at which the dynamic prediction interval varies nuclear@0: double candidateDt = slope * speed; // TODO: Replace with smoothstep function nuclear@0: nuclear@0: double dynamicDt = predictionDt; nuclear@0: nuclear@0: // Choose the candidate if it is shorter, to improve stability nuclear@0: if (candidateDt < predictionDt) nuclear@0: { nuclear@0: dynamicDt = candidateDt; nuclear@0: } nuclear@0: nuclear@0: if (angularSpeed > 0.001) nuclear@0: { nuclear@0: pose.Rotation = pose.Rotation * Quatd(angularVelocity, angularSpeed * dynamicDt); nuclear@0: } nuclear@0: nuclear@0: pose.Translation += poseState.LinearVelocity * dynamicDt; nuclear@0: nuclear@0: return pose; nuclear@0: } nuclear@0: nuclear@0: nuclear@0: //// SensorStateReader nuclear@0: nuclear@0: SensorStateReader::SensorStateReader() : nuclear@0: Updater(NULL), nuclear@0: LastLatWarnTime(0.) nuclear@0: { nuclear@0: } nuclear@0: nuclear@0: void SensorStateReader::SetUpdater(const CombinedSharedStateUpdater* updater) nuclear@0: { nuclear@0: Updater = updater; nuclear@0: } nuclear@0: nuclear@0: void SensorStateReader::RecenterPose() nuclear@0: { nuclear@0: if (!Updater) nuclear@0: { nuclear@0: return; nuclear@0: } nuclear@0: nuclear@0: /* nuclear@0: This resets position to center in x, y, z, and resets yaw to center. nuclear@0: Other rotation components are not affected. nuclear@0: */ nuclear@0: nuclear@0: const LocklessSensorState lstate = Updater->SharedSensorState.GetState(); nuclear@0: nuclear@0: Posed worldFromCpf = lstate.WorldFromImu.ThePose * lstate.ImuFromCpf; nuclear@0: double hmdYaw, hmdPitch, hmdRoll; nuclear@0: worldFromCpf.Rotation.GetEulerAngles(&hmdYaw, &hmdPitch, &hmdRoll); nuclear@0: nuclear@0: Posed worldFromCentered(Quatd(Axis_Y, hmdYaw), worldFromCpf.Translation); nuclear@0: nuclear@0: CenteredFromWorld = worldFromCentered.Inverted(); nuclear@0: } nuclear@0: nuclear@0: bool SensorStateReader::GetSensorStateAtTime(double absoluteTime, TrackingState& ss) const nuclear@0: { nuclear@0: if (!Updater) nuclear@0: { nuclear@0: ss.StatusFlags = 0; nuclear@0: return false; nuclear@0: } nuclear@0: nuclear@0: const LocklessSensorState lstate = Updater->SharedSensorState.GetState(); nuclear@0: nuclear@0: // Update time nuclear@0: ss.HeadPose.TimeInSeconds = absoluteTime; nuclear@0: nuclear@0: // Update the status flags nuclear@0: ss.StatusFlags = lstate.StatusFlags; nuclear@0: // If no hardware is connected, override the tracking flags nuclear@0: if (0 == (ss.StatusFlags & Status_HMDConnected)) nuclear@0: { nuclear@0: ss.StatusFlags &= ~Status_TrackingMask; nuclear@0: } nuclear@0: if (0 == (ss.StatusFlags & Status_PositionConnected)) nuclear@0: { nuclear@0: ss.StatusFlags &= ~(Status_PositionTracked | Status_CameraPoseTracked); nuclear@0: } nuclear@0: nuclear@0: // If tracking info is invalid, nuclear@0: if (0 == (ss.StatusFlags & Status_TrackingMask)) nuclear@0: { nuclear@0: return false; nuclear@0: } nuclear@0: nuclear@0: // Delta time from the last available data nuclear@0: double pdt = absoluteTime - lstate.WorldFromImu.TimeInSeconds; nuclear@0: static const double maxPdt = 0.1; nuclear@0: nuclear@0: // If delta went negative due to synchronization problems between processes or just a lag spike, nuclear@0: if (pdt < 0.) nuclear@0: { nuclear@0: pdt = 0.; nuclear@0: } nuclear@0: else if (pdt > maxPdt) nuclear@0: { nuclear@0: if (LastLatWarnTime != lstate.WorldFromImu.TimeInSeconds) nuclear@0: { nuclear@0: LastLatWarnTime = lstate.WorldFromImu.TimeInSeconds; nuclear@0: LogText("[SensorStateReader] Prediction interval too high: %f s, clamping at %f s\n", pdt, maxPdt); nuclear@0: } nuclear@0: pdt = maxPdt; nuclear@0: } nuclear@0: nuclear@0: ss.HeadPose = PoseStatef(lstate.WorldFromImu); nuclear@0: // Do prediction logic and ImuFromCpf transformation nuclear@0: ss.HeadPose.ThePose = Posef(CenteredFromWorld * calcPredictedPose(lstate.WorldFromImu, pdt) * lstate.ImuFromCpf); nuclear@0: nuclear@0: ss.CameraPose = Posef(CenteredFromWorld * lstate.WorldFromCamera); nuclear@0: nuclear@0: Posed worldFromLeveledCamera = Posed(Quatd(), lstate.WorldFromCamera.Translation); nuclear@0: ss.LeveledCameraPose = Posef(CenteredFromWorld * worldFromLeveledCamera); nuclear@0: nuclear@0: ss.RawSensorData = lstate.RawSensorData; nuclear@0: ss.LastVisionProcessingTime = lstate.LastVisionProcessingTime; nuclear@0: ss.LastVisionFrameLatency = lstate.LastVisionFrameLatency; nuclear@0: nuclear@0: return true; nuclear@0: } nuclear@0: nuclear@0: bool SensorStateReader::GetPoseAtTime(double absoluteTime, Posef& transform) const nuclear@0: { nuclear@0: TrackingState ss; nuclear@0: if (!GetSensorStateAtTime(absoluteTime, ss)) nuclear@0: { nuclear@0: return false; nuclear@0: } nuclear@0: nuclear@0: transform = ss.HeadPose.ThePose; nuclear@0: nuclear@0: return true; nuclear@0: } nuclear@0: nuclear@0: uint32_t SensorStateReader::GetStatus() const nuclear@0: { nuclear@0: if (!Updater) nuclear@0: { nuclear@0: return 0; nuclear@0: } nuclear@0: nuclear@0: const LocklessSensorState lstate = Updater->SharedSensorState.GetState(); nuclear@0: nuclear@0: // If invalid, nuclear@0: if (0 == (lstate.StatusFlags & Status_TrackingMask)) nuclear@0: { nuclear@0: // Return 0 indicating no orientation nor position tracking nuclear@0: return 0; nuclear@0: } nuclear@0: nuclear@0: return lstate.StatusFlags; nuclear@0: } nuclear@0: nuclear@0: }} // namespace OVR::Tracking