From e490c3c7f7bb5461cfa78a214827aa534fb43a3e Mon Sep 17 00:00:00 2001
From: Sven Gothel <sgothel@jausoft.com>
Date: Sat, 21 Mar 2015 21:19:34 +0100
Subject: Bump OculusVR RIFT SDK to 0.4.4
---
LibOVR/Src/OVR_SensorCalibration.cpp | 354 -----------------------------------
1 file changed, 354 deletions(-)
delete mode 100644 LibOVR/Src/OVR_SensorCalibration.cpp
(limited to 'LibOVR/Src/OVR_SensorCalibration.cpp')
diff --git a/LibOVR/Src/OVR_SensorCalibration.cpp b/LibOVR/Src/OVR_SensorCalibration.cpp
deleted file mode 100644
index 94fbb27..0000000
--- a/LibOVR/Src/OVR_SensorCalibration.cpp
+++ /dev/null
@@ -1,354 +0,0 @@
-/************************************************************************************
-
-Filename : OVR_SensorCalibration.cpp
-Content : Calibration data implementation for the IMU messages
-Created : January 28, 2014
-Authors : Max Katsev
-
-Copyright : Copyright 2014 Oculus VR, Inc. All Rights reserved.
-
-Licensed under the Oculus VR Rift SDK License Version 3.1 (the "License");
-you may not use the Oculus VR Rift SDK except in compliance with the License,
-which is provided at the time of installation or download, or which
-otherwise accompanies this software in either electronic or hard copy form.
-
-You may obtain a copy of the License at
-
-http://www.oculusvr.com/licenses/LICENSE-3.1
-
-Unless required by applicable law or agreed to in writing, the Oculus VR SDK
-distributed under the License is distributed on an "AS IS" BASIS,
-WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-See the License for the specific language governing permissions and
-limitations under the License.
-
-*************************************************************************************/
-
-#include "OVR_SensorCalibration.h"
-#include "Kernel/OVR_Log.h"
-#include "Kernel/OVR_Threads.h"
-#include <time.h>
-
-namespace OVR {
-
-using namespace Alg;
-
-const UByte VERSION = 2;
-const UByte MAX_COMPAT_VERSION = 15;
-
-SensorCalibration::SensorCalibration(SensorDevice* pSensor)
- : MagCalibrated(false), GyroFilter(6000), GyroAutoTemperature(0)
-{
- this->pSensor = pSensor;
-};
-
-void SensorCalibration::Initialize()
-{
- // read factory calibration
- pSensor->GetFactoryCalibration(&AccelOffset, &GyroAutoOffset, &AccelMatrix, &GyroMatrix, &GyroAutoTemperature);
-
- // if the headset has an autocalibrated offset, prefer it over the factory defaults
- GyroOffsetReport gyroReport;
- bool result = pSensor->GetGyroOffsetReport(&gyroReport);
- if (result && gyroReport.Version != GyroOffsetReport::Version_NoOffset)
- {
- GyroAutoOffset = (Vector3f) gyroReport.Offset;
- GyroAutoTemperature = (float) gyroReport.Temperature;
- }
-
- // read the temperature tables and prepare the interpolation structures
- result = pSensor->GetAllTemperatureReports(&TemperatureReports);
- OVR_ASSERT(result);
- for (int i = 0; i < 3; i++)
- Interpolators[i].Initialize(TemperatureReports, i);
-
- // read the mag calibration
- MagCalibrationReport report;
- result = pSensor->GetMagCalibrationReport(&report);
- MagCalibrated = result && report.Version > 0;
- MagMatrix = report.Calibration;
- if (!MagCalibrated)
- {
- // OVR_ASSERT(false);
- LogError("Magnetometer calibration not found!\n");
- }
-}
-
-void SensorCalibration::DebugPrintLocalTemperatureTable()
-{
- LogText("TemperatureReports:\n");
- for (int i = 0; i < (int)TemperatureReports.GetSize(); i++)
- {
- for (int j = 0; j < (int)TemperatureReports[i].GetSize(); j++)
- {
- TemperatureReport& tr = TemperatureReports[i][j];
-
- LogText("[%d][%d]: Version=%3d, Bin=%d/%d, "
- "Sample=%d/%d, TargetTemp=%3.1lf, "
- "ActualTemp=%4.1lf, "
- "Offset=(%7.2lf, %7.2lf, %7.2lf), "
- "Time=%d\n", i, j, tr.Version,
- tr.Bin, tr.NumBins,
- tr.Sample, tr.NumSamples,
- tr.TargetTemperature,
- tr.ActualTemperature,
- tr.Offset.x, tr.Offset.y, tr.Offset.z,
- tr.Time);
- }
- }
-}
-
-void SensorCalibration::DebugClearHeadsetTemperatureReports()
-{
- OVR_ASSERT(pSensor != NULL);
-
- bool result;
-
- Array<Array<TemperatureReport> > temperatureReports;
- pSensor->GetAllTemperatureReports(&temperatureReports);
-
- OVR_ASSERT(temperatureReports.GetSize() > 0);
- OVR_ASSERT(temperatureReports[0].GetSize() > 0);
-
- TemperatureReport& tr = TemperatureReports[0][0];
-
- tr.ActualTemperature = 0.0;
- tr.Time = 0;
- tr.Version = 0;
- tr.Offset.x = tr.Offset.y = tr.Offset.z = 0.0;
-
- for (UByte i = 0; i < tr.NumBins; i++)
- {
- tr.Bin = i;
-
- for (UByte j = 0; j < tr.NumSamples; j++)
- {
- tr.Sample = j;
-
- result = pSensor->SetTemperatureReport(tr);
- OVR_ASSERT(result);
-
- // Need to wait for the tracker board to finish writing to eeprom.
- Thread::MSleep(50);
- }
- }
-}
-
-void SensorCalibration::Apply(MessageBodyFrame& msg)
-{
- AutocalibrateGyro(msg);
-
- // compute the interpolated offset
- Vector3f gyroOffset;
- for (int i = 0; i < 3; i++)
- gyroOffset[i] = (float) Interpolators[i].GetOffset(msg.Temperature, GyroAutoTemperature, GyroAutoOffset[i]);
-
- // apply calibration
- msg.RotationRate = GyroMatrix.Transform(msg.RotationRate - gyroOffset);
- msg.Acceleration = AccelMatrix.Transform(msg.Acceleration - AccelOffset);
- if (MagCalibrated)
- msg.MagneticField = MagMatrix.Transform(msg.MagneticField);
-}
-
-void SensorCalibration::AutocalibrateGyro(MessageBodyFrame const& msg)
-{
- const float alpha = 0.4f;
- // 1.25f is a scaling factor related to conversion from per-axis comparison to length comparison
- const float absLimit = 1.25f * 0.349066f;
- const float noiseLimit = 1.25f * 0.03f;
-
- Vector3f gyro = msg.RotationRate;
- // do a moving average to reject short term noise
- Vector3f avg = (GyroFilter.IsEmpty()) ? gyro : gyro * alpha + GyroFilter.PeekBack() * (1 - alpha);
-
- // Make sure the absolute value is below what is likely motion
- // Make sure it is close enough to the current average that it is probably noise and not motion
- if (avg.Length() >= absLimit || (avg - GyroFilter.Mean()).Length() >= noiseLimit)
- GyroFilter.Clear();
- GyroFilter.PushBack(avg);
-
- // if had a reasonable number of samples already use it for the current offset
- if (GyroFilter.GetSize() > GyroFilter.GetCapacity() / 2)
- {
- GyroAutoOffset = GyroFilter.Mean();
- GyroAutoTemperature = msg.Temperature;
- // After ~6 seconds of no motion, use the average as the new zero rate offset
- if (GyroFilter.IsFull())
- StoreAutoOffset();
- }
-}
-
-void SensorCalibration::StoreAutoOffset()
-{
- const double maxDeltaT = 2.5;
- const double minExtraDeltaT = 0.5;
- const UInt32 minDelay = 24 * 3600; // 1 day in seconds
-
- // find the best bin
- UPInt binIdx = 0;
- for (UPInt i = 1; i < TemperatureReports.GetSize(); i++)
- if (Abs(GyroAutoTemperature - TemperatureReports[i][0].TargetTemperature) <
- Abs(GyroAutoTemperature - TemperatureReports[binIdx][0].TargetTemperature))
- binIdx = i;
-
- // find the oldest and newest samples
- // NB: uninitialized samples have Time == 0, so they will get picked as the oldest
- UPInt newestIdx = 0, oldestIdx = 0;
- for (UPInt i = 1; i < TemperatureReports[binIdx].GetSize(); i++)
- {
- // if the version is newer - do nothing
- if (TemperatureReports[binIdx][i].Version > VERSION)
- return;
- if (TemperatureReports[binIdx][i].Time > TemperatureReports[binIdx][newestIdx].Time)
- newestIdx = i;
- if (TemperatureReports[binIdx][i].Time < TemperatureReports[binIdx][oldestIdx].Time)
- oldestIdx = i;
- }
- TemperatureReport& oldestReport = TemperatureReports[binIdx][oldestIdx];
- TemperatureReport& newestReport = TemperatureReports[binIdx][newestIdx];
- OVR_ASSERT((oldestReport.Sample == 0 && newestReport.Sample == 0 && newestReport.Version == 0) ||
- oldestReport.Sample == (newestReport.Sample + 1) % newestReport.NumSamples);
-
- bool writeSuccess = false;
- UInt32 now = (UInt32) time(0);
- if (now - newestReport.Time > minDelay)
- {
- // only write a new sample if the temperature is close enough
- if (Abs(GyroAutoTemperature - oldestReport.TargetTemperature) < maxDeltaT)
- {
- oldestReport.Time = now;
- oldestReport.ActualTemperature = GyroAutoTemperature;
- oldestReport.Offset = (Vector3d) GyroAutoOffset;
- oldestReport.Version = VERSION;
- writeSuccess = pSensor->SetTemperatureReport(oldestReport);
- OVR_ASSERT(writeSuccess);
- }
- }
- else
- {
- // if the newest sample is too recent - _update_ it if significantly closer to the target temp
- if (Abs(GyroAutoTemperature - newestReport.TargetTemperature) + minExtraDeltaT
- < Abs(newestReport.ActualTemperature - newestReport.TargetTemperature))
- {
- // (do not update the time!)
- newestReport.ActualTemperature = GyroAutoTemperature;
- newestReport.Offset = (Vector3d) GyroAutoOffset;
- newestReport.Version = VERSION;
- writeSuccess = pSensor->SetTemperatureReport(newestReport);
- OVR_ASSERT(writeSuccess);
- }
- }
-
- // update the interpolators with the new data
- // this is not particularly expensive call and would only happen rarely
- // but if performance is a problem, it's possible to only recompute the data that has changed
- if (writeSuccess)
- for (int i = 0; i < 3; i++)
- Interpolators[i].Initialize(TemperatureReports, i);
-}
-
-const TemperatureReport& median(const Array<TemperatureReport>& temperatureReportsBin, int coord)
-{
- Array<double> values;
- values.Reserve(temperatureReportsBin.GetSize());
- for (unsigned i = 0; i < temperatureReportsBin.GetSize(); i++)
- if (temperatureReportsBin[i].ActualTemperature != 0)
- values.PushBack(temperatureReportsBin[i].Offset[coord]);
- if (values.GetSize() > 0)
- {
- double med = Median(values);
- // this is kind of a hack
- for (unsigned i = 0; i < temperatureReportsBin.GetSize(); i++)
- if (temperatureReportsBin[i].Offset[coord] == med)
- return temperatureReportsBin[i];
- // if we haven't found the median in the original array, something is wrong
- OVR_DEBUG_BREAK;
- }
- return temperatureReportsBin[0];
-}
-
-void OffsetInterpolator::Initialize(Array<Array<TemperatureReport> > const& temperatureReports, int coord)
-{
- int bins = (int) temperatureReports.GetSize();
- Temperatures.Clear();
- Temperatures.Reserve(bins);
- Values.Clear();
- Values.Reserve(bins);
-
- for (int bin = 0; bin < bins; bin++)
- {
- OVR_ASSERT(temperatureReports[bin].GetSize() == temperatureReports[0].GetSize());
- const TemperatureReport& report = median(temperatureReports[bin], coord);
- if (report.Version > 0 && report.Version <= MAX_COMPAT_VERSION)
- {
- Temperatures.PushBack(report.ActualTemperature);
- Values.PushBack(report.Offset[coord]);
- }
- }
-}
-
-double OffsetInterpolator::GetOffset(double targetTemperature, double autoTemperature, double autoValue)
-{
- const double autoRangeExtra = 1.0;
- const double minInterpolationDist = 0.5;
-
- // difference between current and autocalibrated temperature adjusted for preference over historical data
- const double adjustedDeltaT = Abs(autoTemperature - targetTemperature) - autoRangeExtra;
-
- int count = (int) Temperatures.GetSize();
- // handle special cases when we don't have enough data for proper interpolation
- if (count == 0)
- return autoValue;
- if (count == 1)
- {
- if (adjustedDeltaT < Abs(Temperatures[0] - targetTemperature))
- return autoValue;
- else
- return Values[0];
- }
-
- // first, find the interval that contains targetTemperature
- // if all points are on the same side of targetTemperature, find the adjacent interval
- int l;
- if (targetTemperature < Temperatures[1])
- l = 0;
- else if (targetTemperature >= Temperatures[count - 2])
- l = count - 2;
- else
- for (l = 1; l < count - 2; l++)
- if (Temperatures[l] <= targetTemperature && targetTemperature < Temperatures[l+1])
- break;
- int u = l + 1;
-
- // extend the interval if it's too small and the interpolation is unreliable
- if (Temperatures[u] - Temperatures[l] < minInterpolationDist)
- {
- if (l > 0
- && (u == count - 1 || Temperatures[u] - Temperatures[l - 1] < Temperatures[u + 1] - Temperatures[l]))
- l--;
- else if (u < count - 1)
- u++;
- }
-
- // verify correctness
- OVR_ASSERT(l >= 0 && u < count);
- OVR_ASSERT(l == 0 || Temperatures[l] <= targetTemperature);
- OVR_ASSERT(u == count - 1 || targetTemperature < Temperatures[u]);
- OVR_ASSERT((l == 0 && u == count - 1) || Temperatures[u] - Temperatures[l] > minInterpolationDist);
- OVR_ASSERT(Temperatures[l] <= Temperatures[u]);
-
- // perform the interpolation
- double slope;
- if (Temperatures[u] - Temperatures[l] >= minInterpolationDist)
- slope = (Values[u] - Values[l]) / (Temperatures[u] - Temperatures[l]);
- else
- // avoid a badly conditioned problem
- slope = 0;
- if (adjustedDeltaT < Abs(Temperatures[u] - targetTemperature))
- // use the autocalibrated value, if it's close
- return autoValue + slope * (targetTemperature - autoTemperature);
- else
- return Values[u] + slope * (targetTemperature - Temperatures[u]);
-}
-
-} // namespace OVR
--
cgit v1.2.3