$search
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_