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