/
MedianFilter.h
107 lines (94 loc) · 2.09 KB
/
MedianFilter.h
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
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
#ifndef MedianFilter_h
#define MedianFilter_h
template <typename T, int S>
class MedianFilter {
public:
/* Constructor
*/
MedianFilter()
: m_idx(0), m_cnt(0), m_med(0)
{
}
/* addSample(s): adds the sample S to the window and computes the median
* if enough samples have been gathered
*/
void addSample(T s)
{
m_buf[m_idx] = s;
m_idx = (m_idx + 1) % S;
if(m_cnt == S){
p_calcMedian();
} else {
m_cnt++;
addSample(s);
}
}
/* getMedian(): returns the median computed when the last sample was
* added. Does not return anything meaningful if not enough samples
* have been gathered; check isReady() first.
*/
T getMedian()
{
return m_med;
}
private:
int m_idx, m_cnt;
T m_buf[S], m_tmp[S], m_med;
/* p_calcMedian(): helper to calculate the median. Copies
* the buffer into the temp area, then calls Hoare's in-place
* selection algorithm to obtain the median.
*/
void p_calcMedian()
{
for(int i = 0; i < S; i++){
m_tmp[i] = m_buf[i];
}
m_med = p_select(0, S - 1, S / 2);
}
/* p_partition(l, r, p): partition function, like from quicksort.
* l and r are the left and right bounds of the array (m_tmp),
* respectively, and p is the pivot index.
*/
int p_partition(int l, int r, int p)
{
T tmp;
T pv = m_tmp[p];
m_tmp[p] = m_tmp[r];
m_tmp[r] = pv;
int s = l;
for(int i = l; i < r; i++){
if(m_tmp[i] < pv){
tmp = m_tmp[s];
m_tmp[s] = m_tmp[i];
m_tmp[i] = tmp;
s++;
}
}
tmp = m_tmp[s];
m_tmp[s] = m_tmp[r];
m_tmp[r] = tmp;
return s;
}
/* p_select(l, r, k): Hoare's quickselect. l and r are the
* array bounds, and k conveys that we want to return
* the k-th value
*/
T p_select(int l, int r, int k)
{
if(l == r){
return m_tmp[l];
}
int p = (l + r) / 2;
p = p_partition(l, r, p);
if(p == k){
return m_tmp[k];
}
else if(k < p){
return p_select(l, p - 1, k);
}
else {
return p_select(p + 1, r, k);
}
}
};
#endif