median.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #ifndef FILTERS_MEDIAN_HPP_
31 #define FILTERS_MEDIAN_HPP_
32 
33 #include <stdint.h>
34 #include <sstream>
35 #include <cstdio>
36 
37 #include <memory>
38 
39 #include "filters/filter_base.hpp"
40 
42 
43 
44 /*********************************************************************/
45 /*
46  * Algorithm from N. Wirth's book, implementation by N. Devillard.
47  * This code in public domain.
48  */
49 #define ELEM_SWAP(a,b) { elem_type t=(a);(a)=(b);(b)=t; }
50 
51 namespace filters
52 {
53 /*---------------------------------------------------------------------------
54  Function : kth_smallest()
55  In : array of elements, # of elements in the array, rank k
56  Out : one element
57  Job : find the kth smallest element in the array
58  Notice : use the median() macro defined below to get the median.
59  Reference:
60  Author: Wirth, Niklaus
61  Title: Algorithms + data structures = programs
62  Publisher: Englewood Cliffs: Prentice-Hall, 1976
63  Physical description: 366 p.
64  Series: Prentice-Hall Series in Automatic Computation
65  ---------------------------------------------------------------------------*/
66 template <typename elem_type>
67 elem_type kth_smallest(elem_type a[], int n, int k)
68 {
69  int i,j,l,m ;
70  elem_type x ;
71  l=0 ; m=n-1 ;
72  while (l<m) {
73  x=a[k] ;
74  i=l ;
75  j=m ;
76  do {
77  while (a[i]<x) i++ ;
78  while (x<a[j]) j-- ;
79  if (i<=j) {
80  ELEM_SWAP(a[i],a[j]) ;
81  i++ ; j-- ;
82  }
83  } while (i<=j) ;
84  if (j<k) l=i ;
85  if (k<i) m=j ;
86  }
87  return a[k] ;
88 }
89 #define median(a,n) kth_smallest(a,n,(((n)&1)?((n)/2):(((n)/2)-1)))
90 #undef ELEM_SWAP
91 
92 
96 template <typename T>
98 {
99 public:
101  MedianFilter();
102 
105  ~MedianFilter();
106 
107  virtual bool configure();
108 
113  virtual bool update(const T& data_in, T& data_out);
114 
115 protected:
116  std::vector<T> temp_storage_;
117  std::unique_ptr<RealtimeCircularBuffer<T > > data_storage_;
118 
119  T temp; //used for preallocation and copying from non vector source
120 
121 
123 
124 };
125 
126 template <typename T>
128  number_of_observations_(0)
129 {
130 
131 }
132 
133 template <typename T>
135 {
136 }
137 
138 
139 template <typename T>
141 {
142  int no_obs = -1;
143  if (!FilterBase<T>::getParam(std::string("number_of_observations"), no_obs))
144  {
145  fprintf(stderr, "Error: MedianFilter was not given params.\n");
146  return false;
147  }
148  number_of_observations_ = no_obs;
149 
150  data_storage_.reset( new RealtimeCircularBuffer<T >(number_of_observations_, temp));
151  temp_storage_.resize(number_of_observations_);
152 
153  return true;
154 }
155 
156 template <typename T>
157 bool MedianFilter<T>::update(const T& data_in, T& data_out)
158 {
160  return false;
161 
162  data_storage_->push_back(data_in);
163 
164 
165  unsigned int length = data_storage_->size();
166 
167 
168  for (uint32_t row = 0; row < length; row ++)
169  {
170  temp_storage_[row] = (*data_storage_)[row];
171  }
172  data_out = median(&temp_storage_[0], length);
173 
174 
175  return true;
176 }
180 template <typename T>
182 {
183 public:
186 
190 
191  virtual bool configure();
192 
197  virtual bool update(const std::vector<T>& data_in, std::vector<T>& data_out);
198 
199 protected:
200  std::vector<T> temp_storage_;
201  std::unique_ptr<RealtimeCircularBuffer<std::vector<T> > > data_storage_;
202 
203  std::vector<T> temp; //used for preallocation and copying from non vector source
204 
205 
207 
208 };
209 
210 template <typename T>
212  number_of_observations_(0)
213 {
214 
215 }
216 
217 template <typename T>
219 {
220 }
221 
222 
223 template <typename T>
225 {
226  int no_obs = -1;
227  if (!FilterBase<T>::getParam("number_of_observations", no_obs))
228  {
229  fprintf(stderr, "Error: MultiChannelMedianFilter was not given params.\n");
230  return false;
231  }
232  number_of_observations_ = no_obs;
233 
234  temp.resize(this->number_of_channels_);
235  data_storage_.reset( new RealtimeCircularBuffer<std::vector<T> >(number_of_observations_, temp));
236  temp_storage_.resize(number_of_observations_);
237 
238  return true;
239 }
240 
241 template <typename T>
242 bool MultiChannelMedianFilter<T>::update(const std::vector<T>& data_in, std::vector<T>& data_out)
243 {
244  // printf("Expecting width %d, got %d and %d\n", width_, data_in.size(),data_out.size());
245  if (data_in.size() != this->number_of_channels_ || data_out.size() != this->number_of_channels_)
246  return false;
248  return false;
249 
250  data_storage_->push_back(data_in);
251 
252 
253  unsigned int length = data_storage_->size();
254 
255 
256  for (uint32_t i = 0; i < this->number_of_channels_; i++)
257  {
258  for (uint32_t row = 0; row < length; row ++)
259  {
260  temp_storage_[row] = (*data_storage_)[row][i];
261  }
262  data_out[i] = median(&temp_storage_[0], length);
263  }
264 
265  return true;
266 }
267 
268 
269 }
270 #endif// FILTERS_MEDIAN_HPP_
filters::FilterBase
A Base filter class to provide a standard interface for all filters.
Definition: filter_base.hpp:47
filter_base.hpp
filters
Definition: filter_base.hpp:38
filters::RealtimeCircularBuffer
A realtime safe circular (ring) buffer.
Definition: realtime_circular_buffer.hpp:47
filters::MultiChannelMedianFilter::number_of_observations_
uint32_t number_of_observations_
Number of observations over which to filter.
Definition: median.hpp:206
filters::MedianFilter::temp
T temp
Definition: median.hpp:119
filters::MedianFilter
A median filter which works on arrays.
Definition: median.hpp:97
filters::MedianFilter::configure
virtual bool configure()
Pure virtual function for the sub class to configure the filter This function must be implemented in ...
Definition: median.hpp:140
median
#define median(a, n)
Definition: median.hpp:89
filters::MultiChannelMedianFilter::temp_storage_
std::vector< T > temp_storage_
Preallocated storage for the list to sort.
Definition: median.hpp:200
filters::MedianFilter::temp_storage_
std::vector< T > temp_storage_
Preallocated storage for the list to sort.
Definition: median.hpp:116
ELEM_SWAP
#define ELEM_SWAP(a, b)
Definition: median.hpp:49
filters::MultiChannelMedianFilter
A median filter which works on arrays.
Definition: median.hpp:181
filters::MultiChannelMedianFilter::temp
std::vector< T > temp
Definition: median.hpp:203
filters::MedianFilter::MedianFilter
MedianFilter()
Construct the filter with the expected width and height.
Definition: median.hpp:127
filters::MultiChannelFilterBase
Definition: filter_base.hpp:380
filters::MultiChannelMedianFilter::data_storage_
std::unique_ptr< RealtimeCircularBuffer< std::vector< T > > > data_storage_
Storage for data between updates.
Definition: median.hpp:201
filters::MultiChannelMedianFilter::configure
virtual bool configure()
Pure virtual function for the sub class to configure the filter This function must be implemented in ...
Definition: median.hpp:224
filters::MedianFilter::~MedianFilter
~MedianFilter()
Destructor to clean up.
Definition: median.hpp:134
filters::MedianFilter::number_of_observations_
uint32_t number_of_observations_
Number of observations over which to filter.
Definition: median.hpp:122
filters::kth_smallest
elem_type kth_smallest(elem_type a[], int n, int k)
Definition: median.hpp:67
filters::MultiChannelMedianFilter::~MultiChannelMedianFilter
~MultiChannelMedianFilter()
Destructor to clean up.
Definition: median.hpp:218
realtime_circular_buffer.hpp
filters::MultiChannelMedianFilter::update
virtual bool update(const std::vector< T > &data_in, std::vector< T > &data_out)
Update the filter and return the data seperately.
Definition: median.hpp:242
filters::MultiChannelMedianFilter::MultiChannelMedianFilter
MultiChannelMedianFilter()
Construct the filter with the expected width and height.
Definition: median.hpp:211
filters::MedianFilter::data_storage_
std::unique_ptr< RealtimeCircularBuffer< T > > data_storage_
Storage for data between updates.
Definition: median.hpp:117
filters::MedianFilter::update
virtual bool update(const T &data_in, T &data_out)
Update the filter and return the data seperately.
Definition: median.hpp:157


filters
Author(s):
autogenerated on Fri Nov 11 2022 03:09:05