oculus1
view libovr/Src/OVR_SensorFusion.cpp @ 1:e2f9e4603129
added LibOVR and started a simple vr wrapper.
| author | John Tsiombikas <nuclear@member.fsf.org> |
|---|---|
| date | Sat, 14 Sep 2013 16:14:59 +0300 |
| parents | |
| children |
line source
1 /************************************************************************************
3 Filename : OVR_SensorFusion.cpp
4 Content : Methods that determine head orientation from sensor data over time
5 Created : October 9, 2012
6 Authors : Michael Antonov, Steve LaValle
8 Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved.
10 Use of this software is subject to the terms of the Oculus license
11 agreement provided at the time of installation or download, or which
12 otherwise accompanies this software in either electronic or hard copy form.
14 *************************************************************************************/
16 #include "OVR_SensorFusion.h"
17 #include "Kernel/OVR_Log.h"
18 #include "Kernel/OVR_System.h"
19 #include "OVR_JSON.h"
20 #include "OVR_Profile.h"
22 namespace OVR {
24 //-------------------------------------------------------------------------------------
25 // ***** Sensor Fusion
27 SensorFusion::SensorFusion(SensorDevice* sensor)
28 : Handler(getThis()), pDelegate(0),
29 Gain(0.05f), YawMult(1), EnableGravity(true), Stage(0), RunningTime(0), DeltaT(0.001f),
30 EnablePrediction(true), PredictionDT(0.03f), PredictionTimeIncrement(0.001f),
31 FRawMag(10), FAccW(20), FAngV(20),
32 TiltCondCount(0), TiltErrorAngle(0),
33 TiltErrorAxis(0,1,0),
34 MagCondCount(0), MagCalibrated(false), MagRefQ(0, 0, 0, 1),
35 MagRefM(0), MagRefYaw(0), YawErrorAngle(0), MagRefDistance(0.5f),
36 YawErrorCount(0), YawCorrectionActivated(false), YawCorrectionInProgress(false),
37 EnableYawCorrection(false), MagNumReferences(0), MagHasNearbyReference(false),
38 MotionTrackingEnabled(true)
39 {
40 if (sensor)
41 AttachToSensor(sensor);
42 MagCalibrationMatrix.SetIdentity();
43 }
45 SensorFusion::~SensorFusion()
46 {
47 }
50 bool SensorFusion::AttachToSensor(SensorDevice* sensor)
51 {
52 // clear the cached device information
53 CachedSensorInfo.SerialNumber[0] = 0;
54 CachedSensorInfo.VendorId = 0;
55 CachedSensorInfo.ProductId = 0;
57 if (sensor != NULL)
58 {
59 // Cache the sensor device so we can access this information during
60 // mag saving and loading (avoid holding a reference to sensor to prevent
61 // deadlock on shutdown)
62 sensor->GetDeviceInfo(&CachedSensorInfo); // save the device information
63 MessageHandler* pCurrentHandler = sensor->GetMessageHandler();
65 if (pCurrentHandler == &Handler)
66 {
67 Reset();
68 return true;
69 }
71 if (pCurrentHandler != NULL)
72 {
73 OVR_DEBUG_LOG(
74 ("SensorFusion::AttachToSensor failed - sensor %p already has handler", sensor));
75 return false;
76 }
78 // Automatically load the default mag calibration for this sensor
79 LoadMagCalibration();
80 }
82 if (Handler.IsHandlerInstalled())
83 {
84 Handler.RemoveHandlerFromDevices();
85 }
87 if (sensor != NULL)
88 {
89 sensor->SetMessageHandler(&Handler);
90 }
92 Reset();
93 return true;
94 }
97 // Resets the current orientation
98 void SensorFusion::Reset()
99 {
100 Lock::Locker lockScope(Handler.GetHandlerLock());
101 Q = Quatf();
102 QUncorrected = Quatf();
103 Stage = 0;
104 RunningTime = 0;
105 MagNumReferences = 0;
106 MagHasNearbyReference = false;
107 }
110 void SensorFusion::handleMessage(const MessageBodyFrame& msg)
111 {
112 if (msg.Type != Message_BodyFrame || !IsMotionTrackingEnabled())
113 return;
115 // Put the sensor readings into convenient local variables
116 Vector3f angVel = msg.RotationRate;
117 Vector3f rawAccel = msg.Acceleration;
118 Vector3f mag = msg.MagneticField;
120 // Set variables accessible through the class API
121 DeltaT = msg.TimeDelta;
122 AngV = msg.RotationRate;
123 AngV.y *= YawMult; // Warning: If YawMult != 1, then AngV is not true angular velocity
124 A = rawAccel;
126 // Allow external access to uncalibrated magnetometer values
127 RawMag = mag;
129 // Apply the calibration parameters to raw mag
130 if (HasMagCalibration())
131 {
132 mag.x += MagCalibrationMatrix.M[0][3];
133 mag.y += MagCalibrationMatrix.M[1][3];
134 mag.z += MagCalibrationMatrix.M[2][3];
135 }
137 // Provide external access to calibrated mag values
138 // (if the mag is not calibrated, then the raw value is returned)
139 CalMag = mag;
141 float angVelLength = angVel.Length();
142 float accLength = rawAccel.Length();
145 // Acceleration in the world frame (Q is current HMD orientation)
146 Vector3f accWorld = Q.Rotate(rawAccel);
148 // Keep track of time
149 Stage++;
150 RunningTime += DeltaT;
152 // Insert current sensor data into filter history
153 FRawMag.AddElement(RawMag);
154 FAccW.AddElement(accWorld);
155 FAngV.AddElement(angVel);
157 // Update orientation Q based on gyro outputs. This technique is
158 // based on direct properties of the angular velocity vector:
159 // Its direction is the current rotation axis, and its magnitude
160 // is the rotation rate (rad/sec) about that axis. Our sensor
161 // sampling rate is so fast that we need not worry about integral
162 // approximation error (not yet, anyway).
163 if (angVelLength > 0.0f)
164 {
165 Vector3f rotAxis = angVel / angVelLength;
166 float halfRotAngle = angVelLength * DeltaT * 0.5f;
167 float sinHRA = sin(halfRotAngle);
168 Quatf deltaQ(rotAxis.x*sinHRA, rotAxis.y*sinHRA, rotAxis.z*sinHRA, cos(halfRotAngle));
170 Q = Q * deltaQ;
171 }
173 // The quaternion magnitude may slowly drift due to numerical error,
174 // so it is periodically normalized.
175 if (Stage % 5000 == 0)
176 Q.Normalize();
178 // Maintain the uncorrected orientation for later use by predictive filtering
179 QUncorrected = Q;
181 // Perform tilt correction using the accelerometer data. This enables
182 // drift errors in pitch and roll to be corrected. Note that yaw cannot be corrected
183 // because the rotation axis is parallel to the gravity vector.
184 if (EnableGravity)
185 {
186 // Correcting for tilt error by using accelerometer data
187 const float gravityEpsilon = 0.4f;
188 const float angVelEpsilon = 0.1f; // Relatively slow rotation
189 const int tiltPeriod = 50; // Required time steps of stability
190 const float maxTiltError = 0.05f;
191 const float minTiltError = 0.01f;
193 // This condition estimates whether the only measured acceleration is due to gravity
194 // (the Rift is not linearly accelerating). It is often wrong, but tends to average
195 // out well over time.
196 if ((fabs(accLength - 9.81f) < gravityEpsilon) &&
197 (angVelLength < angVelEpsilon))
198 TiltCondCount++;
199 else
200 TiltCondCount = 0;
202 // After stable measurements have been taken over a sufficiently long period,
203 // estimate the amount of tilt error and calculate the tilt axis for later correction.
204 if (TiltCondCount >= tiltPeriod)
205 { // Update TiltErrorEstimate
206 TiltCondCount = 0;
207 // Use an average value to reduce noise (could alternatively use an LPF)
208 Vector3f accWMean = FAccW.Mean();
209 // Project the acceleration vector into the XZ plane
210 Vector3f xzAcc = Vector3f(accWMean.x, 0.0f, accWMean.z);
211 // The unit normal of xzAcc will be the rotation axis for tilt correction
212 Vector3f tiltAxis = Vector3f(xzAcc.z, 0.0f, -xzAcc.x).Normalized();
213 Vector3f yUp = Vector3f(0.0f, 1.0f, 0.0f);
214 // This is the amount of rotation
215 float tiltAngle = yUp.Angle(accWMean);
216 // Record values if the tilt error is intolerable
217 if (tiltAngle > maxTiltError)
218 {
219 TiltErrorAngle = tiltAngle;
220 TiltErrorAxis = tiltAxis;
221 }
222 }
224 // This part performs the actual tilt correction as needed
225 if (TiltErrorAngle > minTiltError)
226 {
227 if ((TiltErrorAngle > 0.4f)&&(RunningTime < 8.0f))
228 { // Tilt completely to correct orientation
229 Q = Quatf(TiltErrorAxis, -TiltErrorAngle) * Q;
230 TiltErrorAngle = 0.0f;
231 }
232 else
233 {
234 //LogText("Performing tilt correction - Angle: %f Axis: %f %f %f\n",
235 // TiltErrorAngle,TiltErrorAxis.x,TiltErrorAxis.y,TiltErrorAxis.z);
236 //float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f;
237 // This uses aggressive correction steps while your head is moving fast
238 float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f*(5.0f*angVelLength+1.0f);
239 // Incrementally "un-tilt" by a small step size
240 Q = Quatf(TiltErrorAxis, deltaTiltAngle) * Q;
241 TiltErrorAngle += deltaTiltAngle;
242 }
243 }
244 }
246 // Yaw drift correction based on magnetometer data. This corrects the part of the drift
247 // that the accelerometer cannot handle.
248 // This will only work if the magnetometer has been enabled, calibrated, and a reference
249 // point has been set.
250 const float maxAngVelLength = 3.0f;
251 const int magWindow = 5;
252 const float yawErrorMax = 0.1f;
253 const float yawErrorMin = 0.01f;
254 const int yawErrorCountLimit = 50;
255 const float yawRotationStep = 0.00002f;
257 if (angVelLength < maxAngVelLength)
258 MagCondCount++;
259 else
260 MagCondCount = 0;
262 // Find, create, and utilize reference points for the magnetometer
263 // Need to be careful not to set reference points while there is significant tilt error
264 if ((EnableYawCorrection && MagCalibrated)&&(RunningTime > 10.0f)&&(TiltErrorAngle < 0.2f))
265 {
266 if (MagNumReferences == 0)
267 {
268 setMagReference(); // Use the current direction
269 }
270 else if (Q.Distance(MagRefQ) > MagRefDistance)
271 {
272 MagHasNearbyReference = false;
273 float bestDist = 100000.0f;
274 int bestNdx = 0;
275 float dist;
276 for (int i = 0; i < MagNumReferences; i++)
277 {
278 dist = Q.Distance(MagRefTableQ[i]);
279 if (dist < bestDist)
280 {
281 bestNdx = i;
282 bestDist = dist;
283 }
284 }
286 if (bestDist < MagRefDistance)
287 {
288 MagHasNearbyReference = true;
289 MagRefQ = MagRefTableQ[bestNdx];
290 MagRefM = MagRefTableM[bestNdx];
291 MagRefYaw = MagRefTableYaw[bestNdx];
292 //LogText("Using reference %d\n",bestNdx);
293 }
294 else if (MagNumReferences < MagMaxReferences)
295 setMagReference();
296 }
297 }
299 YawCorrectionInProgress = false;
300 if (EnableYawCorrection && MagCalibrated && (RunningTime > 2.0f) && (MagCondCount >= magWindow) &&
301 MagHasNearbyReference)
302 {
303 // Use rotational invariance to bring reference mag value into global frame
304 Vector3f grefmag = MagRefQ.Rotate(GetCalibratedMagValue(MagRefM));
305 // Bring current (averaged) mag reading into global frame
306 Vector3f gmag = Q.Rotate(GetCalibratedMagValue(FRawMag.Mean()));
307 // Calculate the reference yaw in the global frame
308 Anglef gryaw = Anglef(atan2(grefmag.x,grefmag.z));
309 // Calculate the current yaw in the global frame
310 Anglef gyaw = Anglef(atan2(gmag.x,gmag.z));
311 // The difference between reference and current yaws is the perceived error
312 Anglef YawErrorAngle = gyaw - gryaw;
314 //LogText("Yaw error estimate: %f\n",YawErrorAngle.Get());
315 // If the perceived error is large, keep count
316 if ((YawErrorAngle.Abs() > yawErrorMax) && (!YawCorrectionActivated))
317 YawErrorCount++;
318 // After enough iterations of high perceived error, start the correction process
319 if (YawErrorCount > yawErrorCountLimit)
320 YawCorrectionActivated = true;
321 // If the perceived error becomes small, turn off the yaw correction
322 if ((YawErrorAngle.Abs() < yawErrorMin) && YawCorrectionActivated)
323 {
324 YawCorrectionActivated = false;
325 YawErrorCount = 0;
326 }
328 // Perform the actual yaw correction, due to previously detected, large yaw error
329 if (YawCorrectionActivated)
330 {
331 YawCorrectionInProgress = true;
332 // Incrementally "unyaw" by a small step size
333 Q = Quatf(Vector3f(0.0f,1.0f,0.0f), -yawRotationStep * YawErrorAngle.Sign()) * Q;
334 }
335 }
336 }
339 // A predictive filter based on extrapolating the smoothed, current angular velocity
340 Quatf SensorFusion::GetPredictedOrientation(float pdt)
341 {
342 Lock::Locker lockScope(Handler.GetHandlerLock());
343 Quatf qP = QUncorrected;
345 if (EnablePrediction)
346 {
347 // This method assumes a constant angular velocity
348 Vector3f angVelF = FAngV.SavitzkyGolaySmooth8();
349 float angVelFL = angVelF.Length();
351 // Force back to raw measurement
352 angVelF = AngV;
353 angVelFL = AngV.Length();
355 // Dynamic prediction interval: Based on angular velocity to reduce vibration
356 const float minPdt = 0.001f;
357 const float slopePdt = 0.1f;
358 float newpdt = pdt;
359 float tpdt = minPdt + slopePdt * angVelFL;
360 if (tpdt < pdt)
361 newpdt = tpdt;
362 //LogText("PredictonDTs: %d\n",(int)(newpdt / PredictionTimeIncrement + 0.5f));
364 if (angVelFL > 0.001f)
365 {
366 Vector3f rotAxisP = angVelF / angVelFL;
367 float halfRotAngleP = angVelFL * newpdt * 0.5f;
368 float sinaHRAP = sin(halfRotAngleP);
369 Quatf deltaQP(rotAxisP.x*sinaHRAP, rotAxisP.y*sinaHRAP,
370 rotAxisP.z*sinaHRAP, cos(halfRotAngleP));
371 qP = QUncorrected * deltaQP;
372 }
373 }
374 return qP;
375 }
378 Vector3f SensorFusion::GetCalibratedMagValue(const Vector3f& rawMag) const
379 {
380 Vector3f mag = rawMag;
381 OVR_ASSERT(HasMagCalibration());
382 mag.x += MagCalibrationMatrix.M[0][3];
383 mag.y += MagCalibrationMatrix.M[1][3];
384 mag.z += MagCalibrationMatrix.M[2][3];
385 return mag;
386 }
389 void SensorFusion::setMagReference(const Quatf& q, const Vector3f& rawMag)
390 {
391 if (MagNumReferences < MagMaxReferences)
392 {
393 MagRefTableQ[MagNumReferences] = q;
394 MagRefTableM[MagNumReferences] = rawMag; //FRawMag.Mean();
396 //LogText("Inserting reference %d\n",MagNumReferences);
398 MagRefQ = q;
399 MagRefM = rawMag;
401 float pitch, roll, yaw;
402 Quatf q2 = q;
403 q2.GetEulerAngles<Axis_X, Axis_Z, Axis_Y>(&pitch, &roll, &yaw);
404 MagRefTableYaw[MagNumReferences] = yaw;
405 MagRefYaw = yaw;
407 MagNumReferences++;
409 MagHasNearbyReference = true;
410 }
411 }
414 SensorFusion::BodyFrameHandler::~BodyFrameHandler()
415 {
416 RemoveHandlerFromDevices();
417 }
419 void SensorFusion::BodyFrameHandler::OnMessage(const Message& msg)
420 {
421 if (msg.Type == Message_BodyFrame)
422 pFusion->handleMessage(static_cast<const MessageBodyFrame&>(msg));
423 if (pFusion->pDelegate)
424 pFusion->pDelegate->OnMessage(msg);
425 }
427 bool SensorFusion::BodyFrameHandler::SupportsMessageType(MessageType type) const
428 {
429 return (type == Message_BodyFrame);
430 }
432 // Writes the current calibration for a particular device to a device profile file
433 // sensor - the sensor that was calibrated
434 // cal_name - an optional name for the calibration or default if cal_name == NULL
435 bool SensorFusion::SaveMagCalibration(const char* calibrationName) const
436 {
437 if (CachedSensorInfo.SerialNumber[0] == NULL || !HasMagCalibration())
438 return false;
440 // A named calibration may be specified for calibration in different
441 // environments, otherwise the default calibration is used
442 if (calibrationName == NULL)
443 calibrationName = "default";
445 // Generate a mag calibration event
446 JSON* calibration = JSON::CreateObject();
447 // (hardcoded for now) the measurement and representation method
448 calibration->AddStringItem("Version", "1.0");
449 calibration->AddStringItem("Name", "default");
451 // time stamp the calibration
452 char time_str[64];
454 #ifdef OVR_OS_WIN32
455 struct tm caltime;
456 localtime_s(&caltime, &MagCalibrationTime);
457 strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", &caltime);
458 #else
459 struct tm* caltime;
460 caltime = localtime(&MagCalibrationTime);
461 strftime(time_str, 64, "%Y-%m-%d %H:%M:%S", caltime);
462 #endif
464 calibration->AddStringItem("Time", time_str);
466 // write the full calibration matrix
467 Matrix4f calmat = GetMagCalibration();
468 char matrix[128];
469 int pos = 0;
470 for (int r=0; r<4; r++)
471 {
472 for (int c=0; c<4; c++)
473 {
474 pos += (int)OVR_sprintf(matrix+pos, 128, "%g ", calmat.M[r][c]);
475 }
476 }
477 calibration->AddStringItem("Calibration", matrix);
480 String path = GetBaseOVRPath(true);
481 path += "/Devices.json";
483 // Look for a prexisting device file to edit
484 Ptr<JSON> root = *JSON::Load(path);
485 if (root)
486 { // Quick sanity check of the file type and format before we parse it
487 JSON* version = root->GetFirstItem();
488 if (version && version->Name == "Oculus Device Profile Version")
489 { // In the future I may need to check versioning to determine parse method
490 }
491 else
492 {
493 root->Release();
494 root = NULL;
495 }
496 }
498 JSON* device = NULL;
499 if (root)
500 {
501 device = root->GetFirstItem(); // skip the header
502 device = root->GetNextItem(device);
503 while (device)
504 { // Search for a previous calibration with the same name for this device
505 // and remove it before adding the new one
506 if (device->Name == "Device")
507 {
508 JSON* item = device->GetItemByName("Serial");
509 if (item && item->Value == CachedSensorInfo.SerialNumber)
510 { // found an entry for this device
511 item = device->GetNextItem(item);
512 while (item)
513 {
514 if (item->Name == "MagCalibration")
515 {
516 JSON* name = item->GetItemByName("Name");
517 if (name && name->Value == calibrationName)
518 { // found a calibration of the same name
519 item->RemoveNode();
520 item->Release();
521 break;
522 }
523 }
524 item = device->GetNextItem(item);
525 }
527 // update the auto-mag flag
528 item = device->GetItemByName("EnableYawCorrection");
529 if (item)
530 item->dValue = (double)EnableYawCorrection;
531 else
532 device->AddBoolItem("EnableYawCorrection", EnableYawCorrection);
534 break;
535 }
536 }
538 device = root->GetNextItem(device);
539 }
540 }
541 else
542 { // Create a new device root
543 root = *JSON::CreateObject();
544 root->AddStringItem("Oculus Device Profile Version", "1.0");
545 }
547 if (device == NULL)
548 {
549 device = JSON::CreateObject();
550 device->AddStringItem("Product", CachedSensorInfo.ProductName);
551 device->AddNumberItem("ProductID", CachedSensorInfo.ProductId);
552 device->AddStringItem("Serial", CachedSensorInfo.SerialNumber);
553 device->AddBoolItem("EnableYawCorrection", EnableYawCorrection);
555 root->AddItem("Device", device);
556 }
558 // Create and the add the new calibration event to the device
559 device->AddItem("MagCalibration", calibration);
561 return root->Save(path);
562 }
564 // Loads a saved calibration for the specified device from the device profile file
565 // sensor - the sensor that the calibration was saved for
566 // cal_name - an optional name for the calibration or the default if cal_name == NULL
567 bool SensorFusion::LoadMagCalibration(const char* calibrationName)
568 {
569 if (CachedSensorInfo.SerialNumber[0] == NULL)
570 return false;
572 // A named calibration may be specified for calibration in different
573 // environments, otherwise the default calibration is used
574 if (calibrationName == NULL)
575 calibrationName = "default";
577 String path = GetBaseOVRPath(true);
578 path += "/Devices.json";
580 // Load the device profiles
581 Ptr<JSON> root = *JSON::Load(path);
582 if (root == NULL)
583 return false;
585 // Quick sanity check of the file type and format before we parse it
586 JSON* version = root->GetFirstItem();
587 if (version && version->Name == "Oculus Device Profile Version")
588 { // In the future I may need to check versioning to determine parse method
589 }
590 else
591 {
592 return false;
593 }
595 bool autoEnableCorrection = false;
597 JSON* device = root->GetNextItem(version);
598 while (device)
599 { // Search for a previous calibration with the same name for this device
600 // and remove it before adding the new one
601 if (device->Name == "Device")
602 {
603 JSON* item = device->GetItemByName("Serial");
604 if (item && item->Value == CachedSensorInfo.SerialNumber)
605 { // found an entry for this device
607 JSON* autoyaw = device->GetItemByName("EnableYawCorrection");
608 if (autoyaw)
609 autoEnableCorrection = (autoyaw->dValue != 0);
611 item = device->GetNextItem(item);
612 while (item)
613 {
614 if (item->Name == "MagCalibration")
615 {
616 JSON* calibration = item;
617 JSON* name = calibration->GetItemByName("Name");
618 if (name && name->Value == calibrationName)
619 { // found a calibration with this name
621 time_t now;
622 time(&now);
624 // parse the calibration time
625 time_t calibration_time = now;
626 JSON* caltime = calibration->GetItemByName("Time");
627 if (caltime)
628 {
629 const char* caltime_str = caltime->Value.ToCStr();
631 tm ct;
632 memset(&ct, 0, sizeof(tm));
634 #ifdef OVR_OS_WIN32
635 struct tm nowtime;
636 localtime_s(&nowtime, &now);
637 ct.tm_isdst = nowtime.tm_isdst;
638 sscanf_s(caltime_str, "%d-%d-%d %d:%d:%d",
639 &ct.tm_year, &ct.tm_mon, &ct.tm_mday,
640 &ct.tm_hour, &ct.tm_min, &ct.tm_sec);
641 #else
642 struct tm* nowtime = localtime(&now);
643 ct.tm_isdst = nowtime->tm_isdst;
644 sscanf(caltime_str, "%d-%d-%d %d:%d:%d",
645 &ct.tm_year, &ct.tm_mon, &ct.tm_mday,
646 &ct.tm_hour, &ct.tm_min, &ct.tm_sec);
647 #endif
648 ct.tm_year -= 1900;
649 ct.tm_mon--;
650 calibration_time = mktime(&ct);
651 }
653 // parse the calibration matrix
654 JSON* cal = calibration->GetItemByName("Calibration");
655 if (cal)
656 {
657 const char* data_str = cal->Value.ToCStr();
658 Matrix4f calmat;
659 for (int r=0; r<4; r++)
660 {
661 for (int c=0; c<4; c++)
662 {
663 calmat.M[r][c] = (float)atof(data_str);
664 while (data_str && *data_str != ' ')
665 data_str++;
667 if (data_str)
668 data_str++;
669 }
670 }
672 SetMagCalibration(calmat);
673 MagCalibrationTime = calibration_time;
674 EnableYawCorrection = autoEnableCorrection;
676 return true;
677 }
678 }
679 }
680 item = device->GetNextItem(item);
681 }
683 break;
684 }
685 }
687 device = root->GetNextItem(device);
688 }
690 return false;
691 }
695 } // namespace OVR