nuclear@1: /************************************************************************************ nuclear@1: nuclear@1: PublicHeader: OVR.h nuclear@1: Filename : OVR_SensorFilter.cpp nuclear@1: Content : Basic filtering of sensor data nuclear@1: Created : March 7, 2013 nuclear@1: Authors : Steve LaValle, Anna Yershova nuclear@1: nuclear@1: Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved. nuclear@1: nuclear@1: Use of this software is subject to the terms of the Oculus license nuclear@1: agreement provided at the time of installation or download, or which nuclear@1: otherwise accompanies this software in either electronic or hard copy form. nuclear@1: nuclear@1: *************************************************************************************/ nuclear@1: nuclear@1: #include "OVR_SensorFilter.h" nuclear@1: nuclear@1: namespace OVR { nuclear@1: nuclear@1: Vector3f SensorFilter::Total() const nuclear@1: { nuclear@1: Vector3f total = Vector3f(0.0f, 0.0f, 0.0f); nuclear@1: for (int i = 0; i < Size; i++) nuclear@1: total += Elements[i]; nuclear@1: return total; nuclear@1: } nuclear@1: nuclear@1: Vector3f SensorFilter::Mean() const nuclear@1: { nuclear@1: Vector3f total = Vector3f(0.0f, 0.0f, 0.0f); nuclear@1: for (int i = 0; i < Size; i++) nuclear@1: total += Elements[i]; nuclear@1: return total / (float) Size; nuclear@1: } nuclear@1: nuclear@1: Vector3f SensorFilter::Median() const nuclear@1: { nuclear@1: int half_window = (int) Size / 2; nuclear@1: float sortx[MaxFilterSize]; nuclear@1: float resultx = 0.0f; nuclear@1: nuclear@1: float sorty[MaxFilterSize]; nuclear@1: float resulty = 0.0f; nuclear@1: nuclear@1: float sortz[MaxFilterSize]; nuclear@1: float resultz = 0.0f; nuclear@1: nuclear@1: for (int i = 0; i < Size; i++) nuclear@1: { nuclear@1: sortx[i] = Elements[i].x; nuclear@1: sorty[i] = Elements[i].y; nuclear@1: sortz[i] = Elements[i].z; nuclear@1: } nuclear@1: for (int j = 0; j <= half_window; j++) nuclear@1: { nuclear@1: int minx = j; nuclear@1: int miny = j; nuclear@1: int minz = j; nuclear@1: for (int k = j + 1; k < Size; k++) nuclear@1: { nuclear@1: if (sortx[k] < sortx[minx]) minx = k; nuclear@1: if (sorty[k] < sorty[miny]) miny = k; nuclear@1: if (sortz[k] < sortz[minz]) minz = k; nuclear@1: } nuclear@1: const float tempx = sortx[j]; nuclear@1: const float tempy = sorty[j]; nuclear@1: const float tempz = sortz[j]; nuclear@1: sortx[j] = sortx[minx]; nuclear@1: sortx[minx] = tempx; nuclear@1: nuclear@1: sorty[j] = sorty[miny]; nuclear@1: sorty[miny] = tempy; nuclear@1: nuclear@1: sortz[j] = sortz[minz]; nuclear@1: sortz[minz] = tempz; nuclear@1: } nuclear@1: resultx = sortx[half_window]; nuclear@1: resulty = sorty[half_window]; nuclear@1: resultz = sortz[half_window]; nuclear@1: nuclear@1: return Vector3f(resultx, resulty, resultz); nuclear@1: } nuclear@1: nuclear@1: // Only the diagonal of the covariance matrix. nuclear@1: Vector3f SensorFilter::Variance() const nuclear@1: { nuclear@1: Vector3f mean = Mean(); nuclear@1: Vector3f total = Vector3f(0.0f, 0.0f, 0.0f); nuclear@1: for (int i = 0; i < Size; i++) nuclear@1: { nuclear@1: total.x += (Elements[i].x - mean.x) * (Elements[i].x - mean.x); nuclear@1: total.y += (Elements[i].y - mean.y) * (Elements[i].y - mean.y); nuclear@1: total.z += (Elements[i].z - mean.z) * (Elements[i].z - mean.z); nuclear@1: } nuclear@1: return total / (float) Size; nuclear@1: } nuclear@1: nuclear@1: // Should be a 3x3 matrix returned, but OVR_math.h doesn't have one nuclear@1: Matrix4f SensorFilter::Covariance() const nuclear@1: { nuclear@1: Vector3f mean = Mean(); nuclear@1: Matrix4f total = Matrix4f(0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0); nuclear@1: for (int i = 0; i < Size; i++) nuclear@1: { nuclear@1: total.M[0][0] += (Elements[i].x - mean.x) * (Elements[i].x - mean.x); nuclear@1: total.M[1][0] += (Elements[i].y - mean.y) * (Elements[i].x - mean.x); nuclear@1: total.M[2][0] += (Elements[i].z - mean.z) * (Elements[i].x - mean.x); nuclear@1: total.M[1][1] += (Elements[i].y - mean.y) * (Elements[i].y - mean.y); nuclear@1: total.M[2][1] += (Elements[i].z - mean.z) * (Elements[i].y - mean.y); nuclear@1: total.M[2][2] += (Elements[i].z - mean.z) * (Elements[i].z - mean.z); nuclear@1: } nuclear@1: total.M[0][1] = total.M[1][0]; nuclear@1: total.M[0][2] = total.M[2][0]; nuclear@1: total.M[1][2] = total.M[2][1]; nuclear@1: for (int i = 0; i < 3; i++) nuclear@1: for (int j = 0; j < 3; j++) nuclear@1: total.M[i][j] *= 1.0f / Size; nuclear@1: return total; nuclear@1: } nuclear@1: nuclear@1: Vector3f SensorFilter::PearsonCoefficient() const nuclear@1: { nuclear@1: Matrix4f cov = Covariance(); nuclear@1: Vector3f pearson = Vector3f(); nuclear@1: pearson.x = cov.M[0][1]/(sqrt(cov.M[0][0])*sqrt(cov.M[1][1])); nuclear@1: pearson.y = cov.M[1][2]/(sqrt(cov.M[1][1])*sqrt(cov.M[2][2])); nuclear@1: pearson.z = cov.M[2][0]/(sqrt(cov.M[2][2])*sqrt(cov.M[0][0])); nuclear@1: nuclear@1: return pearson; nuclear@1: } nuclear@1: nuclear@1: nuclear@1: Vector3f SensorFilter::SavitzkyGolaySmooth8() const nuclear@1: { nuclear@1: OVR_ASSERT(Size >= 8); nuclear@1: return GetPrev(0)*0.41667f + nuclear@1: GetPrev(1)*0.33333f + nuclear@1: GetPrev(2)*0.25f + nuclear@1: GetPrev(3)*0.16667f + nuclear@1: GetPrev(4)*0.08333f - nuclear@1: GetPrev(6)*0.08333f - nuclear@1: GetPrev(7)*0.16667f; nuclear@1: } nuclear@1: nuclear@1: nuclear@1: Vector3f SensorFilter::SavitzkyGolayDerivative4() const nuclear@1: { nuclear@1: OVR_ASSERT(Size >= 4); nuclear@1: return GetPrev(0)*0.3f + nuclear@1: GetPrev(1)*0.1f - nuclear@1: GetPrev(2)*0.1f - nuclear@1: GetPrev(3)*0.3f; nuclear@1: } nuclear@1: nuclear@1: Vector3f SensorFilter::SavitzkyGolayDerivative5() const nuclear@1: { nuclear@1: OVR_ASSERT(Size >= 5); nuclear@1: return GetPrev(0)*0.2f + nuclear@1: GetPrev(1)*0.1f - nuclear@1: GetPrev(3)*0.1f - nuclear@1: GetPrev(4)*0.2f; nuclear@1: } nuclear@1: nuclear@1: Vector3f SensorFilter::SavitzkyGolayDerivative12() const nuclear@1: { nuclear@1: OVR_ASSERT(Size >= 12); nuclear@1: return GetPrev(0)*0.03846f + nuclear@1: GetPrev(1)*0.03147f + nuclear@1: GetPrev(2)*0.02448f + nuclear@1: GetPrev(3)*0.01748f + nuclear@1: GetPrev(4)*0.01049f + nuclear@1: GetPrev(5)*0.0035f - nuclear@1: GetPrev(6)*0.0035f - nuclear@1: GetPrev(7)*0.01049f - nuclear@1: GetPrev(8)*0.01748f - nuclear@1: GetPrev(9)*0.02448f - nuclear@1: GetPrev(10)*0.03147f - nuclear@1: GetPrev(11)*0.03846f; nuclear@1: } nuclear@1: nuclear@1: Vector3f SensorFilter::SavitzkyGolayDerivativeN(int n) const nuclear@1: { nuclear@1: OVR_ASSERT(Size >= n); nuclear@1: int m = (n-1)/2; nuclear@1: Vector3f result = Vector3f(); nuclear@1: for (int k = 1; k <= m; k++) nuclear@1: { nuclear@1: int ind1 = m - k; nuclear@1: int ind2 = n - m + k - 1; nuclear@1: result += (GetPrev(ind1) - GetPrev(ind2)) * (float) k; nuclear@1: } nuclear@1: float coef = 3.0f/(m*(m+1.0f)*(2.0f*m+1.0f)); nuclear@1: result = result*coef; nuclear@1: return result; nuclear@1: } nuclear@1: nuclear@1: nuclear@1: nuclear@1: nuclear@1: } //namespace OVR