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