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
|
/************************************************************************************
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;
}
// 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; }
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;
unsigned SampleCount;
Vector3f MagSamples[4];
Quatf QuatSamples[4];
};
}}
#endif
|
