summaryrefslogtreecommitdiffstats
path: root/LibOVR/Src/Util
diff options
context:
space:
mode:
Diffstat (limited to 'LibOVR/Src/Util')
-rw-r--r--LibOVR/Src/Util/Util_MagCalibration.cpp227
-rw-r--r--LibOVR/Src/Util/Util_MagCalibration.h138
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
generated by cgit v1.2.3 (git 2.39.1) at 2024-12-25 04:04:35 +0000