mean.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, 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 #ifndef FILTERS_MEAN_H
00031 #define FILTERS_MEAN_H
00032 
00033 #include <stdint.h>
00034 #include <cstring>
00035 #include <stdio.h>
00036 
00037 #include <boost/scoped_ptr.hpp>
00038 
00039 #include "filters/filter_base.h"
00040 #include "ros/assert.h"
00041 
00042 #include "filters/realtime_circular_buffer.h"
00043 
00044 namespace filters
00045 {
00046 
00050 template <typename T>
00051 class MeanFilter: public FilterBase <T>
00052 {
00053 public:
00055   MeanFilter();
00056 
00059   ~MeanFilter();
00060 
00061   virtual bool configure();
00062 
00067   virtual bool update( const T & data_in, T& data_out);
00068   
00069 protected:
00070   boost::scoped_ptr<RealtimeCircularBuffer<T > > data_storage_; 
00071   uint32_t last_updated_row_;                     
00072   T temp_; 
00073   uint32_t number_of_observations_;             
00074   
00075 };
00076 
00077 
00078 template <typename T>
00079 MeanFilter<T>::MeanFilter():
00080   number_of_observations_(0)
00081 {
00082 }
00083 
00084 template <typename T>
00085 bool MeanFilter<T>::configure()
00086 {
00087   
00088   if (!FilterBase<T>::getParam(std::string("number_of_observations"), number_of_observations_))
00089   {
00090     ROS_ERROR("MeanFilter did not find param number_of_observations");
00091     return false;
00092   }
00093   
00094   data_storage_.reset(new RealtimeCircularBuffer<T >(number_of_observations_, temp_));
00095 
00096   return true;
00097 }
00098 
00099 template <typename T>
00100 MeanFilter<T>::~MeanFilter()
00101 {
00102 }
00103 
00104 
00105 template <typename T>
00106 bool MeanFilter<T>::update(const T & data_in, T& data_out)
00107 {
00108   //update active row
00109   if (last_updated_row_ >= number_of_observations_ - 1)
00110     last_updated_row_ = 0;
00111   else
00112     last_updated_row_++;
00113 
00114   data_storage_->push_back(data_in);
00115 
00116 
00117   unsigned int length = data_storage_->size();
00118   
00119   data_out = 0;
00120   for (uint32_t row = 0; row < length; row ++)
00121   {
00122     data_out += data_storage_->at(row);
00123   }
00124   data_out /= length;
00125   
00126 
00127   return true;
00128 };
00129 
00133 template <typename T>
00134 class MultiChannelMeanFilter: public MultiChannelFilterBase <T>
00135 {
00136 public:
00138   MultiChannelMeanFilter();
00139 
00142   ~MultiChannelMeanFilter();
00143 
00144   virtual bool configure();
00145 
00150   virtual bool update( const std::vector<T> & data_in, std::vector<T>& data_out);
00151   
00152 protected:
00153   boost::scoped_ptr<RealtimeCircularBuffer<std::vector<T> > > data_storage_; 
00154   uint32_t last_updated_row_;                     
00155 
00156   std::vector<T> temp;  //used for preallocation and copying from non vector source
00157 
00158   uint32_t number_of_observations_;             
00159   using MultiChannelFilterBase<T>::number_of_channels_;           
00160 
00161   
00162   
00163 };
00164 
00165 
00166 template <typename T>
00167 MultiChannelMeanFilter<T>::MultiChannelMeanFilter():
00168   number_of_observations_(0)
00169 {
00170 }
00171 
00172 template <typename T>
00173 bool MultiChannelMeanFilter<T>::configure()
00174 {
00175   
00176   if (!FilterBase<T>::getParam("number_of_observations", number_of_observations_))
00177   {
00178     ROS_ERROR("MultiChannelMeanFilter did not find param number_of_observations");
00179     return false;
00180   }
00181   
00182   temp.resize(number_of_channels_);
00183   data_storage_.reset(new RealtimeCircularBuffer<std::vector<T> >(number_of_observations_, temp));
00184 
00185   return true;
00186 }
00187 
00188 template <typename T>
00189 MultiChannelMeanFilter<T>::~MultiChannelMeanFilter()
00190 {
00191 }
00192 
00193 
00194 template <typename T>
00195 bool MultiChannelMeanFilter<T>::update(const std::vector<T> & data_in, std::vector<T>& data_out)
00196 {
00197   //  ROS_ASSERT(data_in.size() == width_);
00198   //ROS_ASSERT(data_out.size() == width_);
00199   if (data_in.size() != number_of_channels_ || data_out.size() != number_of_channels_)
00200   {
00201     ROS_ERROR("Configured with wrong size config:%d in:%d out:%d", number_of_channels_, (int)data_in.size(), (int)data_out.size());
00202     return false;
00203   }
00204 
00205   //update active row
00206   if (last_updated_row_ >= number_of_observations_ - 1)
00207     last_updated_row_ = 0;
00208   else
00209     last_updated_row_++;
00210 
00211   data_storage_->push_back(data_in);
00212 
00213 
00214   unsigned int length = data_storage_->size();
00215   
00216   //Return each value
00217   for (uint32_t i = 0; i < number_of_channels_; i++)
00218   {
00219     data_out[i] = 0;
00220     for (uint32_t row = 0; row < length; row ++)
00221     {
00222       data_out[i] += data_storage_->at(row)[i];
00223     }
00224     data_out[i] /= length;
00225   }
00226 
00227   return true;
00228 };
00229 
00230 }
00231 #endif// FILTERS_MEAN_H


filters
Author(s):
autogenerated on Wed Aug 26 2015 11:38:25