nuclear@3: /************************************************************************************
nuclear@3: 
nuclear@3: PublicHeader:   OVR.h
nuclear@3: Filename    :   Util_MagCalibration.h
nuclear@3: Content     :   Procedures for calibrating the magnetometer
nuclear@3: Created     :   April 16, 2013
nuclear@3: Authors     :   Steve LaValle, Andrew Reisse
nuclear@3: 
nuclear@3: Copyright   :   Copyright 2013 Oculus VR, Inc. All Rights reserved.
nuclear@3: 
nuclear@3: Use of this software is subject to the terms of the Oculus license
nuclear@3: agreement provided at the time of installation or download, or which
nuclear@3: otherwise accompanies this software in either electronic or hard copy form.
nuclear@3: 
nuclear@3: *************************************************************************************/
nuclear@3: 
nuclear@3: #ifndef OVR_Util_MagCalibration_h
nuclear@3: #define OVR_Util_MagCalibration_h
nuclear@3: 
nuclear@3: #include "../OVR_SensorFusion.h"
nuclear@3: #include "../Kernel/OVR_String.h"
nuclear@3: #include "../Kernel/OVR_Log.h"
nuclear@3: 
nuclear@3: namespace OVR { namespace Util {
nuclear@3: 
nuclear@3: class MagCalibration
nuclear@3: {
nuclear@3: public:
nuclear@3:     enum MagStatus
nuclear@3:     {
nuclear@3:         Mag_Uninitialized = 0,
nuclear@3:         Mag_AutoCalibrating = 1,
nuclear@3:         Mag_ManuallyCalibrating = 2,
nuclear@3:         Mag_Calibrated  = 3
nuclear@3:     };
nuclear@3: 
nuclear@3:     MagCalibration() :
nuclear@3:         Stat(Mag_Uninitialized),
nuclear@3:         MinMagDistance(0.2f), MinQuatDistance(0.5f),
nuclear@3:         SampleCount(0)
nuclear@3:     {
nuclear@3:         MinMagDistanceSq = MinMagDistance * MinMagDistance;
nuclear@3:         MinQuatDistanceSq = MinQuatDistance * MinQuatDistance;
nuclear@3:         MinMagValues = Vector3f(10000.0f,10000.0f,10000.0f);
nuclear@3:         MaxMagValues = Vector3f(-10000.0f,-10000.0f,-10000.0f);
nuclear@3: 		MinQuatValues = Quatf(1.0f,1.0f,1.0f,1.0f);
nuclear@3: 		MaxQuatValues = Quatf(0.0f,0.0f,0.0f,0.0f);
nuclear@3: 		}
nuclear@3: 
nuclear@3:     // Methods that are useful for either auto or manual calibration
nuclear@3:     bool     IsUnitialized() const       { return Stat == Mag_Uninitialized; }
nuclear@3:     bool     IsCalibrated() const        { return Stat == Mag_Calibrated; }
nuclear@3:     int      NumberOfSamples() const     { return SampleCount; }
nuclear@3:     int      RequiredSampleCount() const { return 4; }
nuclear@3: 	void     AbortCalibration()
nuclear@3: 	{
nuclear@3:         Stat = Mag_Uninitialized;
nuclear@3:         SampleCount = 0;
nuclear@3: 	}
nuclear@3: 
nuclear@3:     void     ClearCalibration(SensorFusion& sf) 
nuclear@3:     {
nuclear@3:         Stat = Mag_Uninitialized;
nuclear@3:         SampleCount = 0;
nuclear@3:         sf.ClearMagCalibration();
nuclear@3: 	};
nuclear@3:   
nuclear@3:     // Methods for automatic magnetometer calibration
nuclear@3:     void     BeginAutoCalibration(SensorFusion& sf);
nuclear@3:     unsigned UpdateAutoCalibration(SensorFusion& sf);
nuclear@3:     bool     IsAutoCalibrating() const { return Stat == Mag_AutoCalibrating; }
nuclear@3: 
nuclear@3:     // Methods for building a manual (user-guided) calibraton procedure
nuclear@3:     void     BeginManualCalibration(SensorFusion& sf);
nuclear@3:     bool     IsAcceptableSample(const Quatf& q, const Vector3f& m);
nuclear@3:     bool     InsertIfAcceptable(const Quatf& q, const Vector3f& m);
nuclear@3:     // Returns true if successful, requiring that SampleCount = 4
nuclear@3:     bool     SetCalibration(SensorFusion& sf);
nuclear@3:     bool     IsManuallyCalibrating() const { return Stat == Mag_ManuallyCalibrating; }
nuclear@3: 
nuclear@3:     // This is the minimum acceptable distance (Euclidean) between raw
nuclear@3:     // magnetometer values to be acceptable for usage in calibration.
nuclear@3:     void SetMinMagDistance(float dist) 
nuclear@3:     { 
nuclear@3:         MinMagDistance = dist; 
nuclear@3:         MinMagDistanceSq = MinMagDistance * MinMagDistance;
nuclear@3:     }
nuclear@3: 
nuclear@3:     // The minimum acceptable distance (4D Euclidean) between orientations
nuclear@3:     // to be acceptable for calibration usage.
nuclear@3:     void SetMinQuatDistance(float dist) 
nuclear@3:     { 
nuclear@3:         MinQuatDistance = dist; 
nuclear@3:         MinQuatDistanceSq = MinQuatDistance * MinQuatDistance;
nuclear@3:     }
nuclear@3: 
nuclear@3:     // A result of the calibration, which is the center of a sphere that 
nuclear@3:     // roughly approximates the magnetometer data.
nuclear@3:     Vector3f GetMagCenter() const { return MagCenter; }
nuclear@3:     // Retrieves the full magnetometer calibration matrix
nuclear@3:     Matrix4f GetMagCalibration() const;
nuclear@3:     // Retrieves the range of each quaternion term during calibration
nuclear@3:     Quatf GetCalibrationQuatSpread() const { return QuatSpread; }
nuclear@3:     // Retrieves the range of each magnetometer term during calibration
nuclear@3:     Vector3f GetCalibrationMagSpread() const { return MagSpread; }
nuclear@3: 
nuclear@3: private:
nuclear@3:     // Determine the unique sphere through 4 non-coplanar points
nuclear@3:     Vector3f CalculateSphereCenter(const Vector3f& p1, const Vector3f& p2,
nuclear@3:                                    const Vector3f& p3, const Vector3f& p4);
nuclear@3: 
nuclear@3:     // Distance from p4 to the nearest point on a plane through p1, p2, p3
nuclear@3:     float PointToPlaneDistance(const Vector3f& p1, const Vector3f& p2,
nuclear@3:                                const Vector3f& p3, const Vector3f& p4);
nuclear@3: 
nuclear@3:     Vector3f MagCenter;
nuclear@3:     unsigned Stat;
nuclear@3:     float    MinMagDistance;
nuclear@3:     float    MinQuatDistance;
nuclear@3:     float    MinMagDistanceSq;
nuclear@3:     float    MinQuatDistanceSq;
nuclear@3: 	// For gathering statistics during calibration
nuclear@3: 	Vector3f    MinMagValues;
nuclear@3: 	Vector3f    MaxMagValues;
nuclear@3: 	Vector3f    MagSpread;
nuclear@3: 	Quatf		MinQuatValues;
nuclear@3: 	Quatf		MaxQuatValues;
nuclear@3: 	Quatf       QuatSpread;
nuclear@3: 
nuclear@3:     unsigned SampleCount;
nuclear@3:     Vector3f MagSamples[4];
nuclear@3:     Quatf    QuatSamples[4];
nuclear@3: 
nuclear@3: };
nuclear@3: 
nuclear@3: }}
nuclear@3: 
nuclear@3: #endif