ovr_sdk

annotate LibOVR/Src/Tracking/Tracking_SensorStateReader.cpp @ 1:bd36c61436fa

find changesets by author, revision, files, or words in the commit message
added oculusd too
author John Tsiombikas <nuclear@member.fsf.org>
date Wed, 14 Jan 2015 06:51:30 +0200
parents
children
rev   line source
nuclear@0 1 /************************************************************************************
nuclear@0 2
nuclear@0 3 Filename : Tracking_SensorStateReader.cpp
nuclear@0 4 Content : Separate reader component that is able to recover sensor pose
nuclear@0 5 Created : June 4, 2014
nuclear@0 6 Authors : Chris Taylor
nuclear@0 7
nuclear@0 8 Copyright : Copyright 2014 Oculus VR, LLC All Rights reserved.
nuclear@0 9
nuclear@0 10 Licensed under the Oculus VR Rift SDK License Version 3.2 (the "License");
nuclear@0 11 you may not use the Oculus VR Rift SDK except in compliance with the License,
nuclear@0 12 which is provided at the time of installation or download, or which
nuclear@0 13 otherwise accompanies this software in either electronic or hard copy form.
nuclear@0 14
nuclear@0 15 You may obtain a copy of the License at
nuclear@0 16
nuclear@0 17 http://www.oculusvr.com/licenses/LICENSE-3.2
nuclear@0 18
nuclear@0 19 Unless required by applicable law or agreed to in writing, the Oculus VR SDK
nuclear@0 20 distributed under the License is distributed on an "AS IS" BASIS,
nuclear@0 21 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
nuclear@0 22 See the License for the specific language governing permissions and
nuclear@0 23 limitations under the License.
nuclear@0 24
nuclear@0 25 *************************************************************************************/
nuclear@0 26
nuclear@0 27 #include "Tracking_SensorStateReader.h"
nuclear@0 28 #include "Tracking_PoseState.h"
nuclear@0 29
nuclear@0 30 namespace OVR { namespace Tracking {
nuclear@0 31
nuclear@0 32
nuclear@0 33 //-------------------------------------------------------------------------------------
nuclear@0 34
nuclear@0 35 // This is a "perceptually tuned predictive filter", which means that it is optimized
nuclear@0 36 // for improvements in the VR experience, rather than pure error. In particular,
nuclear@0 37 // jitter is more perceptible at lower speeds whereas latency is more perceptible
nuclear@0 38 // after a high-speed motion. Therefore, the prediction interval is dynamically
nuclear@0 39 // adjusted based on speed. Significant more research is needed to further improve
nuclear@0 40 // this family of filters.
nuclear@0 41 static Pose<double> calcPredictedPose(const PoseState<double>& poseState, double predictionDt)
nuclear@0 42 {
nuclear@0 43 Pose<double> pose = poseState.ThePose;
nuclear@0 44 const double linearCoef = 1.0;
nuclear@0 45 Vector3d angularVelocity = poseState.AngularVelocity;
nuclear@0 46 double angularSpeed = angularVelocity.Length();
nuclear@0 47
nuclear@0 48 // This could be tuned so that linear and angular are combined with different coefficients
nuclear@0 49 double speed = angularSpeed + linearCoef * poseState.LinearVelocity.Length();
nuclear@0 50
nuclear@0 51 const double slope = 0.2; // The rate at which the dynamic prediction interval varies
nuclear@0 52 double candidateDt = slope * speed; // TODO: Replace with smoothstep function
nuclear@0 53
nuclear@0 54 double dynamicDt = predictionDt;
nuclear@0 55
nuclear@0 56 // Choose the candidate if it is shorter, to improve stability
nuclear@0 57 if (candidateDt < predictionDt)
nuclear@0 58 {
nuclear@0 59 dynamicDt = candidateDt;
nuclear@0 60 }
nuclear@0 61
nuclear@0 62 if (angularSpeed > 0.001)
nuclear@0 63 {
nuclear@0 64 pose.Rotation = pose.Rotation * Quatd(angularVelocity, angularSpeed * dynamicDt);
nuclear@0 65 }
nuclear@0 66
nuclear@0 67 pose.Translation += poseState.LinearVelocity * dynamicDt;
nuclear@0 68
nuclear@0 69 return pose;
nuclear@0 70 }
nuclear@0 71
nuclear@0 72
nuclear@0 73 //// SensorStateReader
nuclear@0 74
nuclear@0 75 SensorStateReader::SensorStateReader() :
nuclear@0 76 Updater(NULL),
nuclear@0 77 LastLatWarnTime(0.)
nuclear@0 78 {
nuclear@0 79 }
nuclear@0 80
nuclear@0 81 void SensorStateReader::SetUpdater(const CombinedSharedStateUpdater* updater)
nuclear@0 82 {
nuclear@0 83 Updater = updater;
nuclear@0 84 }
nuclear@0 85
nuclear@0 86 void SensorStateReader::RecenterPose()
nuclear@0 87 {
nuclear@0 88 if (!Updater)
nuclear@0 89 {
nuclear@0 90 return;
nuclear@0 91 }
nuclear@0 92
nuclear@0 93 /*
nuclear@0 94 This resets position to center in x, y, z, and resets yaw to center.
nuclear@0 95 Other rotation components are not affected.
nuclear@0 96 */
nuclear@0 97
nuclear@0 98 const LocklessSensorState lstate = Updater->SharedSensorState.GetState();
nuclear@0 99
nuclear@0 100 Posed worldFromCpf = lstate.WorldFromImu.ThePose * lstate.ImuFromCpf;
nuclear@0 101 double hmdYaw, hmdPitch, hmdRoll;
nuclear@0 102 worldFromCpf.Rotation.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&hmdYaw, &hmdPitch, &hmdRoll);
nuclear@0 103
nuclear@0 104 Posed worldFromCentered(Quatd(Axis_Y, hmdYaw), worldFromCpf.Translation);
nuclear@0 105
nuclear@0 106 CenteredFromWorld = worldFromCentered.Inverted();
nuclear@0 107 }
nuclear@0 108
nuclear@0 109 bool SensorStateReader::GetSensorStateAtTime(double absoluteTime, TrackingState& ss) const
nuclear@0 110 {
nuclear@0 111 if (!Updater)
nuclear@0 112 {
nuclear@0 113 ss.StatusFlags = 0;
nuclear@0 114 return false;
nuclear@0 115 }
nuclear@0 116
nuclear@0 117 const LocklessSensorState lstate = Updater->SharedSensorState.GetState();
nuclear@0 118
nuclear@0 119 // Update time
nuclear@0 120 ss.HeadPose.TimeInSeconds = absoluteTime;
nuclear@0 121
nuclear@0 122 // Update the status flags
nuclear@0 123 ss.StatusFlags = lstate.StatusFlags;
nuclear@0 124 // If no hardware is connected, override the tracking flags
nuclear@0 125 if (0 == (ss.StatusFlags & Status_HMDConnected))
nuclear@0 126 {
nuclear@0 127 ss.StatusFlags &= ~Status_TrackingMask;
nuclear@0 128 }
nuclear@0 129 if (0 == (ss.StatusFlags & Status_PositionConnected))
nuclear@0 130 {
nuclear@0 131 ss.StatusFlags &= ~(Status_PositionTracked | Status_CameraPoseTracked);
nuclear@0 132 }
nuclear@0 133
nuclear@0 134 // If tracking info is invalid,
nuclear@0 135 if (0 == (ss.StatusFlags & Status_TrackingMask))
nuclear@0 136 {
nuclear@0 137 return false;
nuclear@0 138 }
nuclear@0 139
nuclear@0 140 // Delta time from the last available data
nuclear@0 141 double pdt = absoluteTime - lstate.WorldFromImu.TimeInSeconds;
nuclear@0 142 static const double maxPdt = 0.1;
nuclear@0 143
nuclear@0 144 // If delta went negative due to synchronization problems between processes or just a lag spike,
nuclear@0 145 if (pdt < 0.)
nuclear@0 146 {
nuclear@0 147 pdt = 0.;
nuclear@0 148 }
nuclear@0 149 else if (pdt > maxPdt)
nuclear@0 150 {
nuclear@0 151 if (LastLatWarnTime != lstate.WorldFromImu.TimeInSeconds)
nuclear@0 152 {
nuclear@0 153 LastLatWarnTime = lstate.WorldFromImu.TimeInSeconds;
nuclear@0 154 LogText("[SensorStateReader] Prediction interval too high: %f s, clamping at %f s\n", pdt, maxPdt);
nuclear@0 155 }
nuclear@0 156 pdt = maxPdt;
nuclear@0 157 }
nuclear@0 158
nuclear@0 159 ss.HeadPose = PoseStatef(lstate.WorldFromImu);
nuclear@0 160 // Do prediction logic and ImuFromCpf transformation
nuclear@0 161 ss.HeadPose.ThePose = Posef(CenteredFromWorld * calcPredictedPose(lstate.WorldFromImu, pdt) * lstate.ImuFromCpf);
nuclear@0 162
nuclear@0 163 ss.CameraPose = Posef(CenteredFromWorld * lstate.WorldFromCamera);
nuclear@0 164
nuclear@0 165 Posed worldFromLeveledCamera = Posed(Quatd(), lstate.WorldFromCamera.Translation);
nuclear@0 166 ss.LeveledCameraPose = Posef(CenteredFromWorld * worldFromLeveledCamera);
nuclear@0 167
nuclear@0 168 ss.RawSensorData = lstate.RawSensorData;
nuclear@0 169 ss.LastVisionProcessingTime = lstate.LastVisionProcessingTime;
nuclear@0 170 ss.LastVisionFrameLatency = lstate.LastVisionFrameLatency;
nuclear@0 171
nuclear@0 172 return true;
nuclear@0 173 }
nuclear@0 174
nuclear@0 175 bool SensorStateReader::GetPoseAtTime(double absoluteTime, Posef& transform) const
nuclear@0 176 {
nuclear@0 177 TrackingState ss;
nuclear@0 178 if (!GetSensorStateAtTime(absoluteTime, ss))
nuclear@0 179 {
nuclear@0 180 return false;
nuclear@0 181 }
nuclear@0 182
nuclear@0 183 transform = ss.HeadPose.ThePose;
nuclear@0 184
nuclear@0 185 return true;
nuclear@0 186 }
nuclear@0 187
nuclear@0 188 uint32_t SensorStateReader::GetStatus() const
nuclear@0 189 {
nuclear@0 190 if (!Updater)
nuclear@0 191 {
nuclear@0 192 return 0;
nuclear@0 193 }
nuclear@0 194
nuclear@0 195 const LocklessSensorState lstate = Updater->SharedSensorState.GetState();
nuclear@0 196
nuclear@0 197 // If invalid,
nuclear@0 198 if (0 == (lstate.StatusFlags & Status_TrackingMask))
nuclear@0 199 {
nuclear@0 200 // Return 0 indicating no orientation nor position tracking
nuclear@0 201 return 0;
nuclear@0 202 }
nuclear@0 203
nuclear@0 204 return lstate.StatusFlags;
nuclear@0 205 }
nuclear@0 206
nuclear@0 207 }} // namespace OVR::Tracking