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