diff options
author | Brad Davis <[email protected]> | 2013-10-13 20:28:58 -0700 |
---|---|---|
committer | Brad Davis <[email protected]> | 2013-10-13 20:28:58 -0700 |
commit | 672cdd0ef5455cd62a0d7f7eb6b9889f3ea35f21 (patch) | |
tree | f3454dc864242744aef1ea5474b5011556d0f388 /LibOVR/Src/Util | |
parent | 1408ed7b208c7a1cff1a2448fc890e9b8bd6dc4e (diff) |
Updating to cmake, glew, xrandr
Diffstat (limited to 'LibOVR/Src/Util')
-rw-r--r-- | LibOVR/Src/Util/Util_MagCalibration.cpp | 227 | ||||
-rw-r--r-- | LibOVR/Src/Util/Util_MagCalibration.h | 138 |
2 files changed, 0 insertions, 365 deletions
diff --git a/LibOVR/Src/Util/Util_MagCalibration.cpp b/LibOVR/Src/Util/Util_MagCalibration.cpp deleted file mode 100644 index 58b8c45..0000000 --- a/LibOVR/Src/Util/Util_MagCalibration.cpp +++ /dev/null @@ -1,227 +0,0 @@ -/************************************************************************************ - -Filename : Util_MagCalibration.cpp -Content : Procedures for calibrating the magnetometer -Created : April 16, 2013 -Authors : Steve LaValle, Andrew Reisse - -Copyright : Copyright 2013 Oculus VR, Inc. All Rights reserved. - -Use of this software is subject to the terms of the Oculus license -agreement provided at the time of installation or download, or which -otherwise accompanies this software in either electronic or hard copy form. - -*************************************************************************************/ - -#include "Util_MagCalibration.h" - -namespace OVR { namespace Util { - -void MagCalibration::BeginAutoCalibration(SensorFusion& sf) -{ - Stat = Mag_AutoCalibrating; - // This is a "hard" reset of the mag, so need to clear stored values - sf.ClearMagCalibration(); - SampleCount = 0; - - // reset the statistics - MinMagValues = Vector3f(10000.0f,10000.0f,10000.0f); - MaxMagValues = Vector3f(-10000.0f,-10000.0f,-10000.0f); - MinQuatValues = Quatf(1.0f,1.0f,1.0f,1.0f); - MaxQuatValues = Quatf(0.0f,0.0f,0.0f,0.0f); -} - -unsigned MagCalibration::UpdateAutoCalibration(SensorFusion& sf) -{ - if (Stat != Mag_AutoCalibrating) - return Stat; - - Quatf q = sf.GetOrientation(); - Vector3f m = sf.GetMagnetometer(); - - InsertIfAcceptable(q, m); - - if ((SampleCount == 4) && (Stat == Mag_AutoCalibrating)) - { - //LogText("Magnetometer Output Spread: %f %f %f\n",MagSpread.x,MagSpread.y,MagSpread.z); - //LogText("Quaternion Spread: %f %f %f %f\n",QuatSpread.x,QuatSpread.y,QuatSpread.z,QuatSpread.w); - SetCalibration(sf); - } - - return Stat; - -} - -void MagCalibration::BeginManualCalibration(SensorFusion& sf) -{ - Stat = Mag_ManuallyCalibrating; - sf.ClearMagCalibration(); - SampleCount = 0; -} - -bool MagCalibration::IsAcceptableSample(const Quatf& q, const Vector3f& m) -{ - switch (SampleCount) - { - // Initial sample is always acceptable - case 0: - return true; - break; - case 1: - return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&& - ((m - MagSamples[0]).LengthSq() > MinMagDistanceSq); - break; - case 2: - return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&& - (q.DistanceSq(QuatSamples[1]) > MinQuatDistanceSq)&& - ((m - MagSamples[0]).LengthSq() > MinMagDistanceSq)&& - ((m - MagSamples[1]).LengthSq() > MinMagDistanceSq); - break; - case 3: - return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&& - (q.DistanceSq(QuatSamples[1]) > MinQuatDistanceSq)&& - (q.DistanceSq(QuatSamples[2]) > MinQuatDistanceSq)&& - ((PointToPlaneDistance(MagSamples[0],MagSamples[1],MagSamples[2],m) > MinMagDistance)|| - (PointToPlaneDistance(MagSamples[1],MagSamples[2],m,MagSamples[0]) > MinMagDistance)|| - (PointToPlaneDistance(MagSamples[2],m,MagSamples[0],MagSamples[1]) > MinMagDistance)|| - (PointToPlaneDistance(m,MagSamples[0],MagSamples[1],MagSamples[2]) > MinMagDistance)); - } - - return false; -} - - -bool MagCalibration::InsertIfAcceptable(const Quatf& q, const Vector3f& m) -{ - // Update some statistics - if (m.x < MinMagValues.x) - MinMagValues.x = m.x; - if (m.y < MinMagValues.y) - MinMagValues.y = m.y; - if (m.z < MinMagValues.z) - MinMagValues.z = m.z; - if (m.x > MaxMagValues.x) - MaxMagValues.x = m.x; - if (m.y > MaxMagValues.y) - MaxMagValues.y = m.y; - if (m.z > MaxMagValues.z) - MaxMagValues.z = m.z; - if (q.x < MinQuatValues.x) - MinQuatValues.x = q.x; - if (q.y < MinQuatValues.y) - MinQuatValues.y = q.y; - if (q.z < MinQuatValues.z) - MinQuatValues.z = q.z; - if (q.w < MinQuatValues.w) - MinQuatValues.w = q.w; - if (q.x > MaxQuatValues.x) - MaxQuatValues.x = q.x; - if (q.y > MaxQuatValues.y) - MaxQuatValues.y = q.y; - if (q.z > MaxQuatValues.z) - MaxQuatValues.z = q.z; - if (q.w > MaxQuatValues.w) - MaxQuatValues.w = q.w; - MagSpread = MaxMagValues - MinMagValues; - QuatSpread = MaxQuatValues - MinQuatValues; - - if (IsAcceptableSample(q, m)) - { - MagSamples[SampleCount] = m; - QuatSamples[SampleCount] = q; - SampleCount++; - return true; - } - - return false; -} - -Matrix4f MagCalibration::GetMagCalibration() const -{ - Matrix4f calMat = Matrix4f(); - calMat.M[0][3] = -MagCenter.x; - calMat.M[1][3] = -MagCenter.y; - calMat.M[2][3] = -MagCenter.z; - return calMat; -} - -bool MagCalibration::SetCalibration(SensorFusion& sf) -{ - if (SampleCount < 4) - return false; - - MagCenter = CalculateSphereCenter(MagSamples[0],MagSamples[1],MagSamples[2],MagSamples[3]); - Matrix4f calMat = GetMagCalibration(); - sf.SetMagCalibration(calMat); - Stat = Mag_Calibrated; - //LogText("MagCenter: %f %f %f\n",MagCenter.x,MagCenter.y,MagCenter.z); - - return true; -} - - -// Calculate the center of a sphere that passes through p1, p2, p3, p4 -Vector3f MagCalibration::CalculateSphereCenter(const Vector3f& p1, const Vector3f& p2, - const Vector3f& p3, const Vector3f& p4) -{ - Matrix4f A; - int i; - Vector3f p[4]; - p[0] = p1; - p[1] = p2; - p[2] = p3; - p[3] = p4; - - for (i = 0; i < 4; i++) - { - A.M[i][0] = p[i].x; - A.M[i][1] = p[i].y; - A.M[i][2] = p[i].z; - A.M[i][3] = 1.0f; - } - float m11 = A.Determinant(); - OVR_ASSERT(m11 != 0.0f); - - for (i = 0; i < 4; i++) - { - A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z; - A.M[i][1] = p[i].y; - A.M[i][2] = p[i].z; - A.M[i][3] = 1.0f; - } - float m12 = A.Determinant(); - - for (i = 0; i < 4; i++) - { - A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z; - A.M[i][1] = p[i].x; - A.M[i][2] = p[i].z; - A.M[i][3] = 1.0f; - } - float m13 = A.Determinant(); - - for (i = 0; i < 4; i++) - { - A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z; - A.M[i][1] = p[i].x; - A.M[i][2] = p[i].y; - A.M[i][3] = 1.0f; - } - float m14 = A.Determinant(); - - float c = 0.5f / m11; - return Vector3f(c*m12, -c*m13, c*m14); -} - -// Distance from p4 to the nearest point on a plane through p1, p2, p3 -float MagCalibration::PointToPlaneDistance(const Vector3f& p1, const Vector3f& p2, - const Vector3f& p3, const Vector3f& p4) -{ - Vector3f v1 = p1 - p2; - Vector3f v2 = p1 - p3; - Vector3f planeNormal = v1.Cross(v2); - planeNormal.Normalize(); - return (fabs((planeNormal * p4) - planeNormal * p1)); -} - -}} diff --git a/LibOVR/Src/Util/Util_MagCalibration.h b/LibOVR/Src/Util/Util_MagCalibration.h deleted file mode 100644 index 1f8e8cb..0000000 --- a/LibOVR/Src/Util/Util_MagCalibration.h +++ /dev/null @@ -1,138 +0,0 @@ -/************************************************************************************ - -PublicHeader: OVR.h -Filename : Util_MagCalibration.h -Content : Procedures for calibrating the magnetometer -Created : April 16, 2013 -Authors : Steve LaValle, Andrew Reisse - -Copyright : Copyright 2013 Oculus VR, Inc. All Rights reserved. - -Use of this software is subject to the terms of the Oculus license -agreement provided at the time of installation or download, or which -otherwise accompanies this software in either electronic or hard copy form. - -*************************************************************************************/ - -#ifndef OVR_Util_MagCalibration_h -#define OVR_Util_MagCalibration_h - -#include "../OVR_SensorFusion.h" -#include "../Kernel/OVR_String.h" -#include "../Kernel/OVR_Log.h" - -namespace OVR { namespace Util { - -class MagCalibration -{ -public: - enum MagStatus - { - Mag_Uninitialized = 0, - Mag_AutoCalibrating = 1, - Mag_ManuallyCalibrating = 2, - Mag_Calibrated = 3 - }; - - MagCalibration() : - Stat(Mag_Uninitialized), - MinMagDistance(0.2f), MinQuatDistance(0.5f), - SampleCount(0) - { - MinMagDistanceSq = MinMagDistance * MinMagDistance; - MinQuatDistanceSq = MinQuatDistance * MinQuatDistance; - MinMagValues = Vector3f(10000.0f,10000.0f,10000.0f); - MaxMagValues = Vector3f(-10000.0f,-10000.0f,-10000.0f); - MinQuatValues = Quatf(1.0f,1.0f,1.0f,1.0f); - MaxQuatValues = Quatf(0.0f,0.0f,0.0f,0.0f); - } - - // Methods that are useful for either auto or manual calibration - bool IsUnitialized() const { return Stat == Mag_Uninitialized; } - bool IsCalibrated() const { return Stat == Mag_Calibrated; } - int NumberOfSamples() const { return SampleCount; } - int RequiredSampleCount() const { return 4; } - void AbortCalibration() - { - Stat = Mag_Uninitialized; - SampleCount = 0; - } - - void ClearCalibration(SensorFusion& sf) - { - Stat = Mag_Uninitialized; - SampleCount = 0; - sf.ClearMagCalibration(); - }; - - // Methods for automatic magnetometer calibration - void BeginAutoCalibration(SensorFusion& sf); - unsigned UpdateAutoCalibration(SensorFusion& sf); - bool IsAutoCalibrating() const { return Stat == Mag_AutoCalibrating; } - - // Methods for building a manual (user-guided) calibraton procedure - void BeginManualCalibration(SensorFusion& sf); - bool IsAcceptableSample(const Quatf& q, const Vector3f& m); - bool InsertIfAcceptable(const Quatf& q, const Vector3f& m); - // Returns true if successful, requiring that SampleCount = 4 - bool SetCalibration(SensorFusion& sf); - bool IsManuallyCalibrating() const { return Stat == Mag_ManuallyCalibrating; } - - // This is the minimum acceptable distance (Euclidean) between raw - // magnetometer values to be acceptable for usage in calibration. - void SetMinMagDistance(float dist) - { - MinMagDistance = dist; - MinMagDistanceSq = MinMagDistance * MinMagDistance; - } - - // The minimum acceptable distance (4D Euclidean) between orientations - // to be acceptable for calibration usage. - void SetMinQuatDistance(float dist) - { - MinQuatDistance = dist; - MinQuatDistanceSq = MinQuatDistance * MinQuatDistance; - } - - // A result of the calibration, which is the center of a sphere that - // roughly approximates the magnetometer data. - Vector3f GetMagCenter() const { return MagCenter; } - // Retrieves the full magnetometer calibration matrix - Matrix4f GetMagCalibration() const; - // Retrieves the range of each quaternion term during calibration - Quatf GetCalibrationQuatSpread() const { return QuatSpread; } - // Retrieves the range of each magnetometer term during calibration - Vector3f GetCalibrationMagSpread() const { return MagSpread; } - -private: - // Determine the unique sphere through 4 non-coplanar points - Vector3f CalculateSphereCenter(const Vector3f& p1, const Vector3f& p2, - const Vector3f& p3, const Vector3f& p4); - - // Distance from p4 to the nearest point on a plane through p1, p2, p3 - float PointToPlaneDistance(const Vector3f& p1, const Vector3f& p2, - const Vector3f& p3, const Vector3f& p4); - - Vector3f MagCenter; - unsigned Stat; - float MinMagDistance; - float MinQuatDistance; - float MinMagDistanceSq; - float MinQuatDistanceSq; - // For gathering statistics during calibration - Vector3f MinMagValues; - Vector3f MaxMagValues; - Vector3f MagSpread; - Quatf MinQuatValues; - Quatf MaxQuatValues; - Quatf QuatSpread; - - unsigned SampleCount; - Vector3f MagSamples[4]; - Quatf QuatSamples[4]; - -}; - -}} - -#endif |