blob: bc2ab82179a02af83d71a4246015b44cda6e05e7 (
plain)
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
|
#ifndef C_3D_POINTS_SMOOTHER_H_
#define C_3D_POINT_SMOOTHER_H_
#include <iostream>
#include <list>
using namespace std;
#include "Vect3D.h"
#include "wiimote.h"
typedef enum
{ // k-1
SimpMovingAvg, //S(t) = (1/k)*sum(X(t-n)) = S(t-1) + (X(t) - X(t-k))/k
// n=0
ExpMovingAvg //S(0) = X(0)
//S(t) = a*X(t) + (1 - a)*S(t-1) = S(t-1) + a*(X(t) - S(t-1))
} SmoothingMethod;
typedef v3 Vect3D_t;
class C_3DPointSmoother
{
public:
C_3DPointSmoother() :
m_Method(SimpMovingAvg),
m_iWindowSize(2),
m_iSampCount(0),
m_dExponent(0.0),
m_St() {}
void SetSimpleMovingAverage(int f_iWindowSize);
void SetExponentialMovingAverage(double f_dExponent);
Vect3D_t Smooth(const Vect3D_t &f_Xt);
private:
SmoothingMethod m_Method;
int m_iWindowSize, //window size for SimpMovingAvg
m_iSampCount; //number of processed samples
list<Vect3D_t> m_SampWindow;
double m_dExponent; //exponent for ExpMovingAvg
Vect3D_t m_St; //last smoothed statistic S(t)
Vect3D_t SmoothSample_SimpMovingAvg(const Vect3D_t &f_Xt);
Vect3D_t SmoothSample_ExpMovingAvg(const Vect3D_t &f_Xt);
};
typedef struct rd
{
int rx, ry;
rd() { rx = ry = 0;}
rd(const rd &d) { rx=d.rx; ry=d.ry; }
inline const rd& operator=(const rd &rhs) { rx=rhs.rx; ry=rhs.ry; return *this;}
inline const rd& operator-=(const rd &rhs) { rx-=rhs.rx; ry-=rhs.ry; return *this;}
inline const rd operator-(const rd &rhs) { return rd(*this)-=rhs;}
} RawDot;
inline ostream& operator<<(ostream& os, const RawDot& r )
{
stringstream ss;
ss << setprecision(4)
<< "(" << r.rx << ", " << r.ry << ")";
return os << ss.str();
}
bool FindSensorBarDots(wiimote_state::ir &f_ir, Vect3D_t f_Dots[2]);
bool FindSensorBarDots(RawDot *f_rd, int f_iNumdots, Vect3D_t f_Dots[2]);
#endif //C_3D_POINT_SMOOTHER_H_
|