1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
|
/************************************************************************************
PublicHeader: OVR.h
Filename : Util_MagCalibration.h
Content : Procedures for calibrating the magnetometer
Created : April 16, 2013
Authors : Steve LaValle, Andrew Reisse
Copyright : Copyright 2013 Oculus VR, Inc. All Rights reserved.
Use of this software is subject to the terms of the Oculus license
agreement provided at the time of installation or download, or which
otherwise accompanies this software in either electronic or hard copy form.
*************************************************************************************/
#ifndef OVR_Util_MagCalibration_h
#define OVR_Util_MagCalibration_h
#include "../OVR_SensorFusion.h"
#include "../Kernel/OVR_String.h"
#include "../Kernel/OVR_Log.h"
namespace OVR { namespace Util {
class MagCalibration
{
public:
enum MagStatus
{
Mag_Uninitialized = 0,
Mag_AutoCalibrating = 1,
Mag_ManuallyCalibrating = 2,
Mag_Calibrated = 3
};
MagCalibration() :
Stat(Mag_Uninitialized),
MinMagDistance(0.2f), MinQuatDistance(0.5f),
SampleCount(0)
{
MinMagDistanceSq = MinMagDistance * MinMagDistance;
MinQuatDistanceSq = MinQuatDistance * MinQuatDistance;
MinMagValues = Vector3f(10000.0f,10000.0f,10000.0f);
MaxMagValues = Vector3f(-10000.0f,-10000.0f,-10000.0f);
MinQuatValues = Quatf(1.0f,1.0f,1.0f,1.0f);
MaxQuatValues = Quatf(0.0f,0.0f,0.0f,0.0f);
}
// Methods that are useful for either auto or manual calibration
bool IsUnitialized() const { return Stat == Mag_Uninitialized; }
bool IsCalibrated() const { return Stat == Mag_Calibrated; }
int NumberOfSamples() const { return SampleCount; }
int RequiredSampleCount() const { return 4; }
void AbortCalibration()
{
Stat = Mag_Uninitialized;
SampleCount = 0;
}
void ClearCalibration(SensorFusion& sf)
{
Stat = Mag_Uninitialized;
SampleCount = 0;
sf.ClearMagCalibration();
};
// Methods for automatic magnetometer calibration
void BeginAutoCalibration(SensorFusion& sf);
unsigned UpdateAutoCalibration(SensorFusion& sf);
bool IsAutoCalibrating() const { return Stat == Mag_AutoCalibrating; }
// Methods for building a manual (user-guided) calibraton procedure
void BeginManualCalibration(SensorFusion& sf);
bool IsAcceptableSample(const Quatf& q, const Vector3f& m);
bool InsertIfAcceptable(const Quatf& q, const Vector3f& m);
// Returns true if successful, requiring that SampleCount = 4
bool SetCalibration(SensorFusion& sf);
bool IsManuallyCalibrating() const { return Stat == Mag_ManuallyCalibrating; }
// This is the minimum acceptable distance (Euclidean) between raw
// magnetometer values to be acceptable for usage in calibration.
void SetMinMagDistance(float dist)
{
MinMagDistance = dist;
MinMagDistanceSq = MinMagDistance * MinMagDistance;
}
// The minimum acceptable distance (4D Euclidean) between orientations
// to be acceptable for calibration usage.
void SetMinQuatDistance(float dist)
{
MinQuatDistance = dist;
MinQuatDistanceSq = MinQuatDistance * MinQuatDistance;
}
// A result of the calibration, which is the center of a sphere that
// roughly approximates the magnetometer data.
Vector3f GetMagCenter() const { return MagCenter; }
// Retrieves the full magnetometer calibration matrix
Matrix4f GetMagCalibration() const;
// Retrieves the range of each quaternion term during calibration
Quatf GetCalibrationQuatSpread() const { return QuatSpread; }
// Retrieves the range of each magnetometer term during calibration
Vector3f GetCalibrationMagSpread() const { return MagSpread; }
private:
// Determine the unique sphere through 4 non-coplanar points
Vector3f CalculateSphereCenter(const Vector3f& p1, const Vector3f& p2,
const Vector3f& p3, const Vector3f& p4);
// Distance from p4 to the nearest point on a plane through p1, p2, p3
float PointToPlaneDistance(const Vector3f& p1, const Vector3f& p2,
const Vector3f& p3, const Vector3f& p4);
Vector3f MagCenter;
unsigned Stat;
float MinMagDistance;
float MinQuatDistance;
float MinMagDistanceSq;
float MinQuatDistanceSq;
// For gathering statistics during calibration
Vector3f MinMagValues;
Vector3f MaxMagValues;
Vector3f MagSpread;
Quatf MinQuatValues;
Quatf MaxQuatValues;
Quatf QuatSpread;
unsigned SampleCount;
Vector3f MagSamples[4];
Quatf QuatSamples[4];
};
}}
#endif
|
