nuclear@3: /************************************************************************************
nuclear@3: 
nuclear@3: Filename    :   Util_MagCalibration.cpp
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: #include "Util_MagCalibration.h"
nuclear@3: 
nuclear@3: namespace OVR { namespace Util {
nuclear@3: 
nuclear@3: void MagCalibration::BeginAutoCalibration(SensorFusion& sf)
nuclear@3: {
nuclear@3:     Stat = Mag_AutoCalibrating;
nuclear@3:     // This is a "hard" reset of the mag, so need to clear stored values
nuclear@3:     sf.ClearMagCalibration();
nuclear@3:     SampleCount = 0;
nuclear@3: 
nuclear@3:     // reset the statistics
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: unsigned MagCalibration::UpdateAutoCalibration(SensorFusion& sf)
nuclear@3: {
nuclear@3:     if (Stat != Mag_AutoCalibrating)
nuclear@3:         return Stat;
nuclear@3: 
nuclear@3:     Quatf q = sf.GetOrientation();
nuclear@3:     Vector3f m = sf.GetMagnetometer();
nuclear@3: 
nuclear@3:     InsertIfAcceptable(q, m);
nuclear@3: 
nuclear@3:     if ((SampleCount == 4) && (Stat == Mag_AutoCalibrating))
nuclear@3: 	{
nuclear@3: 		//LogText("Magnetometer Output Spread: %f %f %f\n",MagSpread.x,MagSpread.y,MagSpread.z);
nuclear@3: 		//LogText("Quaternion Spread: %f %f %f %f\n",QuatSpread.x,QuatSpread.y,QuatSpread.z,QuatSpread.w);
nuclear@3:         SetCalibration(sf);
nuclear@3: 	}
nuclear@3: 
nuclear@3:     return Stat;
nuclear@3: 
nuclear@3: }
nuclear@3: 
nuclear@3: void MagCalibration::BeginManualCalibration(SensorFusion& sf)
nuclear@3: {
nuclear@3:     Stat = Mag_ManuallyCalibrating;
nuclear@3:     sf.ClearMagCalibration();
nuclear@3:     SampleCount = 0;
nuclear@3: }
nuclear@3: 
nuclear@3: bool MagCalibration::IsAcceptableSample(const Quatf& q, const Vector3f& m)
nuclear@3: {
nuclear@3:     switch (SampleCount)
nuclear@3:     {
nuclear@3:         // Initial sample is always acceptable
nuclear@3:     case 0:
nuclear@3:         return true;
nuclear@3:         break;
nuclear@3:     case 1:
nuclear@3:         return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&&
nuclear@3:                ((m - MagSamples[0]).LengthSq() > MinMagDistanceSq);
nuclear@3:         break;
nuclear@3:     case 2:
nuclear@3:         return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&&
nuclear@3:                (q.DistanceSq(QuatSamples[1]) > MinQuatDistanceSq)&&
nuclear@3:                ((m - MagSamples[0]).LengthSq() > MinMagDistanceSq)&&
nuclear@3:                ((m - MagSamples[1]).LengthSq() > MinMagDistanceSq);
nuclear@3:         break;
nuclear@3:     case 3:
nuclear@3:         return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&&
nuclear@3:                (q.DistanceSq(QuatSamples[1]) > MinQuatDistanceSq)&&
nuclear@3:                (q.DistanceSq(QuatSamples[2]) > MinQuatDistanceSq)&&
nuclear@3:                ((PointToPlaneDistance(MagSamples[0],MagSamples[1],MagSamples[2],m) > MinMagDistance)||
nuclear@3:                 (PointToPlaneDistance(MagSamples[1],MagSamples[2],m,MagSamples[0]) > MinMagDistance)||
nuclear@3:                 (PointToPlaneDistance(MagSamples[2],m,MagSamples[0],MagSamples[1]) > MinMagDistance)||
nuclear@3:                 (PointToPlaneDistance(m,MagSamples[0],MagSamples[1],MagSamples[2]) > MinMagDistance));
nuclear@3:     }
nuclear@3: 
nuclear@3:     return false;
nuclear@3: }
nuclear@3: 
nuclear@3: 
nuclear@3: bool MagCalibration::InsertIfAcceptable(const Quatf& q, const Vector3f& m)
nuclear@3: {
nuclear@3: 	// Update some statistics
nuclear@3: 	if (m.x < MinMagValues.x)
nuclear@3: 		MinMagValues.x = m.x;
nuclear@3: 	if (m.y < MinMagValues.y)
nuclear@3: 		MinMagValues.y = m.y;
nuclear@3: 	if (m.z < MinMagValues.z)
nuclear@3: 		MinMagValues.z = m.z;
nuclear@3: 	if (m.x > MaxMagValues.x)
nuclear@3: 		MaxMagValues.x = m.x;
nuclear@3: 	if (m.y > MaxMagValues.y)
nuclear@3: 		MaxMagValues.y = m.y;
nuclear@3: 	if (m.z > MaxMagValues.z)
nuclear@3: 		MaxMagValues.z = m.z;
nuclear@3: 	if (q.x < MinQuatValues.x)
nuclear@3: 		MinQuatValues.x = q.x;
nuclear@3: 	if (q.y < MinQuatValues.y)
nuclear@3: 		MinQuatValues.y = q.y;
nuclear@3: 	if (q.z < MinQuatValues.z)
nuclear@3: 		MinQuatValues.z = q.z;
nuclear@3: 	if (q.w < MinQuatValues.w)
nuclear@3: 		MinQuatValues.w = q.w;
nuclear@3: 	if (q.x > MaxQuatValues.x)
nuclear@3: 		MaxQuatValues.x = q.x;
nuclear@3: 	if (q.y > MaxQuatValues.y)
nuclear@3: 		MaxQuatValues.y = q.y;
nuclear@3: 	if (q.z > MaxQuatValues.z)
nuclear@3: 		MaxQuatValues.z = q.z;
nuclear@3: 	if (q.w > MaxQuatValues.w)
nuclear@3: 		MaxQuatValues.w = q.w;
nuclear@3: 	MagSpread = MaxMagValues - MinMagValues;
nuclear@3: 	QuatSpread = MaxQuatValues - MinQuatValues;
nuclear@3: 
nuclear@3: 	if (IsAcceptableSample(q, m))
nuclear@3:     {
nuclear@3:         MagSamples[SampleCount] = m;
nuclear@3:         QuatSamples[SampleCount] = q;
nuclear@3:         SampleCount++;
nuclear@3:         return true;
nuclear@3:     }
nuclear@3: 
nuclear@3:     return false;
nuclear@3: }
nuclear@3: 
nuclear@3: Matrix4f MagCalibration::GetMagCalibration() const
nuclear@3: {
nuclear@3:     Matrix4f calMat = Matrix4f();
nuclear@3:     calMat.M[0][3] = -MagCenter.x;
nuclear@3:     calMat.M[1][3] = -MagCenter.y;
nuclear@3:     calMat.M[2][3] = -MagCenter.z;
nuclear@3:     return calMat;
nuclear@3: }
nuclear@3: 
nuclear@3: bool MagCalibration::SetCalibration(SensorFusion& sf)
nuclear@3: {
nuclear@3:     if (SampleCount < 4)
nuclear@3:         return false;
nuclear@3: 
nuclear@3:     MagCenter = CalculateSphereCenter(MagSamples[0],MagSamples[1],MagSamples[2],MagSamples[3]);
nuclear@3:     Matrix4f calMat = GetMagCalibration();
nuclear@3:     sf.SetMagCalibration(calMat);
nuclear@3:     Stat = Mag_Calibrated;
nuclear@3:     //LogText("MagCenter: %f %f %f\n",MagCenter.x,MagCenter.y,MagCenter.z);
nuclear@3: 
nuclear@3:     return true;
nuclear@3: }
nuclear@3: 
nuclear@3: 
nuclear@3: // Calculate the center of a sphere that passes through p1, p2, p3, p4
nuclear@3: Vector3f MagCalibration::CalculateSphereCenter(const Vector3f& p1, const Vector3f& p2,
nuclear@3:                                                const Vector3f& p3, const Vector3f& p4) 
nuclear@3: {
nuclear@3:     Matrix4f A;
nuclear@3:     int i;
nuclear@3:     Vector3f p[4];
nuclear@3:     p[0] = p1;
nuclear@3:     p[1] = p2;
nuclear@3:     p[2] = p3;
nuclear@3:     p[3] = p4;
nuclear@3: 
nuclear@3:     for (i = 0; i < 4; i++) 
nuclear@3:     {
nuclear@3:         A.M[i][0] = p[i].x;
nuclear@3:         A.M[i][1] = p[i].y;
nuclear@3:         A.M[i][2] = p[i].z;
nuclear@3:         A.M[i][3] = 1.0f;
nuclear@3:     }
nuclear@3:     float m11 = A.Determinant();
nuclear@3:     OVR_ASSERT(m11 != 0.0f);
nuclear@3: 
nuclear@3:     for (i = 0; i < 4; i++) 
nuclear@3:     {
nuclear@3:         A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z;
nuclear@3:         A.M[i][1] = p[i].y;
nuclear@3:         A.M[i][2] = p[i].z;
nuclear@3:         A.M[i][3] = 1.0f;
nuclear@3:     }
nuclear@3:     float m12 = A.Determinant();
nuclear@3: 
nuclear@3:     for (i = 0; i < 4; i++) 
nuclear@3:     {
nuclear@3:         A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z;
nuclear@3:         A.M[i][1] = p[i].x;
nuclear@3:         A.M[i][2] = p[i].z;
nuclear@3:         A.M[i][3] = 1.0f;
nuclear@3:     }
nuclear@3:     float m13 = A.Determinant();
nuclear@3: 
nuclear@3:     for (i = 0; i < 4; i++) 
nuclear@3:     {
nuclear@3:         A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z;
nuclear@3:         A.M[i][1] = p[i].x;
nuclear@3:         A.M[i][2] = p[i].y;
nuclear@3:         A.M[i][3] = 1.0f;
nuclear@3:     }
nuclear@3:     float m14 = A.Determinant();
nuclear@3: 
nuclear@3:     float c = 0.5f / m11;
nuclear@3:     return Vector3f(c*m12, -c*m13, c*m14);
nuclear@3: }
nuclear@3: 
nuclear@3: // Distance from p4 to the nearest point on a plane through p1, p2, p3
nuclear@3: float MagCalibration::PointToPlaneDistance(const Vector3f& p1, const Vector3f& p2,
nuclear@3:                                            const Vector3f& p3, const Vector3f& p4) 
nuclear@3: {
nuclear@3:     Vector3f v1 = p1 - p2;
nuclear@3:     Vector3f v2 = p1 - p3;
nuclear@3:     Vector3f planeNormal = v1.Cross(v2);
nuclear@3:     planeNormal.Normalize();
nuclear@3:     return (fabs((planeNormal * p4) - planeNormal * p1));
nuclear@3: }
nuclear@3: 
nuclear@3: }}