oculus1

annotate libovr/Src/OVR_SensorFilter.cpp @ 26:75ab0d4ce2bb

find changesets by author, revision, files, or words in the commit message
backed out the shaderless oculus distortion method, as it's completely pointless
author John Tsiombikas <nuclear@member.fsf.org>
date Fri, 04 Oct 2013 14:57:33 +0300
parents
children
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