median.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_MEDIAN_H
00031 #define FILTERS_MEDIAN_H
00032 
00033 #include <stdint.h>
00034 #include <sstream>
00035 #include <cstdio>
00036 
00037 #include <boost/scoped_ptr.hpp>
00038 
00039 #include "filters/filter_base.h"
00040 
00041 #include "filters/realtime_circular_buffer.h"
00042 
00043 
00044 /*********************************************************************/
00045 /*
00046  * Algorithm from N. Wirth's book, implementation by N. Devillard.
00047  * This code in public domain.
00048  */
00049 #define ELEM_SWAP(a,b) { register elem_type t=(a);(a)=(b);(b)=t; }
00050 
00051 namespace filters
00052 {
00053 /*---------------------------------------------------------------------------
00054   Function : kth_smallest()
00055   In : array of elements, # of elements in the array, rank k
00056   Out : one element
00057   Job : find the kth smallest element in the array
00058   Notice : use the median() macro defined below to get the median.
00059   Reference:
00060   Author: Wirth, Niklaus
00061   Title: Algorithms + data structures = programs
00062   Publisher: Englewood Cliffs: Prentice-Hall, 1976
00063   Physical description: 366 p.
00064   Series: Prentice-Hall Series in Automatic Computation
00065   ---------------------------------------------------------------------------*/
00066 template <typename elem_type>
00067 elem_type kth_smallest(elem_type a[], int n, int k)
00068 {
00069   register int i,j,l,m ;
00070   register elem_type x ;
00071   l=0 ; m=n-1 ;
00072   while (l<m) {
00073     x=a[k] ;
00074     i=l ;
00075     j=m ;
00076     do {
00077       while (a[i]<x) i++ ;
00078       while (x<a[j]) j-- ;
00079       if (i<=j) {
00080         ELEM_SWAP(a[i],a[j]) ;
00081         i++ ; j-- ;
00082       }
00083     } while (i<=j) ;
00084     if (j<k) l=i ;
00085     if (k<i) m=j ;
00086   }
00087   return a[k] ;
00088 }
00089 #define median(a,n) kth_smallest(a,n,(((n)&1)?((n)/2):(((n)/2)-1)))
00090 #undef ELEM_SWAP
00091 
00092 
00096 template <typename T>
00097 class MedianFilter: public filters::FilterBase <T>
00098 {
00099 public:
00101   MedianFilter();
00102 
00105   ~MedianFilter();
00106 
00107   virtual bool configure();
00108 
00113   virtual bool update(const T& data_in, T& data_out);
00114   
00115 protected:
00116   std::vector<T> temp_storage_;                       
00117   boost::scoped_ptr<RealtimeCircularBuffer<T > > data_storage_;                       
00118   
00119   T temp;  //used for preallocation and copying from non vector source
00120 
00121 
00122   uint32_t number_of_observations_;             
00123 
00124 };
00125 
00126 template <typename T>
00127 MedianFilter<T>::MedianFilter():
00128   number_of_observations_(0)
00129 {
00130   
00131 };
00132 
00133 template <typename T>
00134 MedianFilter<T>::~MedianFilter()
00135 {
00136 };
00137 
00138 
00139 template <typename T>
00140 bool MedianFilter<T>::configure()
00141 {
00142   int no_obs = -1;
00143   if (!FilterBase<T>::getParam(std::string("number_of_observations"), no_obs))
00144   {
00145     fprintf(stderr, "Error: MedianFilter was not given params.\n");
00146     return false;
00147   }
00148   number_of_observations_ = no_obs;
00149     
00150   data_storage_.reset( new RealtimeCircularBuffer<T >(number_of_observations_, temp));
00151   temp_storage_.resize(number_of_observations_);
00152   
00153   return true;
00154 };
00155 
00156 template <typename T>
00157 bool MedianFilter<T>::update(const T& data_in, T& data_out)
00158 {
00159   if (!FilterBase<T>::configured_)
00160     return false;
00161 
00162   data_storage_->push_back(data_in);
00163 
00164 
00165   unsigned int length = data_storage_->size();
00166  
00167 
00168   for (uint32_t row = 0; row < length; row ++)
00169   {
00170     temp_storage_[row] = (*data_storage_)[row];
00171   }
00172   data_out = median(&temp_storage_[0], length);
00173   
00174 
00175   return true;
00176 };
00180 template <typename T>
00181 class MultiChannelMedianFilter: public filters::MultiChannelFilterBase <T>
00182 {
00183 public:
00185   MultiChannelMedianFilter();
00186 
00189   ~MultiChannelMedianFilter();
00190 
00191   virtual bool configure();
00192 
00197   virtual bool update(const std::vector<T>& data_in, std::vector<T>& data_out);
00198   
00199 protected:
00200   std::vector<T> temp_storage_;                       
00201   boost::scoped_ptr<RealtimeCircularBuffer<std::vector<T> > > data_storage_;                       
00202   
00203   std::vector<T> temp;  //used for preallocation and copying from non vector source
00204 
00205 
00206   uint32_t number_of_observations_;             
00207 
00208 };
00209 
00210 template <typename T>
00211 MultiChannelMedianFilter<T>::MultiChannelMedianFilter():
00212   number_of_observations_(0)
00213 {
00214   
00215 };
00216 
00217 template <typename T>
00218 MultiChannelMedianFilter<T>::~MultiChannelMedianFilter()
00219 {
00220 };
00221 
00222 
00223 template <typename T>
00224 bool MultiChannelMedianFilter<T>::configure()
00225 {
00226   int no_obs = -1;
00227   if (!FilterBase<T>::getParam("number_of_observations", no_obs))
00228   {
00229     fprintf(stderr, "Error: MultiChannelMedianFilter was not given params.\n");
00230     return false;
00231   }
00232   number_of_observations_ = no_obs;
00233     
00234   temp.resize(this->number_of_channels_);
00235   data_storage_.reset( new RealtimeCircularBuffer<std::vector<T> >(number_of_observations_, temp));
00236   temp_storage_.resize(number_of_observations_);
00237   
00238   return true;
00239 };
00240 
00241 template <typename T>
00242 bool MultiChannelMedianFilter<T>::update(const std::vector<T>& data_in, std::vector<T>& data_out)
00243 {
00244   //  printf("Expecting width %d, got %d and %d\n", width_, data_in.size(),data_out.size());
00245   if (data_in.size() != this->number_of_channels_ || data_out.size() != this->number_of_channels_)
00246     return false;
00247   if (!FilterBase<T>::configured_)
00248     return false;
00249 
00250   data_storage_->push_back(data_in);
00251 
00252 
00253   unsigned int length = data_storage_->size();
00254  
00255 
00256   for (uint32_t i = 0; i < this->number_of_channels_; i++)
00257   {
00258     for (uint32_t row = 0; row < length; row ++)
00259     {
00260       temp_storage_[row] = (*data_storage_)[row][i];
00261     }
00262     data_out[i] = median(&temp_storage_[0], length);
00263   }
00264 
00265   return true;
00266 };
00267 
00268 
00269 }
00270 #endif// FILTERS_MEDIAN_H


filters
Author(s):
autogenerated on Sat Jun 8 2019 19:38:00