1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/libovr/Src/Util/Util_MagCalibration.cpp	Sat Sep 14 16:14:59 2013 +0300
     1.3 @@ -0,0 +1,1 @@
     1.4 +/************************************************************************************
     1.5 
     1.6 Filename    :   Util_MagCalibration.cpp
     1.7 Content     :   Procedures for calibrating the magnetometer
     1.8 Created     :   April 16, 2013
     1.9 Authors     :   Steve LaValle, Andrew Reisse
    1.10 
    1.11 Copyright   :   Copyright 2013 Oculus VR, Inc. All Rights reserved.
    1.12 
    1.13 Use of this software is subject to the terms of the Oculus license
    1.14 agreement provided at the time of installation or download, or which
    1.15 otherwise accompanies this software in either electronic or hard copy form.
    1.16 
    1.17 *************************************************************************************/
    1.18 
    1.19 #include "Util_MagCalibration.h"
    1.20 
    1.21 namespace OVR { namespace Util {
    1.22 
    1.23 void MagCalibration::BeginAutoCalibration(SensorFusion& sf)
    1.24 {
    1.25     Stat = Mag_AutoCalibrating;
    1.26     // This is a "hard" reset of the mag, so need to clear stored values
    1.27     sf.ClearMagCalibration();
    1.28     SampleCount = 0;
    1.29 
    1.30     // reset the statistics
    1.31     MinMagValues = Vector3f(10000.0f,10000.0f,10000.0f);
    1.32     MaxMagValues = Vector3f(-10000.0f,-10000.0f,-10000.0f);
    1.33 	MinQuatValues = Quatf(1.0f,1.0f,1.0f,1.0f);
    1.34 	MaxQuatValues = Quatf(0.0f,0.0f,0.0f,0.0f);
    1.35 }
    1.36 
    1.37 unsigned MagCalibration::UpdateAutoCalibration(SensorFusion& sf)
    1.38 {
    1.39     if (Stat != Mag_AutoCalibrating)
    1.40         return Stat;
    1.41 
    1.42     Quatf q = sf.GetOrientation();
    1.43     Vector3f m = sf.GetMagnetometer();
    1.44 
    1.45     InsertIfAcceptable(q, m);
    1.46 
    1.47     if ((SampleCount == 4) && (Stat == Mag_AutoCalibrating))
    1.48 	{
    1.49 		//LogText("Magnetometer Output Spread: %f %f %f\n",MagSpread.x,MagSpread.y,MagSpread.z);
    1.50 		//LogText("Quaternion Spread: %f %f %f %f\n",QuatSpread.x,QuatSpread.y,QuatSpread.z,QuatSpread.w);
    1.51         SetCalibration(sf);
    1.52 	}
    1.53 
    1.54     return Stat;
    1.55 
    1.56 }
    1.57 
    1.58 void MagCalibration::BeginManualCalibration(SensorFusion& sf)
    1.59 {
    1.60     Stat = Mag_ManuallyCalibrating;
    1.61     sf.ClearMagCalibration();
    1.62     SampleCount = 0;
    1.63 }
    1.64 
    1.65 bool MagCalibration::IsAcceptableSample(const Quatf& q, const Vector3f& m)
    1.66 {
    1.67     switch (SampleCount)
    1.68     {
    1.69         // Initial sample is always acceptable
    1.70     case 0:
    1.71         return true;
    1.72         break;
    1.73     case 1:
    1.74         return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&&
    1.75                ((m - MagSamples[0]).LengthSq() > MinMagDistanceSq);
    1.76         break;
    1.77     case 2:
    1.78         return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&&
    1.79                (q.DistanceSq(QuatSamples[1]) > MinQuatDistanceSq)&&
    1.80                ((m - MagSamples[0]).LengthSq() > MinMagDistanceSq)&&
    1.81                ((m - MagSamples[1]).LengthSq() > MinMagDistanceSq);
    1.82         break;
    1.83     case 3:
    1.84         return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&&
    1.85                (q.DistanceSq(QuatSamples[1]) > MinQuatDistanceSq)&&
    1.86                (q.DistanceSq(QuatSamples[2]) > MinQuatDistanceSq)&&
    1.87                ((PointToPlaneDistance(MagSamples[0],MagSamples[1],MagSamples[2],m) > MinMagDistance)||
    1.88                 (PointToPlaneDistance(MagSamples[1],MagSamples[2],m,MagSamples[0]) > MinMagDistance)||
    1.89                 (PointToPlaneDistance(MagSamples[2],m,MagSamples[0],MagSamples[1]) > MinMagDistance)||
    1.90                 (PointToPlaneDistance(m,MagSamples[0],MagSamples[1],MagSamples[2]) > MinMagDistance));
    1.91     }
    1.92 
    1.93     return false;
    1.94 }
    1.95 
    1.96 
    1.97 bool MagCalibration::InsertIfAcceptable(const Quatf& q, const Vector3f& m)
    1.98 {
    1.99 	// Update some statistics
   1.100 	if (m.x < MinMagValues.x)
   1.101 		MinMagValues.x = m.x;
   1.102 	if (m.y < MinMagValues.y)
   1.103 		MinMagValues.y = m.y;
   1.104 	if (m.z < MinMagValues.z)
   1.105 		MinMagValues.z = m.z;
   1.106 	if (m.x > MaxMagValues.x)
   1.107 		MaxMagValues.x = m.x;
   1.108 	if (m.y > MaxMagValues.y)
   1.109 		MaxMagValues.y = m.y;
   1.110 	if (m.z > MaxMagValues.z)
   1.111 		MaxMagValues.z = m.z;
   1.112 	if (q.x < MinQuatValues.x)
   1.113 		MinQuatValues.x = q.x;
   1.114 	if (q.y < MinQuatValues.y)
   1.115 		MinQuatValues.y = q.y;
   1.116 	if (q.z < MinQuatValues.z)
   1.117 		MinQuatValues.z = q.z;
   1.118 	if (q.w < MinQuatValues.w)
   1.119 		MinQuatValues.w = q.w;
   1.120 	if (q.x > MaxQuatValues.x)
   1.121 		MaxQuatValues.x = q.x;
   1.122 	if (q.y > MaxQuatValues.y)
   1.123 		MaxQuatValues.y = q.y;
   1.124 	if (q.z > MaxQuatValues.z)
   1.125 		MaxQuatValues.z = q.z;
   1.126 	if (q.w > MaxQuatValues.w)
   1.127 		MaxQuatValues.w = q.w;
   1.128 	MagSpread = MaxMagValues - MinMagValues;
   1.129 	QuatSpread = MaxQuatValues - MinQuatValues;
   1.130 
   1.131 	if (IsAcceptableSample(q, m))
   1.132     {
   1.133         MagSamples[SampleCount] = m;
   1.134         QuatSamples[SampleCount] = q;
   1.135         SampleCount++;
   1.136         return true;
   1.137     }
   1.138 
   1.139     return false;
   1.140 }
   1.141 
   1.142 Matrix4f MagCalibration::GetMagCalibration() const
   1.143 {
   1.144     Matrix4f calMat = Matrix4f();
   1.145     calMat.M[0][3] = -MagCenter.x;
   1.146     calMat.M[1][3] = -MagCenter.y;
   1.147     calMat.M[2][3] = -MagCenter.z;
   1.148     return calMat;
   1.149 }
   1.150 
   1.151 bool MagCalibration::SetCalibration(SensorFusion& sf)
   1.152 {
   1.153     if (SampleCount < 4)
   1.154         return false;
   1.155 
   1.156     MagCenter = CalculateSphereCenter(MagSamples[0],MagSamples[1],MagSamples[2],MagSamples[3]);
   1.157     Matrix4f calMat = GetMagCalibration();
   1.158     sf.SetMagCalibration(calMat);
   1.159     Stat = Mag_Calibrated;
   1.160     //LogText("MagCenter: %f %f %f\n",MagCenter.x,MagCenter.y,MagCenter.z);
   1.161 
   1.162     return true;
   1.163 }
   1.164 
   1.165 
   1.166 // Calculate the center of a sphere that passes through p1, p2, p3, p4
   1.167 Vector3f MagCalibration::CalculateSphereCenter(const Vector3f& p1, const Vector3f& p2,
   1.168                                                const Vector3f& p3, const Vector3f& p4) 
   1.169 {
   1.170     Matrix4f A;
   1.171     int i;
   1.172     Vector3f p[4];
   1.173     p[0] = p1;
   1.174     p[1] = p2;
   1.175     p[2] = p3;
   1.176     p[3] = p4;
   1.177 
   1.178     for (i = 0; i < 4; i++) 
   1.179     {
   1.180         A.M[i][0] = p[i].x;
   1.181         A.M[i][1] = p[i].y;
   1.182         A.M[i][2] = p[i].z;
   1.183         A.M[i][3] = 1.0f;
   1.184     }
   1.185     float m11 = A.Determinant();
   1.186     OVR_ASSERT(m11 != 0.0f);
   1.187 
   1.188     for (i = 0; i < 4; i++) 
   1.189     {
   1.190         A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z;
   1.191         A.M[i][1] = p[i].y;
   1.192         A.M[i][2] = p[i].z;
   1.193         A.M[i][3] = 1.0f;
   1.194     }
   1.195     float m12 = A.Determinant();
   1.196 
   1.197     for (i = 0; i < 4; i++) 
   1.198     {
   1.199         A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z;
   1.200         A.M[i][1] = p[i].x;
   1.201         A.M[i][2] = p[i].z;
   1.202         A.M[i][3] = 1.0f;
   1.203     }
   1.204     float m13 = A.Determinant();
   1.205 
   1.206     for (i = 0; i < 4; i++) 
   1.207     {
   1.208         A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z;
   1.209         A.M[i][1] = p[i].x;
   1.210         A.M[i][2] = p[i].y;
   1.211         A.M[i][3] = 1.0f;
   1.212     }
   1.213     float m14 = A.Determinant();
   1.214 
   1.215     float c = 0.5f / m11;
   1.216     return Vector3f(c*m12, -c*m13, c*m14);
   1.217 }
   1.218 
   1.219 // Distance from p4 to the nearest point on a plane through p1, p2, p3
   1.220 float MagCalibration::PointToPlaneDistance(const Vector3f& p1, const Vector3f& p2,
   1.221                                            const Vector3f& p3, const Vector3f& p4) 
   1.222 {
   1.223     Vector3f v1 = p1 - p2;
   1.224     Vector3f v2 = p1 - p3;
   1.225     Vector3f planeNormal = v1.Cross(v2);
   1.226     planeNormal.Normalize();
   1.227     return (fabs((planeNormal * p4) - planeNormal * p1));
   1.228 }
   1.229 
   1.230 }}
   1.231 \ No newline at end of file