transfer_function.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 // Original version: Melonee Wise <mwise@willowgarage.com>
00031 
00032 #ifndef FILTERS_TRANSFER_FUNCTION_H_
00033 #define FILTERS_TRANSFER_FUNCTION_H_
00034 
00035 #include <stdint.h>
00036 #include <math.h>
00037 #include <vector>
00038 #include <string>
00039 
00040 #include <boost/scoped_ptr.hpp>
00041 
00042 #include "filters/filter_base.h"
00043 #include "filters/realtime_circular_buffer.h"
00044 
00045 namespace filters
00046 {
00047 
00048 /***************************************************/
00072 /***************************************************/
00073 template <typename T>
00074 class SingleChannelTransferFunctionFilter: public filters::FilterBase <T>
00075 {
00076 public:
00080   SingleChannelTransferFunctionFilter() ;
00081 
00084   ~SingleChannelTransferFunctionFilter();
00085 
00090   virtual bool configure();
00091 
00096   virtual bool update(const T & data_in, T& data_out) ;
00097 
00098 
00099 
00100 protected:
00101 
00102   boost::scoped_ptr<RealtimeCircularBuffer<T > > input_buffer_; //The input sample history.
00103   boost::scoped_ptr<RealtimeCircularBuffer<T > > output_buffer_; //The output sample history.
00104 
00105   T  temp_; //used for storage and preallocation
00106 
00107   std::vector<double> a_;   //Transfer functon coefficients (output).
00108   std::vector<double> b_;   //Transfer functon coefficients (input).
00109 
00110 };
00111 
00112 template <typename T>
00113 SingleChannelTransferFunctionFilter<T>::SingleChannelTransferFunctionFilter()
00114 {
00115 };
00116 
00117 template <typename T>
00118 SingleChannelTransferFunctionFilter<T>::~SingleChannelTransferFunctionFilter()
00119 {
00120 };
00121 
00122 template <typename T>
00123 bool SingleChannelTransferFunctionFilter<T>::configure()
00124 {
00125 
00126   // Parse a and b into a std::vector<double>.
00127   if (!FilterBase<T>::getParam("a", a_))
00128   {
00129     ROS_ERROR("TransferFunctionFilter, \"%s\", params has no attribute a.", FilterBase<T>::getName().c_str());
00130     return false;
00131   }
00132 
00133 
00134   if (!FilterBase<T>::getParam("b", b_))
00135   {
00136     ROS_ERROR("TransferFunctionFilter, \"%s\", params has no attribute b.", FilterBase<T>::getName().c_str());
00137     return false;
00138   }
00139 
00140   // Create the input and output buffers of the correct size.
00141   input_buffer_.reset(new RealtimeCircularBuffer<T >(b_.size()-1, temp_));
00142   output_buffer_.reset(new RealtimeCircularBuffer<T >(a_.size()-1, temp_));
00143 
00144   // Prevent divide by zero while normalizing coeffs.
00145   if ( a_[0] == 0)
00146   {
00147     ROS_ERROR("a[0] can not equal 0.");
00148     return false;
00149   }
00150 
00151   // Normalize the coeffs by a[0].
00152   if(a_[0] != 1)
00153   {
00154     for(uint32_t i = 0; i < b_.size(); i++)
00155     {
00156       b_[i] = (b_[i] / a_[0]);
00157     }
00158     for(uint32_t i = 1; i < a_.size(); i++)
00159     {
00160       a_[i] = (a_[i] / a_[0]);
00161     }
00162     a_[0] = (a_[0] / a_[0]);
00163   }
00164 
00165   return true;
00166 };
00167 
00168 
00169 template <typename T>
00170 bool SingleChannelTransferFunctionFilter<T>::update(const T  & data_in, T & data_out)
00171 {
00172   if (!FilterBase<T>::configured_)
00173     return false;
00174 
00175   // Copy data to prevent mutation if in and out are the same ptr
00176   temp_ = data_in;
00177 
00178   data_out=b_[0] * temp_;
00179 
00180   for (uint32_t row = 1; row <= input_buffer_->size(); row++)
00181   {
00182     data_out += b_[row] * (*input_buffer_)[row-1];
00183   }
00184   for (uint32_t row = 1; row <= output_buffer_->size(); row++)
00185   {
00186     data_out -= a_[row] * (*output_buffer_)[row-1];
00187   }
00188 
00189   input_buffer_->push_front(temp_);
00190   output_buffer_->push_front(data_out);
00191 
00192   return true;
00193 };
00194 
00195 
00196 
00197 /***************************************************/
00222 /***************************************************/
00223 
00224 template <typename T>
00225 class MultiChannelTransferFunctionFilter: public filters::MultiChannelFilterBase <T>
00226 {
00227 public:
00231   MultiChannelTransferFunctionFilter() ;
00232 
00235   ~MultiChannelTransferFunctionFilter();
00236 
00241   virtual bool configure();
00242 
00247   virtual bool update(const std::vector<T> & data_in, std::vector<T>& data_out) ;
00248 
00249 
00250 
00251 protected:
00252 
00253   boost::scoped_ptr<RealtimeCircularBuffer<std::vector<T> > > input_buffer_; //The input sample history.
00254   boost::scoped_ptr<RealtimeCircularBuffer<std::vector<T> > > output_buffer_; //The output sample history.
00255 
00256   std::vector<T>  temp_; //used for storage and preallocation
00257 
00258   std::vector<double> a_;   //Transfer functon coefficients (output).
00259   std::vector<double> b_;   //Transfer functon coefficients (input).
00260 
00261 };
00262 
00263 template <typename T>
00264 MultiChannelTransferFunctionFilter<T>::MultiChannelTransferFunctionFilter()
00265 {
00266 };
00267 
00268 template <typename T>
00269 MultiChannelTransferFunctionFilter<T>::~MultiChannelTransferFunctionFilter()
00270 {
00271 };
00272 
00273 template <typename T>
00274 bool MultiChannelTransferFunctionFilter<T>::configure()
00275 {
00276   // Parse a and b into a std::vector<double>.
00277   if (!FilterBase<T>::getParam("a", a_))
00278   {
00279     ROS_ERROR("TransferFunctionFilter, \"%s\", params has no attribute a.", FilterBase<T>::getName().c_str());
00280     return false;
00281   }
00282 
00283 
00284   if (!FilterBase<T>::getParam("b", b_))
00285   {
00286     ROS_ERROR("TransferFunctionFilter, \"%s\", params has no attribute b.", FilterBase<T>::getName().c_str());
00287     return false;
00288   }
00289 
00290   // Create the input and output buffers of the correct size.
00291   temp_.resize(this->number_of_channels_);
00292   input_buffer_.reset(new RealtimeCircularBuffer<std::vector<T> >(b_.size()-1, temp_));
00293   output_buffer_.reset(new RealtimeCircularBuffer<std::vector<T> >(a_.size()-1, temp_));
00294 
00295   // Prevent divide by zero while normalizing coeffs.
00296   if ( a_[0] == 0)
00297   {
00298     ROS_ERROR("a[0] can not equal 0.");
00299     return false;
00300   }
00301 
00302   // Normalize the coeffs by a[0].
00303   if(a_[0] != 1)
00304   {
00305     for(uint32_t i = 0; i < b_.size(); i++)
00306     {
00307       b_[i] = (b_[i] / a_[0]);
00308     }
00309     for(uint32_t i = 1; i < a_.size(); i++)
00310     {
00311       a_[i] = (a_[i] / a_[0]);
00312     }
00313     a_[0] = (a_[0] / a_[0]);
00314   }
00315 
00316   return true;
00317 };
00318 
00319 
00320 template <typename T>
00321 bool MultiChannelTransferFunctionFilter<T>::update(const std::vector<T>  & data_in, std::vector<T> & data_out)
00322 {
00323 
00324   // Ensure the correct number of inputs
00325   if (data_in.size() != this->number_of_channels_ || data_out.size() != this->number_of_channels_ )
00326   {
00327     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());
00328     return false;
00329   }
00330   // Copy data to prevent mutation if in and out are the same ptr
00331   temp_ = data_in;
00332 
00333   for (uint32_t i = 0; i < temp_.size(); i++)
00334   {
00335     data_out[i]=b_[0] * temp_[i];
00336 
00337     for (uint32_t row = 1; row <= input_buffer_->size(); row++)
00338     {
00339       (data_out)[i] += b_[row] * (*input_buffer_)[row-1][i];
00340     }
00341     for (uint32_t row = 1; row <= output_buffer_->size(); row++)
00342     {
00343       (data_out)[i] -= a_[row] * (*output_buffer_)[row-1][i];
00344     }
00345   }
00346   input_buffer_->push_front(temp_);
00347   output_buffer_->push_front(data_out);
00348   return true;
00349 };
00350 
00351 }
00352 
00353 #endif //#ifndef FILTERS_TRAjNSFER_FUNCTION_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Defines


filters
Author(s): Tully Foote/tfoote@willowgarage.com
autogenerated on Mon Aug 19 2013 11:35:05