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