summaryrefslogtreecommitdiffstats
path: root/LibOVR/Src/Util/Util_MagCalibration.cpp
diff options
context:
space:
mode:
authorBrad Davis <[email protected]>2013-10-13 20:28:58 -0700
committerBrad Davis <[email protected]>2013-10-13 20:28:58 -0700
commit672cdd0ef5455cd62a0d7f7eb6b9889f3ea35f21 (patch)
treef3454dc864242744aef1ea5474b5011556d0f388 /LibOVR/Src/Util/Util_MagCalibration.cpp
parent1408ed7b208c7a1cff1a2448fc890e9b8bd6dc4e (diff)
Updating to cmake, glew, xrandr
Diffstat (limited to 'LibOVR/Src/Util/Util_MagCalibration.cpp')
-rw-r--r--LibOVR/Src/Util/Util_MagCalibration.cpp227
1 files changed, 0 insertions, 227 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));
-}
-
-}}