rev |
line source |
nuclear@1
|
1 /************************************************************************************
|
nuclear@1
|
2
|
nuclear@1
|
3 PublicHeader: OVR.h
|
nuclear@1
|
4 Filename : OVR_SensorFilter.cpp
|
nuclear@1
|
5 Content : Basic filtering of sensor data
|
nuclear@1
|
6 Created : March 7, 2013
|
nuclear@1
|
7 Authors : Steve LaValle, Anna Yershova
|
nuclear@1
|
8
|
nuclear@1
|
9 Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved.
|
nuclear@1
|
10
|
nuclear@1
|
11 Use of this software is subject to the terms of the Oculus license
|
nuclear@1
|
12 agreement provided at the time of installation or download, or which
|
nuclear@1
|
13 otherwise accompanies this software in either electronic or hard copy form.
|
nuclear@1
|
14
|
nuclear@1
|
15 *************************************************************************************/
|
nuclear@1
|
16
|
nuclear@1
|
17 #include "OVR_SensorFilter.h"
|
nuclear@1
|
18
|
nuclear@1
|
19 namespace OVR {
|
nuclear@1
|
20
|
nuclear@1
|
21 Vector3f SensorFilter::Total() const
|
nuclear@1
|
22 {
|
nuclear@1
|
23 Vector3f total = Vector3f(0.0f, 0.0f, 0.0f);
|
nuclear@1
|
24 for (int i = 0; i < Size; i++)
|
nuclear@1
|
25 total += Elements[i];
|
nuclear@1
|
26 return total;
|
nuclear@1
|
27 }
|
nuclear@1
|
28
|
nuclear@1
|
29 Vector3f SensorFilter::Mean() const
|
nuclear@1
|
30 {
|
nuclear@1
|
31 Vector3f total = Vector3f(0.0f, 0.0f, 0.0f);
|
nuclear@1
|
32 for (int i = 0; i < Size; i++)
|
nuclear@1
|
33 total += Elements[i];
|
nuclear@1
|
34 return total / (float) Size;
|
nuclear@1
|
35 }
|
nuclear@1
|
36
|
nuclear@1
|
37 Vector3f SensorFilter::Median() const
|
nuclear@1
|
38 {
|
nuclear@1
|
39 int half_window = (int) Size / 2;
|
nuclear@1
|
40 float sortx[MaxFilterSize];
|
nuclear@1
|
41 float resultx = 0.0f;
|
nuclear@1
|
42
|
nuclear@1
|
43 float sorty[MaxFilterSize];
|
nuclear@1
|
44 float resulty = 0.0f;
|
nuclear@1
|
45
|
nuclear@1
|
46 float sortz[MaxFilterSize];
|
nuclear@1
|
47 float resultz = 0.0f;
|
nuclear@1
|
48
|
nuclear@1
|
49 for (int i = 0; i < Size; i++)
|
nuclear@1
|
50 {
|
nuclear@1
|
51 sortx[i] = Elements[i].x;
|
nuclear@1
|
52 sorty[i] = Elements[i].y;
|
nuclear@1
|
53 sortz[i] = Elements[i].z;
|
nuclear@1
|
54 }
|
nuclear@1
|
55 for (int j = 0; j <= half_window; j++)
|
nuclear@1
|
56 {
|
nuclear@1
|
57 int minx = j;
|
nuclear@1
|
58 int miny = j;
|
nuclear@1
|
59 int minz = j;
|
nuclear@1
|
60 for (int k = j + 1; k < Size; k++)
|
nuclear@1
|
61 {
|
nuclear@1
|
62 if (sortx[k] < sortx[minx]) minx = k;
|
nuclear@1
|
63 if (sorty[k] < sorty[miny]) miny = k;
|
nuclear@1
|
64 if (sortz[k] < sortz[minz]) minz = k;
|
nuclear@1
|
65 }
|
nuclear@1
|
66 const float tempx = sortx[j];
|
nuclear@1
|
67 const float tempy = sorty[j];
|
nuclear@1
|
68 const float tempz = sortz[j];
|
nuclear@1
|
69 sortx[j] = sortx[minx];
|
nuclear@1
|
70 sortx[minx] = tempx;
|
nuclear@1
|
71
|
nuclear@1
|
72 sorty[j] = sorty[miny];
|
nuclear@1
|
73 sorty[miny] = tempy;
|
nuclear@1
|
74
|
nuclear@1
|
75 sortz[j] = sortz[minz];
|
nuclear@1
|
76 sortz[minz] = tempz;
|
nuclear@1
|
77 }
|
nuclear@1
|
78 resultx = sortx[half_window];
|
nuclear@1
|
79 resulty = sorty[half_window];
|
nuclear@1
|
80 resultz = sortz[half_window];
|
nuclear@1
|
81
|
nuclear@1
|
82 return Vector3f(resultx, resulty, resultz);
|
nuclear@1
|
83 }
|
nuclear@1
|
84
|
nuclear@1
|
85 // Only the diagonal of the covariance matrix.
|
nuclear@1
|
86 Vector3f SensorFilter::Variance() const
|
nuclear@1
|
87 {
|
nuclear@1
|
88 Vector3f mean = Mean();
|
nuclear@1
|
89 Vector3f total = Vector3f(0.0f, 0.0f, 0.0f);
|
nuclear@1
|
90 for (int i = 0; i < Size; i++)
|
nuclear@1
|
91 {
|
nuclear@1
|
92 total.x += (Elements[i].x - mean.x) * (Elements[i].x - mean.x);
|
nuclear@1
|
93 total.y += (Elements[i].y - mean.y) * (Elements[i].y - mean.y);
|
nuclear@1
|
94 total.z += (Elements[i].z - mean.z) * (Elements[i].z - mean.z);
|
nuclear@1
|
95 }
|
nuclear@1
|
96 return total / (float) Size;
|
nuclear@1
|
97 }
|
nuclear@1
|
98
|
nuclear@1
|
99 // Should be a 3x3 matrix returned, but OVR_math.h doesn't have one
|
nuclear@1
|
100 Matrix4f SensorFilter::Covariance() const
|
nuclear@1
|
101 {
|
nuclear@1
|
102 Vector3f mean = Mean();
|
nuclear@1
|
103 Matrix4f total = Matrix4f(0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0);
|
nuclear@1
|
104 for (int i = 0; i < Size; i++)
|
nuclear@1
|
105 {
|
nuclear@1
|
106 total.M[0][0] += (Elements[i].x - mean.x) * (Elements[i].x - mean.x);
|
nuclear@1
|
107 total.M[1][0] += (Elements[i].y - mean.y) * (Elements[i].x - mean.x);
|
nuclear@1
|
108 total.M[2][0] += (Elements[i].z - mean.z) * (Elements[i].x - mean.x);
|
nuclear@1
|
109 total.M[1][1] += (Elements[i].y - mean.y) * (Elements[i].y - mean.y);
|
nuclear@1
|
110 total.M[2][1] += (Elements[i].z - mean.z) * (Elements[i].y - mean.y);
|
nuclear@1
|
111 total.M[2][2] += (Elements[i].z - mean.z) * (Elements[i].z - mean.z);
|
nuclear@1
|
112 }
|
nuclear@1
|
113 total.M[0][1] = total.M[1][0];
|
nuclear@1
|
114 total.M[0][2] = total.M[2][0];
|
nuclear@1
|
115 total.M[1][2] = total.M[2][1];
|
nuclear@1
|
116 for (int i = 0; i < 3; i++)
|
nuclear@1
|
117 for (int j = 0; j < 3; j++)
|
nuclear@1
|
118 total.M[i][j] *= 1.0f / Size;
|
nuclear@1
|
119 return total;
|
nuclear@1
|
120 }
|
nuclear@1
|
121
|
nuclear@1
|
122 Vector3f SensorFilter::PearsonCoefficient() const
|
nuclear@1
|
123 {
|
nuclear@1
|
124 Matrix4f cov = Covariance();
|
nuclear@1
|
125 Vector3f pearson = Vector3f();
|
nuclear@1
|
126 pearson.x = cov.M[0][1]/(sqrt(cov.M[0][0])*sqrt(cov.M[1][1]));
|
nuclear@1
|
127 pearson.y = cov.M[1][2]/(sqrt(cov.M[1][1])*sqrt(cov.M[2][2]));
|
nuclear@1
|
128 pearson.z = cov.M[2][0]/(sqrt(cov.M[2][2])*sqrt(cov.M[0][0]));
|
nuclear@1
|
129
|
nuclear@1
|
130 return pearson;
|
nuclear@1
|
131 }
|
nuclear@1
|
132
|
nuclear@1
|
133
|
nuclear@1
|
134 Vector3f SensorFilter::SavitzkyGolaySmooth8() const
|
nuclear@1
|
135 {
|
nuclear@1
|
136 OVR_ASSERT(Size >= 8);
|
nuclear@1
|
137 return GetPrev(0)*0.41667f +
|
nuclear@1
|
138 GetPrev(1)*0.33333f +
|
nuclear@1
|
139 GetPrev(2)*0.25f +
|
nuclear@1
|
140 GetPrev(3)*0.16667f +
|
nuclear@1
|
141 GetPrev(4)*0.08333f -
|
nuclear@1
|
142 GetPrev(6)*0.08333f -
|
nuclear@1
|
143 GetPrev(7)*0.16667f;
|
nuclear@1
|
144 }
|
nuclear@1
|
145
|
nuclear@1
|
146
|
nuclear@1
|
147 Vector3f SensorFilter::SavitzkyGolayDerivative4() const
|
nuclear@1
|
148 {
|
nuclear@1
|
149 OVR_ASSERT(Size >= 4);
|
nuclear@1
|
150 return GetPrev(0)*0.3f +
|
nuclear@1
|
151 GetPrev(1)*0.1f -
|
nuclear@1
|
152 GetPrev(2)*0.1f -
|
nuclear@1
|
153 GetPrev(3)*0.3f;
|
nuclear@1
|
154 }
|
nuclear@1
|
155
|
nuclear@1
|
156 Vector3f SensorFilter::SavitzkyGolayDerivative5() const
|
nuclear@1
|
157 {
|
nuclear@1
|
158 OVR_ASSERT(Size >= 5);
|
nuclear@1
|
159 return GetPrev(0)*0.2f +
|
nuclear@1
|
160 GetPrev(1)*0.1f -
|
nuclear@1
|
161 GetPrev(3)*0.1f -
|
nuclear@1
|
162 GetPrev(4)*0.2f;
|
nuclear@1
|
163 }
|
nuclear@1
|
164
|
nuclear@1
|
165 Vector3f SensorFilter::SavitzkyGolayDerivative12() const
|
nuclear@1
|
166 {
|
nuclear@1
|
167 OVR_ASSERT(Size >= 12);
|
nuclear@1
|
168 return GetPrev(0)*0.03846f +
|
nuclear@1
|
169 GetPrev(1)*0.03147f +
|
nuclear@1
|
170 GetPrev(2)*0.02448f +
|
nuclear@1
|
171 GetPrev(3)*0.01748f +
|
nuclear@1
|
172 GetPrev(4)*0.01049f +
|
nuclear@1
|
173 GetPrev(5)*0.0035f -
|
nuclear@1
|
174 GetPrev(6)*0.0035f -
|
nuclear@1
|
175 GetPrev(7)*0.01049f -
|
nuclear@1
|
176 GetPrev(8)*0.01748f -
|
nuclear@1
|
177 GetPrev(9)*0.02448f -
|
nuclear@1
|
178 GetPrev(10)*0.03147f -
|
nuclear@1
|
179 GetPrev(11)*0.03846f;
|
nuclear@1
|
180 }
|
nuclear@1
|
181
|
nuclear@1
|
182 Vector3f SensorFilter::SavitzkyGolayDerivativeN(int n) const
|
nuclear@1
|
183 {
|
nuclear@1
|
184 OVR_ASSERT(Size >= n);
|
nuclear@1
|
185 int m = (n-1)/2;
|
nuclear@1
|
186 Vector3f result = Vector3f();
|
nuclear@1
|
187 for (int k = 1; k <= m; k++)
|
nuclear@1
|
188 {
|
nuclear@1
|
189 int ind1 = m - k;
|
nuclear@1
|
190 int ind2 = n - m + k - 1;
|
nuclear@1
|
191 result += (GetPrev(ind1) - GetPrev(ind2)) * (float) k;
|
nuclear@1
|
192 }
|
nuclear@1
|
193 float coef = 3.0f/(m*(m+1.0f)*(2.0f*m+1.0f));
|
nuclear@1
|
194 result = result*coef;
|
nuclear@1
|
195 return result;
|
nuclear@1
|
196 }
|
nuclear@1
|
197
|
nuclear@1
|
198
|
nuclear@1
|
199
|
nuclear@1
|
200
|
nuclear@1
|
201 } //namespace OVR |