oculus1

annotate libovr/Src/Util/Util_MagCalibration.cpp @ 29:9a973ef0e2a3

find changesets by author, revision, files, or words in the commit message
fixed the performance issue under MacOSX by replacing glutSolidTeapot (which uses glEvalMesh) with my own teapot generator.
author John Tsiombikas <nuclear@member.fsf.org>
date Sun, 27 Oct 2013 06:31:18 +0200
parents e2f9e4603129
children
rev   line source
nuclear@3 1 /************************************************************************************
nuclear@3 2
nuclear@3 3 Filename : Util_MagCalibration.cpp
nuclear@3 4 Content : Procedures for calibrating the magnetometer
nuclear@3 5 Created : April 16, 2013
nuclear@3 6 Authors : Steve LaValle, Andrew Reisse
nuclear@3 7
nuclear@3 8 Copyright : Copyright 2013 Oculus VR, Inc. All Rights reserved.
nuclear@3 9
nuclear@3 10 Use of this software is subject to the terms of the Oculus license
nuclear@3 11 agreement provided at the time of installation or download, or which
nuclear@3 12 otherwise accompanies this software in either electronic or hard copy form.
nuclear@3 13
nuclear@3 14 *************************************************************************************/
nuclear@3 15
nuclear@3 16 #include "Util_MagCalibration.h"
nuclear@3 17
nuclear@3 18 namespace OVR { namespace Util {
nuclear@3 19
nuclear@3 20 void MagCalibration::BeginAutoCalibration(SensorFusion& sf)
nuclear@3 21 {
nuclear@3 22 Stat = Mag_AutoCalibrating;
nuclear@3 23 // This is a "hard" reset of the mag, so need to clear stored values
nuclear@3 24 sf.ClearMagCalibration();
nuclear@3 25 SampleCount = 0;
nuclear@3 26
nuclear@3 27 // reset the statistics
nuclear@3 28 MinMagValues = Vector3f(10000.0f,10000.0f,10000.0f);
nuclear@3 29 MaxMagValues = Vector3f(-10000.0f,-10000.0f,-10000.0f);
nuclear@3 30 MinQuatValues = Quatf(1.0f,1.0f,1.0f,1.0f);
nuclear@3 31 MaxQuatValues = Quatf(0.0f,0.0f,0.0f,0.0f);
nuclear@3 32 }
nuclear@3 33
nuclear@3 34 unsigned MagCalibration::UpdateAutoCalibration(SensorFusion& sf)
nuclear@3 35 {
nuclear@3 36 if (Stat != Mag_AutoCalibrating)
nuclear@3 37 return Stat;
nuclear@3 38
nuclear@3 39 Quatf q = sf.GetOrientation();
nuclear@3 40 Vector3f m = sf.GetMagnetometer();
nuclear@3 41
nuclear@3 42 InsertIfAcceptable(q, m);
nuclear@3 43
nuclear@3 44 if ((SampleCount == 4) && (Stat == Mag_AutoCalibrating))
nuclear@3 45 {
nuclear@3 46 //LogText("Magnetometer Output Spread: %f %f %f\n",MagSpread.x,MagSpread.y,MagSpread.z);
nuclear@3 47 //LogText("Quaternion Spread: %f %f %f %f\n",QuatSpread.x,QuatSpread.y,QuatSpread.z,QuatSpread.w);
nuclear@3 48 SetCalibration(sf);
nuclear@3 49 }
nuclear@3 50
nuclear@3 51 return Stat;
nuclear@3 52
nuclear@3 53 }
nuclear@3 54
nuclear@3 55 void MagCalibration::BeginManualCalibration(SensorFusion& sf)
nuclear@3 56 {
nuclear@3 57 Stat = Mag_ManuallyCalibrating;
nuclear@3 58 sf.ClearMagCalibration();
nuclear@3 59 SampleCount = 0;
nuclear@3 60 }
nuclear@3 61
nuclear@3 62 bool MagCalibration::IsAcceptableSample(const Quatf& q, const Vector3f& m)
nuclear@3 63 {
nuclear@3 64 switch (SampleCount)
nuclear@3 65 {
nuclear@3 66 // Initial sample is always acceptable
nuclear@3 67 case 0:
nuclear@3 68 return true;
nuclear@3 69 break;
nuclear@3 70 case 1:
nuclear@3 71 return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&&
nuclear@3 72 ((m - MagSamples[0]).LengthSq() > MinMagDistanceSq);
nuclear@3 73 break;
nuclear@3 74 case 2:
nuclear@3 75 return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&&
nuclear@3 76 (q.DistanceSq(QuatSamples[1]) > MinQuatDistanceSq)&&
nuclear@3 77 ((m - MagSamples[0]).LengthSq() > MinMagDistanceSq)&&
nuclear@3 78 ((m - MagSamples[1]).LengthSq() > MinMagDistanceSq);
nuclear@3 79 break;
nuclear@3 80 case 3:
nuclear@3 81 return (q.DistanceSq(QuatSamples[0]) > MinQuatDistanceSq)&&
nuclear@3 82 (q.DistanceSq(QuatSamples[1]) > MinQuatDistanceSq)&&
nuclear@3 83 (q.DistanceSq(QuatSamples[2]) > MinQuatDistanceSq)&&
nuclear@3 84 ((PointToPlaneDistance(MagSamples[0],MagSamples[1],MagSamples[2],m) > MinMagDistance)||
nuclear@3 85 (PointToPlaneDistance(MagSamples[1],MagSamples[2],m,MagSamples[0]) > MinMagDistance)||
nuclear@3 86 (PointToPlaneDistance(MagSamples[2],m,MagSamples[0],MagSamples[1]) > MinMagDistance)||
nuclear@3 87 (PointToPlaneDistance(m,MagSamples[0],MagSamples[1],MagSamples[2]) > MinMagDistance));
nuclear@3 88 }
nuclear@3 89
nuclear@3 90 return false;
nuclear@3 91 }
nuclear@3 92
nuclear@3 93
nuclear@3 94 bool MagCalibration::InsertIfAcceptable(const Quatf& q, const Vector3f& m)
nuclear@3 95 {
nuclear@3 96 // Update some statistics
nuclear@3 97 if (m.x < MinMagValues.x)
nuclear@3 98 MinMagValues.x = m.x;
nuclear@3 99 if (m.y < MinMagValues.y)
nuclear@3 100 MinMagValues.y = m.y;
nuclear@3 101 if (m.z < MinMagValues.z)
nuclear@3 102 MinMagValues.z = m.z;
nuclear@3 103 if (m.x > MaxMagValues.x)
nuclear@3 104 MaxMagValues.x = m.x;
nuclear@3 105 if (m.y > MaxMagValues.y)
nuclear@3 106 MaxMagValues.y = m.y;
nuclear@3 107 if (m.z > MaxMagValues.z)
nuclear@3 108 MaxMagValues.z = m.z;
nuclear@3 109 if (q.x < MinQuatValues.x)
nuclear@3 110 MinQuatValues.x = q.x;
nuclear@3 111 if (q.y < MinQuatValues.y)
nuclear@3 112 MinQuatValues.y = q.y;
nuclear@3 113 if (q.z < MinQuatValues.z)
nuclear@3 114 MinQuatValues.z = q.z;
nuclear@3 115 if (q.w < MinQuatValues.w)
nuclear@3 116 MinQuatValues.w = q.w;
nuclear@3 117 if (q.x > MaxQuatValues.x)
nuclear@3 118 MaxQuatValues.x = q.x;
nuclear@3 119 if (q.y > MaxQuatValues.y)
nuclear@3 120 MaxQuatValues.y = q.y;
nuclear@3 121 if (q.z > MaxQuatValues.z)
nuclear@3 122 MaxQuatValues.z = q.z;
nuclear@3 123 if (q.w > MaxQuatValues.w)
nuclear@3 124 MaxQuatValues.w = q.w;
nuclear@3 125 MagSpread = MaxMagValues - MinMagValues;
nuclear@3 126 QuatSpread = MaxQuatValues - MinQuatValues;
nuclear@3 127
nuclear@3 128 if (IsAcceptableSample(q, m))
nuclear@3 129 {
nuclear@3 130 MagSamples[SampleCount] = m;
nuclear@3 131 QuatSamples[SampleCount] = q;
nuclear@3 132 SampleCount++;
nuclear@3 133 return true;
nuclear@3 134 }
nuclear@3 135
nuclear@3 136 return false;
nuclear@3 137 }
nuclear@3 138
nuclear@3 139 Matrix4f MagCalibration::GetMagCalibration() const
nuclear@3 140 {
nuclear@3 141 Matrix4f calMat = Matrix4f();
nuclear@3 142 calMat.M[0][3] = -MagCenter.x;
nuclear@3 143 calMat.M[1][3] = -MagCenter.y;
nuclear@3 144 calMat.M[2][3] = -MagCenter.z;
nuclear@3 145 return calMat;
nuclear@3 146 }
nuclear@3 147
nuclear@3 148 bool MagCalibration::SetCalibration(SensorFusion& sf)
nuclear@3 149 {
nuclear@3 150 if (SampleCount < 4)
nuclear@3 151 return false;
nuclear@3 152
nuclear@3 153 MagCenter = CalculateSphereCenter(MagSamples[0],MagSamples[1],MagSamples[2],MagSamples[3]);
nuclear@3 154 Matrix4f calMat = GetMagCalibration();
nuclear@3 155 sf.SetMagCalibration(calMat);
nuclear@3 156 Stat = Mag_Calibrated;
nuclear@3 157 //LogText("MagCenter: %f %f %f\n",MagCenter.x,MagCenter.y,MagCenter.z);
nuclear@3 158
nuclear@3 159 return true;
nuclear@3 160 }
nuclear@3 161
nuclear@3 162
nuclear@3 163 // Calculate the center of a sphere that passes through p1, p2, p3, p4
nuclear@3 164 Vector3f MagCalibration::CalculateSphereCenter(const Vector3f& p1, const Vector3f& p2,
nuclear@3 165 const Vector3f& p3, const Vector3f& p4)
nuclear@3 166 {
nuclear@3 167 Matrix4f A;
nuclear@3 168 int i;
nuclear@3 169 Vector3f p[4];
nuclear@3 170 p[0] = p1;
nuclear@3 171 p[1] = p2;
nuclear@3 172 p[2] = p3;
nuclear@3 173 p[3] = p4;
nuclear@3 174
nuclear@3 175 for (i = 0; i < 4; i++)
nuclear@3 176 {
nuclear@3 177 A.M[i][0] = p[i].x;
nuclear@3 178 A.M[i][1] = p[i].y;
nuclear@3 179 A.M[i][2] = p[i].z;
nuclear@3 180 A.M[i][3] = 1.0f;
nuclear@3 181 }
nuclear@3 182 float m11 = A.Determinant();
nuclear@3 183 OVR_ASSERT(m11 != 0.0f);
nuclear@3 184
nuclear@3 185 for (i = 0; i < 4; i++)
nuclear@3 186 {
nuclear@3 187 A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z;
nuclear@3 188 A.M[i][1] = p[i].y;
nuclear@3 189 A.M[i][2] = p[i].z;
nuclear@3 190 A.M[i][3] = 1.0f;
nuclear@3 191 }
nuclear@3 192 float m12 = A.Determinant();
nuclear@3 193
nuclear@3 194 for (i = 0; i < 4; i++)
nuclear@3 195 {
nuclear@3 196 A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z;
nuclear@3 197 A.M[i][1] = p[i].x;
nuclear@3 198 A.M[i][2] = p[i].z;
nuclear@3 199 A.M[i][3] = 1.0f;
nuclear@3 200 }
nuclear@3 201 float m13 = A.Determinant();
nuclear@3 202
nuclear@3 203 for (i = 0; i < 4; i++)
nuclear@3 204 {
nuclear@3 205 A.M[i][0] = p[i].x*p[i].x + p[i].y*p[i].y + p[i].z*p[i].z;
nuclear@3 206 A.M[i][1] = p[i].x;
nuclear@3 207 A.M[i][2] = p[i].y;
nuclear@3 208 A.M[i][3] = 1.0f;
nuclear@3 209 }
nuclear@3 210 float m14 = A.Determinant();
nuclear@3 211
nuclear@3 212 float c = 0.5f / m11;
nuclear@3 213 return Vector3f(c*m12, -c*m13, c*m14);
nuclear@3 214 }
nuclear@3 215
nuclear@3 216 // Distance from p4 to the nearest point on a plane through p1, p2, p3
nuclear@3 217 float MagCalibration::PointToPlaneDistance(const Vector3f& p1, const Vector3f& p2,
nuclear@3 218 const Vector3f& p3, const Vector3f& p4)
nuclear@3 219 {
nuclear@3 220 Vector3f v1 = p1 - p2;
nuclear@3 221 Vector3f v2 = p1 - p3;
nuclear@3 222 Vector3f planeNormal = v1.Cross(v2);
nuclear@3 223 planeNormal.Normalize();
nuclear@3 224 return (fabs((planeNormal * p4) - planeNormal * p1));
nuclear@3 225 }
nuclear@3 226
nuclear@3 227 }}