diff options
Diffstat (limited to 'LibOVR/Src/Util/Util_MagCalibration.cpp')
| -rw-r--r-- | LibOVR/Src/Util/Util_MagCalibration.cpp | 227 |
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)); -} - -}} |