transfer_function.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, 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 // Original version: Melonee Wise <mwise@willowgarage.com>
31 
32 #ifndef FILTERS_TRANSFER_FUNCTION_H_
33 #define FILTERS_TRANSFER_FUNCTION_H_
34 
35 #include <stdint.h>
36 #include <math.h>
37 #include <vector>
38 #include <string>
39 
40 #include <boost/scoped_ptr.hpp>
41 
42 #include "filters/filter_base.h"
44 
45 namespace filters
46 {
47 
48 /***************************************************/
72 /***************************************************/
73 template <typename T>
75 {
76 public:
81 
85 
90  virtual bool configure();
91 
96  virtual bool update(const T & data_in, T& data_out) ;
97 
98 
99 
100 protected:
101 
102  boost::scoped_ptr<RealtimeCircularBuffer<T > > input_buffer_; //The input sample history.
103  boost::scoped_ptr<RealtimeCircularBuffer<T > > output_buffer_; //The output sample history.
104 
105  T temp_; //used for storage and preallocation
106 
107  std::vector<double> a_; //Transfer functon coefficients (output).
108  std::vector<double> b_; //Transfer functon coefficients (input).
109 
110 };
111 
112 template <typename T>
114 {
115 };
116 
117 template <typename T>
119 {
120 };
121 
122 template <typename T>
124 {
125 
126  // Parse a and b into a std::vector<double>.
127  if (!FilterBase<T>::getParam("a", a_))
128  {
129  ROS_ERROR("TransferFunctionFilter, \"%s\", params has no attribute a.", FilterBase<T>::getName().c_str());
130  return false;
131  }
132 
133 
134  if (!FilterBase<T>::getParam("b", b_))
135  {
136  ROS_ERROR("TransferFunctionFilter, \"%s\", params has no attribute b.", FilterBase<T>::getName().c_str());
137  return false;
138  }
139 
140  // Create the input and output buffers of the correct size.
141  input_buffer_.reset(new RealtimeCircularBuffer<T >(b_.size()-1, temp_));
142  output_buffer_.reset(new RealtimeCircularBuffer<T >(a_.size()-1, temp_));
143 
144  // Prevent divide by zero while normalizing coeffs.
145  if ( a_[0] == 0)
146  {
147  ROS_ERROR("a[0] can not equal 0.");
148  return false;
149  }
150 
151  // Normalize the coeffs by a[0].
152  if(a_[0] != 1)
153  {
154  for(uint32_t i = 0; i < b_.size(); i++)
155  {
156  b_[i] = (b_[i] / a_[0]);
157  }
158  for(uint32_t i = 1; i < a_.size(); i++)
159  {
160  a_[i] = (a_[i] / a_[0]);
161  }
162  a_[0] = (a_[0] / a_[0]);
163  }
164 
165  return true;
166 };
167 
168 
169 template <typename T>
170 bool SingleChannelTransferFunctionFilter<T>::update(const T & data_in, T & data_out)
171 {
173  return false;
174 
175  // Copy data to prevent mutation if in and out are the same ptr
176  temp_ = data_in;
177 
178  data_out=b_[0] * temp_;
179 
180  for (uint32_t row = 1; row <= input_buffer_->size(); row++)
181  {
182  data_out += b_[row] * (*input_buffer_)[row-1];
183  }
184  for (uint32_t row = 1; row <= output_buffer_->size(); row++)
185  {
186  data_out -= a_[row] * (*output_buffer_)[row-1];
187  }
188 
189  input_buffer_->push_front(temp_);
190  output_buffer_->push_front(data_out);
191 
192  return true;
193 };
194 
195 
196 
197 /***************************************************/
222 /***************************************************/
223 
224 template <typename T>
226 {
227 public:
232 
236 
241  virtual bool configure();
242 
247  virtual bool update(const std::vector<T> & data_in, std::vector<T>& data_out) ;
248 
249 
250 
251 protected:
252 
253  boost::scoped_ptr<RealtimeCircularBuffer<std::vector<T> > > input_buffer_; //The input sample history.
254  boost::scoped_ptr<RealtimeCircularBuffer<std::vector<T> > > output_buffer_; //The output sample history.
255 
256  std::vector<T> temp_; //used for storage and preallocation
257 
258  std::vector<double> a_; //Transfer functon coefficients (output).
259  std::vector<double> b_; //Transfer functon coefficients (input).
260 
261 };
262 
263 template <typename T>
265 {
266 };
267 
268 template <typename T>
270 {
271 };
272 
273 template <typename T>
275 {
276  // Parse a and b into a std::vector<double>.
277  if (!FilterBase<T>::getParam("a", a_))
278  {
279  ROS_ERROR("TransferFunctionFilter, \"%s\", params has no attribute a.", FilterBase<T>::getName().c_str());
280  return false;
281  }
282 
283 
284  if (!FilterBase<T>::getParam("b", b_))
285  {
286  ROS_ERROR("TransferFunctionFilter, \"%s\", params has no attribute b.", FilterBase<T>::getName().c_str());
287  return false;
288  }
289 
290  // Create the input and output buffers of the correct size.
291  temp_.resize(this->number_of_channels_);
292  input_buffer_.reset(new RealtimeCircularBuffer<std::vector<T> >(b_.size()-1, temp_));
293  output_buffer_.reset(new RealtimeCircularBuffer<std::vector<T> >(a_.size()-1, temp_));
294 
295  // Prevent divide by zero while normalizing coeffs.
296  if ( a_[0] == 0)
297  {
298  ROS_ERROR("a[0] can not equal 0.");
299  return false;
300  }
301 
302  // Normalize the coeffs by a[0].
303  if(a_[0] != 1)
304  {
305  for(uint32_t i = 0; i < b_.size(); i++)
306  {
307  b_[i] = (b_[i] / a_[0]);
308  }
309  for(uint32_t i = 1; i < a_.size(); i++)
310  {
311  a_[i] = (a_[i] / a_[0]);
312  }
313  a_[0] = (a_[0] / a_[0]);
314  }
315 
316  return true;
317 };
318 
319 
320 template <typename T>
321 bool MultiChannelTransferFunctionFilter<T>::update(const std::vector<T> & data_in, std::vector<T> & data_out)
322 {
323 
324  // Ensure the correct number of inputs
325  if (data_in.size() != this->number_of_channels_ || data_out.size() != this->number_of_channels_ )
326  {
327  ROS_ERROR("Number of channels is %d, but data_in.size() = %d and data_out.size() = %d. They must match", this->number_of_channels_, (int)data_in.size(), (int)data_out.size());
328  return false;
329  }
330  // Copy data to prevent mutation if in and out are the same ptr
331  temp_ = data_in;
332 
333  for (uint32_t i = 0; i < temp_.size(); i++)
334  {
335  data_out[i]=b_[0] * temp_[i];
336 
337  for (uint32_t row = 1; row <= input_buffer_->size(); row++)
338  {
339  (data_out)[i] += b_[row] * (*input_buffer_)[row-1][i];
340  }
341  for (uint32_t row = 1; row <= output_buffer_->size(); row++)
342  {
343  (data_out)[i] -= a_[row] * (*output_buffer_)[row-1][i];
344  }
345  }
346  input_buffer_->push_front(temp_);
347  output_buffer_->push_front(data_out);
348  return true;
349 };
350 
351 }
352 
353 #endif //#ifndef FILTERS_TRAjNSFER_FUNCTION_H_
boost::scoped_ptr< RealtimeCircularBuffer< T > > input_buffer_
~MultiChannelTransferFunctionFilter()
Destructor to clean up.
A Base filter class to provide a standard interface for all filters.
Definition: filter_base.h:50
A realtime safe circular (ring) buffer.
virtual bool configure()
Configure the filter with the correct number of channels and params.
virtual bool update(const T &data_in, T &data_out)
Update the filter and return the data seperately.
One-dimensional digital filter class.
boost::scoped_ptr< RealtimeCircularBuffer< std::vector< T > > > input_buffer_
~SingleChannelTransferFunctionFilter()
Destructor to clean up.
MultiChannelTransferFunctionFilter()
Construct the filter.
One-dimensional digital filter class.
virtual bool update(const std::vector< T > &data_in, std::vector< T > &data_out)
Update the filter and return the data seperately.
SingleChannelTransferFunctionFilter()
Construct the filter.
#define ROS_ERROR(...)
boost::scoped_ptr< RealtimeCircularBuffer< std::vector< T > > > output_buffer_
virtual bool configure()
Configure the filter with the correct number of channels and params.
boost::scoped_ptr< RealtimeCircularBuffer< T > > output_buffer_


filters
Author(s):
autogenerated on Sat Jun 8 2019 04:37:52