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