Util_MagCalibration.cpp
Go to the documentation of this file.
00001 /************************************************************************************
00002 
00003 Filename    :   Util_MagCalibration.cpp
00004 Content     :   Procedures for calibrating the magnetometer
00005 Created     :   April 16, 2013
00006 Authors     :   Steve LaValle, Andrew Reisse
00007 
00008 Copyright   :   Copyright 2013 Oculus VR, Inc. All Rights reserved.
00009 
00010 Use of this software is subject to the terms of the Oculus license
00011 agreement provided at the time of installation or download, or which
00012 otherwise accompanies this software in either electronic or hard copy form.
00013 
00014 *************************************************************************************/
00015 
00016 #include "Util_MagCalibration.h"
00017 
00018 namespace OVR { namespace Util {
00019 
00020 void MagCalibration::BeginAutoCalibration(SensorFusion& sf)
00021 {
00022     Stat = Mag_AutoCalibrating;
00023     // This is a "hard" reset of the mag, so need to clear stored values
00024     sf.ClearMagCalibration();
00025     SampleCount = 0;
00026 }
00027 
00028 unsigned MagCalibration::UpdateAutoCalibration(SensorFusion& sf)
00029 {
00030     if (Stat != Mag_AutoCalibrating)
00031         return Stat;
00032 
00033     Quatf q = sf.GetOrientation();
00034     Vector3f m = sf.GetMagnetometer();
00035 
00036     InsertIfAcceptable(q, m);
00037 
00038     if ((SampleCount == 4) && (Stat == Mag_AutoCalibrating))
00039         SetCalibration(sf);
00040 
00041     return Stat;
00042 
00043 }
00044 
00045 void MagCalibration::BeginManualCalibration(SensorFusion& sf)
00046 {
00047     Stat = Mag_ManuallyCalibrating;
00048     sf.ClearMagCalibration();
00049     SampleCount = 0;
00050 }
00051 
00052 bool MagCalibration::IsAcceptableSample(const Quatf& q, const Vector3f& m)
00053 {
00054     switch (SampleCount)
00055     {
00056         // Initial sample is always acceptable
00057     case 0:
00058         return true;
00059         break;
00060     case 1:
00061         return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&&
00062                ((m - MagSamples[0]).LengthSq() > MinMagDistanceSq);
00063         break;
00064     case 2:
00065         return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&&
00066                (q.DistanceSq(QuatSamples[1]) > MinQuatDistanceSq)&&
00067                ((m - MagSamples[0]).LengthSq() > MinMagDistanceSq)&&
00068                ((m - MagSamples[1]).LengthSq() > MinMagDistanceSq);
00069         break;
00070     case 3:
00071         return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&&
00072                (q.DistanceSq(QuatSamples[1]) > MinQuatDistanceSq)&&
00073                (q.DistanceSq(QuatSamples[2]) > MinQuatDistanceSq)&&
00074                ((PointToPlaneDistance(MagSamples[0],MagSamples[1],MagSamples[2],m) > MinMagDistance)||
00075                 (PointToPlaneDistance(MagSamples[1],MagSamples[2],m,MagSamples[0]) > MinMagDistance)||
00076                 (PointToPlaneDistance(MagSamples[2],m,MagSamples[0],MagSamples[1]) > MinMagDistance)||
00077                 (PointToPlaneDistance(m,MagSamples[0],MagSamples[1],MagSamples[2]) > MinMagDistance));
00078     }
00079 
00080     return false;
00081 }
00082 
00083 
00084 bool MagCalibration::InsertIfAcceptable(const Quatf& q, const Vector3f& m)
00085 {
00086     if (IsAcceptableSample(q, m))
00087     {
00088         MagSamples[SampleCount] = m;
00089         QuatSamples[SampleCount] = q;
00090         SampleCount++;
00091         return true;
00092     }
00093 
00094     return false;
00095 }
00096 
00097 
00098 bool MagCalibration::SetCalibration(SensorFusion& sf)
00099 {
00100     if (SampleCount < 4)
00101         return false;
00102 
00103     MagCenter = CalculateSphereCenter(MagSamples[0],MagSamples[1],MagSamples[2],MagSamples[3]);
00104     Matrix4f calMat = Matrix4f();
00105     calMat.M[0][3] = -MagCenter.x;
00106     calMat.M[1][3] = -MagCenter.y;
00107     calMat.M[2][3] = -MagCenter.z;
00108     sf.SetMagCalibration(calMat);
00109     Stat = Mag_Calibrated;
00110     //LogText("MagCenter: %f %f %f\n",MagCenter.x,MagCenter.y,MagCenter.z);
00111 
00112     return true;
00113 }
00114 
00115 
00116 // Calculate the center of a sphere that passes through p1, p2, p3, p4
00117 Vector3f MagCalibration::CalculateSphereCenter(const Vector3f& p1, const Vector3f& p2,
00118                                                const Vector3f& p3, const Vector3f& p4) 
00119 {
00120     Matrix4f A;
00121     int i;
00122     Vector3f p[4];
00123     p[0] = p1;
00124     p[1] = p2;
00125     p[2] = p3;
00126     p[3] = p4;
00127 
00128     for (i = 0; i < 4; i++) 
00129     {
00130         A.M[i][0] = p[i].x;
00131         A.M[i][1] = p[i].y;
00132         A.M[i][2] = p[i].z;
00133         A.M[i][3] = 1.0f;
00134     }
00135     float m11 = A.Determinant();
00136     OVR_ASSERT(m11 != 0.0f);
00137 
00138     for (i = 0; i < 4; i++) 
00139     {
00140         A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z;
00141         A.M[i][1] = p[i].y;
00142         A.M[i][2] = p[i].z;
00143         A.M[i][3] = 1.0f;
00144     }
00145     float m12 = A.Determinant();
00146 
00147     for (i = 0; i < 4; i++) 
00148     {
00149         A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z;
00150         A.M[i][1] = p[i].x;
00151         A.M[i][2] = p[i].z;
00152         A.M[i][3] = 1.0f;
00153     }
00154     float m13 = A.Determinant();
00155 
00156     for (i = 0; i < 4; i++) 
00157     {
00158         A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z;
00159         A.M[i][1] = p[i].x;
00160         A.M[i][2] = p[i].y;
00161         A.M[i][3] = 1.0f;
00162     }
00163     float m14 = A.Determinant();
00164 
00165     float c = 0.5f / m11;
00166     return Vector3f(c*m12, -c*m13, c*m14);
00167 }
00168 
00169 // Distance from p4 to the nearest point on a plane through p1, p2, p3
00170 float MagCalibration::PointToPlaneDistance(const Vector3f& p1, const Vector3f& p2,
00171                                            const Vector3f& p3, const Vector3f& p4) 
00172 {
00173     Vector3f v1 = p1 - p2;
00174     Vector3f v2 = p1 - p3;
00175     Vector3f planeNormal = v1.Cross(v2);
00176     planeNormal.Normalize();
00177     return (fabs((planeNormal * p4) - planeNormal * p1));
00178 }
00179 
00180 }}


oculus_sdk
Author(s): Tully Foote
autogenerated on Thu Jun 6 2019 20:13:49