mean.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_MEAN_HPP_
31 #define FILTERS_MEAN_HPP_
32 
33 #include <stdint.h>
34 #include <cstring>
35 #include <stdio.h>
36 
37 #include <memory>
38 
39 #include "filters/filter_base.hpp"
40 #include "ros/assert.h"
41 
43 
44 namespace filters
45 {
46 
50 template <typename T>
51 class MeanFilter: public FilterBase <T>
52 {
53 public:
55  MeanFilter();
56 
59  ~MeanFilter();
60 
61  virtual bool configure();
62 
67  virtual bool update( const T & data_in, T& data_out);
68 
69 protected:
70  std::unique_ptr<RealtimeCircularBuffer<T > > data_storage_;
71  uint32_t last_updated_row_;
72  T temp_;
74 
75 };
76 
77 
78 template <typename T>
80  number_of_observations_(0)
81 {
82 }
83 
84 template <typename T>
86 {
87 
88  if (!FilterBase<T>::getParam(std::string("number_of_observations"), number_of_observations_))
89  {
90  ROS_ERROR("MeanFilter did not find param number_of_observations");
91  return false;
92  }
93 
94  data_storage_.reset(new RealtimeCircularBuffer<T >(number_of_observations_, temp_));
95 
96  return true;
97 }
98 
99 template <typename T>
101 {
102 }
103 
104 
105 template <typename T>
106 bool MeanFilter<T>::update(const T & data_in, T& data_out)
107 {
108  //update active row
109  if (last_updated_row_ >= number_of_observations_ - 1)
110  last_updated_row_ = 0;
111  else
112  last_updated_row_++;
113 
114  data_storage_->push_back(data_in);
115 
116 
117  unsigned int length = data_storage_->size();
118 
119  data_out = 0;
120  for (uint32_t row = 0; row < length; row ++)
121  {
122  data_out += data_storage_->at(row);
123  }
124  data_out /= length;
125 
126 
127  return true;
128 }
129 
133 template <typename T>
135 {
136 public:
139 
143 
144  virtual bool configure();
145 
150  virtual bool update( const std::vector<T> & data_in, std::vector<T>& data_out);
151 
152 protected:
153  std::unique_ptr<RealtimeCircularBuffer<std::vector<T> > > data_storage_;
154  uint32_t last_updated_row_;
155 
156  std::vector<T> temp; //used for preallocation and copying from non vector source
157 
160 
161 
162 
163 };
164 
165 
166 template <typename T>
168  number_of_observations_(0)
169 {
170 }
171 
172 template <typename T>
174 {
175 
176  if (!FilterBase<T>::getParam("number_of_observations", number_of_observations_))
177  {
178  ROS_ERROR("MultiChannelMeanFilter did not find param number_of_observations");
179  return false;
180  }
181 
182  temp.resize(number_of_channels_);
183  data_storage_.reset(new RealtimeCircularBuffer<std::vector<T> >(number_of_observations_, temp));
184 
185  return true;
186 }
187 
188 template <typename T>
190 {
191 }
192 
193 
194 template <typename T>
195 bool MultiChannelMeanFilter<T>::update(const std::vector<T> & data_in, std::vector<T>& data_out)
196 {
197  if (data_in.size() != number_of_channels_ || data_out.size() != number_of_channels_)
198  {
199  ROS_ERROR("Configured with wrong size config:%d in:%d out:%d", number_of_channels_, (int)data_in.size(), (int)data_out.size());
200  return false;
201  }
202 
203  //update active row
204  if (last_updated_row_ >= number_of_observations_ - 1)
205  last_updated_row_ = 0;
206  else
207  last_updated_row_++;
208 
209  data_storage_->push_back(data_in);
210 
211 
212  unsigned int length = data_storage_->size();
213 
214  //Return each value
215  for (uint32_t i = 0; i < number_of_channels_; i++)
216  {
217  data_out[i] = 0;
218  for (uint32_t row = 0; row < length; row ++)
219  {
220  data_out[i] += data_storage_->at(row)[i];
221  }
222  data_out[i] /= length;
223  }
224 
225  return true;
226 }
227 
228 }
229 #endif// FILTERS_MEAN_HPP_
filters::FilterBase
A Base filter class to provide a standard interface for all filters.
Definition: filter_base.hpp:47
filters::MultiChannelMeanFilter::update
virtual bool update(const std::vector< T > &data_in, std::vector< T > &data_out)
Update the filter and return the data seperately.
Definition: mean.hpp:195
filter_base.hpp
filters
Definition: filter_base.hpp:38
filters::RealtimeCircularBuffer
A realtime safe circular (ring) buffer.
Definition: realtime_circular_buffer.hpp:47
filters::MeanFilter::data_storage_
std::unique_ptr< RealtimeCircularBuffer< T > > data_storage_
Storage for data between updates.
Definition: mean.hpp:70
filters::MultiChannelMeanFilter::temp
std::vector< T > temp
Definition: mean.hpp:156
filters::MeanFilter::update
virtual bool update(const T &data_in, T &data_out)
Update the filter and return the data seperately.
Definition: mean.hpp:106
filters::MultiChannelMeanFilter::MultiChannelMeanFilter
MultiChannelMeanFilter()
Construct the filter with the expected width and height.
Definition: mean.hpp:167
filters::MeanFilter::number_of_observations_
uint32_t number_of_observations_
Temporary storage.
Definition: mean.hpp:73
filters::MeanFilter::configure
virtual bool configure()
Pure virtual function for the sub class to configure the filter This function must be implemented in ...
Definition: mean.hpp:85
filters::MultiChannelMeanFilter::data_storage_
std::unique_ptr< RealtimeCircularBuffer< std::vector< T > > > data_storage_
Storage for data between updates.
Definition: mean.hpp:153
filters::MultiChannelFilterBase
Definition: filter_base.hpp:380
filters::MultiChannelMeanFilter
A mean filter which works on double arrays.
Definition: mean.hpp:134
filters::MultiChannelMeanFilter::number_of_observations_
uint32_t number_of_observations_
Number of observations over which to filter.
Definition: mean.hpp:158
ROS_ERROR
#define ROS_ERROR(...)
filters::MeanFilter::last_updated_row_
uint32_t last_updated_row_
The last row to have been updated by the filter.
Definition: mean.hpp:71
filters::MeanFilter::~MeanFilter
~MeanFilter()
Destructor to clean up.
Definition: mean.hpp:100
filters::MultiChannelMeanFilter::~MultiChannelMeanFilter
~MultiChannelMeanFilter()
Destructor to clean up.
Definition: mean.hpp:189
filters::MeanFilter::temp_
T temp_
Definition: mean.hpp:72
realtime_circular_buffer.hpp
filters::MeanFilter
A mean filter which works on doubles.
Definition: mean.hpp:51
assert.h
filters::MultiChannelMeanFilter::configure
virtual bool configure()
Pure virtual function for the sub class to configure the filter This function must be implemented in ...
Definition: mean.hpp:173
filters::MultiChannelMeanFilter::last_updated_row_
uint32_t last_updated_row_
The last row to have been updated by the filter.
Definition: mean.hpp:154
filters::MeanFilter::MeanFilter
MeanFilter()
Construct the filter with the expected width and height.
Definition: mean.hpp:79


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