Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[10573] pkg/trunk
|
From: <tf...@us...> - 2009-02-04 22:46:36
|
Revision: 10573
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10573&view=rev
Author: tfoote
Date: 2009-02-04 22:46:30 +0000 (Wed, 04 Feb 2009)
Log Message:
-----------
Filters prototypes basically operational. Merging back before I diverge too much.
r12651@raven: tfoote | 2009-01-18 21:17:12 -0800
creating a branch for offline work
r12652@raven: tfoote | 2009-01-18 21:20:02 -0800
i've got a templated factory working and am now working on making instantiation into a chain easy
r12653@raven: tfoote | 2009-01-20 21:35:12 -0800
adding robot_pose_ekf to joystick control
r12852@raven: tfoote | 2009-01-21 12:58:13 -0800
trying to create a chain and configure it
r12853@raven: tfoote | 2009-01-21 14:38:41 -0800
switching to shared pointers
r12948@raven: tfoote | 2009-01-21 22:37:14 -0800
compiling thanks to josh
r12949@raven: tfoote | 2009-01-21 22:50:10 -0800
two filters working together
r12950@raven: tfoote | 2009-01-21 23:27:00 -0800
mean and median now passing test
r12951@raven: tfoote | 2009-01-21 23:45:25 -0800
fixed macro, preceeding # was preventing replacement
r12959@raven: tfoote | 2009-01-22 01:37:38 -0800
fixing ogre_tools build on intrepid
r13029@raven: tfoote | 2009-01-22 16:19:47 -0800
starting conversion to new filters
r13533@raven: tfoote | 2009-02-03 11:08:51 -0800
making mean and median tests seperate. changing elements_per_observation to width as per API
r13534@raven: tfoote | 2009-02-03 11:44:11 -0800
switching to vectors per API review
r13775@raven: tfoote | 2009-02-03 13:26:34 -0800
templating on std::vector<data> instead of data
r13776@raven: tfoote | 2009-02-03 13:37:24 -0800
switching to new templating standard
r13777@raven: tfoote | 2009-02-03 14:20:05 -0800
commenting transferfunction for it's broken at the moment. and I don't want to convert it to parsing a string before I lay down how to do that fully.
r13825@raven: tfoote | 2009-02-03 18:01:42 -0800
factory working with complicated types, still working on chains. it seems to need the typedef.
r13826@raven: tfoote | 2009-02-03 18:13:28 -0800
chain working. the string matching was a proble again. this is going to be fun
r13827@raven: tfoote | 2009-02-04 11:46:33 -0800
changing to typeids
r13920@raven: tfoote | 2009-02-04 12:41:49 -0800
chains working with typeid
r13921@raven: tfoote | 2009-02-04 12:49:40 -0800
converting to gtests
r13922@raven: tfoote | 2009-02-04 13:30:02 -0800
converting to gtests and adding first cut at update method
r13923@raven: tfoote | 2009-02-04 13:43:05 -0800
updating of chain reporting success, hard coded so that chaining can only do vectors at the moment
r13956@raven: tfoote | 2009-02-04 14:23:25 -0800
fixing downstream build while filters are unstable
r13957@raven: tfoote | 2009-02-04 14:24:53 -0800
chains working and tested
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
pkg/trunk/deprecated/world_3d_map/CMakeLists.txt
pkg/trunk/deprecated/world_3d_map/manifest.xml
pkg/trunk/util/filter_demo/CMakeLists.txt
pkg/trunk/util/filters/CMakeLists.txt
pkg/trunk/util/filters/include/filters/filter_base.h
pkg/trunk/util/filters/include/filters/mean.h
pkg/trunk/util/filters/include/filters/median.h
pkg/trunk/util/filters/include/filters/transfer_function.h
pkg/trunk/util/filters/manifest.xml
pkg/trunk/util/filters/test/test_median.cpp
pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h
pkg/trunk/util/laser_scan/src/median_filter.cpp
Added Paths:
-----------
pkg/trunk/util/filters/include/filters/filter_chain.h
pkg/trunk/util/filters/test/test_chain.cpp
pkg/trunk/util/filters/test/test_factory.cpp
pkg/trunk/util/filters/test/test_mean.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-02-04 22:46:30 UTC (rev 10573)
@@ -12,7 +12,7 @@
src/base_controller_pos.cpp
src/base_position_controller.cpp
src/laser_scanner_controller.cpp
- src/laser_scanner_traj_controller.cpp
+#Removed while filters are unstable src/laser_scanner_traj_controller.cpp
src/laser_scanner_velocity_controller.cpp
src/arm_position_controller.cpp
src/arm_velocity_controller.cpp
Modified: pkg/trunk/deprecated/world_3d_map/CMakeLists.txt
===================================================================
--- pkg/trunk/deprecated/world_3d_map/CMakeLists.txt 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/deprecated/world_3d_map/CMakeLists.txt 2009-02-04 22:46:30 UTC (rev 10573)
@@ -2,7 +2,4 @@
set(ROS_BUILD_TYPE Release)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(world_3d_map)
-
-rospack_add_boost_directories()
-
rospack_add_executable(world_3d_map src/world_3d_map.cpp)
Modified: pkg/trunk/deprecated/world_3d_map/manifest.xml
===================================================================
--- pkg/trunk/deprecated/world_3d_map/manifest.xml 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/deprecated/world_3d_map/manifest.xml 2009-02-04 22:46:30 UTC (rev 10573)
@@ -2,8 +2,9 @@
<description>3D Map Construction</description>
<author>Ioan Sucan</author>
<license>BSD</license>
-<review status="deprecated" notes="awaiting #900 for removal"/>
+<review status="unreviewed" notes=""/>
<depend package="roscpp" />
+<depend package="boost" />
<depend package="std_msgs" />
<depend package="tf" />
<depend package="laser_scan" />
Modified: pkg/trunk/util/filter_demo/CMakeLists.txt
===================================================================
--- pkg/trunk/util/filter_demo/CMakeLists.txt 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/util/filter_demo/CMakeLists.txt 2009-02-04 22:46:30 UTC (rev 10573)
@@ -4,9 +4,9 @@
genmsg()
gensrv()
-rospack_add_library(filter_demo
- src/filtering_example.cpp)
+#broken until transfer function filter is fixed
+#rospack_add_library(filter_demo
+# src/filtering_example.cpp)
+#rospack_add_executable(filtering_example
+# src/filtering_example.cpp)
-rospack_add_executable(filtering_example
- src/filtering_example.cpp)
-
Modified: pkg/trunk/util/filters/CMakeLists.txt
===================================================================
--- pkg/trunk/util/filters/CMakeLists.txt 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/util/filters/CMakeLists.txt 2009-02-04 22:46:30 UTC (rev 10573)
@@ -3,10 +3,12 @@
rospack(filters)
rospack_add_gtest(median_test test/test_median.cpp)
-rospack_add_gtest(transfer_function_test test/test_transfer_function.cpp)
-target_link_libraries(median_test)
-target_link_libraries(transfer_function_test)
+# broken, needs to go to the new form factor
+#rospack_add_gtest(transfer_function_test test/test_transfer_function.cpp)
-#rospack_add_gtest(mean_test test/test_mean.cpp)
-#target_link_libraries(mean_test)
+
+rospack_add_gtest(mean_test test/test_mean.cpp)
+
+rospack_add_gtest(test_factory test/test_factory.cpp)
+rospack_add_gtest(test_chain test/test_chain.cpp)
\ No newline at end of file
Modified: pkg/trunk/util/filters/include/filters/filter_base.h
===================================================================
--- pkg/trunk/util/filters/include/filters/filter_base.h 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/util/filters/include/filters/filter_base.h 2009-02-04 22:46:30 UTC (rev 10573)
@@ -31,9 +31,42 @@
#define FILTERS_FILTER_BASE_H_
+#include <typeinfo>
+#include <loki/Factory.h>
+#include "ros/assert.h"
+
+typedef std::vector<float> std_vector_float;
+typedef std::vector<double> std_vector_double;
+
+
namespace filters
{
+std::string getFilterID(const std::string & filter_name, const std::string & typestring)
+{
+ if (typestring == "int")
+ return filter_name + "i";
+
+ else if (typestring == "float")
+ return filter_name + "f";
+
+ else if (typestring == "double")
+ return filter_name + "d";
+
+ else if (typestring == "std_vector_float")
+ return filter_name + "St6vectorIfSaIfEE";
+
+ else if (typestring == "std_vector_double")
+ return filter_name + "St6vectorIfSaIdEE";
+
+ else
+ printf("%s\n", typestring.c_str());
+ ROS_ASSERT(typestring == "NOT A SUPPORTED TYPE");
+ return "";
+
+}
+
+
/** \brief A Base filter class to provide a standard interface for all filters
*
*/
@@ -41,14 +74,37 @@
class FilterBase
{
public:
- virtual ~FilterBase(){;};
+ FilterBase(){};
+ virtual ~FilterBase(){};
+ virtual bool configure(unsigned int number_of_elements, const std::string & arguments)=0;
+
+
/** \brief Update the filter and return the data seperately
*/
- virtual bool update(const T* const data_in, T* data_out)=0;
+ virtual bool update(const T& data_in, T& data_out)=0;
+ std::string getType() {return typeid(T).name();};
};
+
+template <typename T>
+class FilterFactory : public Loki::SingletonHolder < Loki::Factory< filters::FilterBase<T>, std::string >,
+ Loki::CreateUsingNew,
+ Loki::LongevityLifetime::DieAsSmallObjectChild >
+{
+ //empty
+};
+
+
+
+#define ROS_REGISTER_FILTER(c,t) \
+ filters::FilterBase<t> * Filters_New_##c##__##t() {return new c< t >;}; \
+ bool ROS_FILTER_## c ## _ ## t = \
+ filters::FilterFactory<t>::Instance().Register(filters::getFilterID(#c , #t ), Filters_New_##c##__##t);
+///\todo make this use templating to get the data type, the user doesn't ever set the data type at runtime
+///\todo make sure that the error messages for invalid data types are reasonable
+
}
#endif //#ifndef FILTERS_FILTER_BASE_H_
Added: pkg/trunk/util/filters/include/filters/filter_chain.h
===================================================================
--- pkg/trunk/util/filters/include/filters/filter_chain.h (rev 0)
+++ pkg/trunk/util/filters/include/filters/filter_chain.h 2009-02-04 22:46:30 UTC (rev 10573)
@@ -0,0 +1,179 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef FILTERS_FILTER_CHAIN_H_
+#define FILTERS_FILTER_CHAIN_H_
+
+#include "filters/filter_base.h"
+#include <sstream>
+#include <vector>
+
+#include "boost/shared_ptr.hpp"
+
+namespace filters
+{
+
+template <typename T>
+class FilterReference
+{
+public:
+ FilterReference(const std::string& filter_name, const std::string& filter_argsuments):name_(filter_name), arguments_(filter_argsuments)
+ {
+ std::stringstream constructor_string;
+ constructor_string << filter_name << typeid(T).name();
+ filter_ = filters::FilterFactory<T>::Instance().CreateObject(constructor_string.str());
+ printf("Created filter at %p\n in reference %p\n", filter_, this);
+ };
+ ~FilterReference(){ printf("reference destructor -> deleting filter\n"); delete filter_; };
+
+ filters::FilterBase<T> * filter_;
+ std::string name_;
+ std::string arguments_;
+};
+
+
+template <typename T>
+class FilterChain
+{
+public:
+ /** \brief Create the filter chain object */
+ FilterChain(): configured_(false){};
+ /** \brief Configure the filter chain
+ * This will call configure on all filters which have been added
+ * as well as allocate the buffers*/
+ bool configure(unsigned int size)
+ {
+ buffer0_.resize(size);
+ buffer1_.resize(size);
+
+ bool result = true;
+
+ // for each element in vector configure
+ typename std::vector<boost::shared_ptr<filters::FilterReference<T> > >::iterator it;
+
+ for ( it = reference_pointers_.begin();
+ it != reference_pointers_.end(); it++) ///\todo check allignment of for
+ {
+ printf("Configured %s filter at %p\n", (*it).get()->name_.c_str(), (*it).get());
+ result = result && it->get()->filter_->configure(size, it->get()->arguments_);
+ }
+
+ if (result == true)
+ {
+ configured_ = true;
+ }
+ return result;
+ };
+
+ /** \brief Add filters to the list of filters to run on incoming data
+ * This will not configure, you must call configure before they will
+ * be useful. */
+ bool add(const std::string& filter_name, const std::string& filter_arguments)
+ {
+ configured_ = false;
+ boost::shared_ptr<filters::FilterReference<T> > p(new filters::FilterReference<T>(filter_name, filter_arguments));
+ reference_pointers_.push_back(p);
+ printf("%s filter added to vector\n", filter_name.c_str());
+ return true;
+ };
+
+ /** \brief Clear all filters from this chain */
+ bool clear()
+ {
+ configured_ = false;
+ reference_pointers_.clear();
+ buffer0_.clear();
+ buffer1_.clear();
+ return true;
+ };
+
+ /** \brief process data through each of the filters added sequentially */
+ bool update(const T& data_in, T& data_out);
+
+
+ ~FilterChain()
+ {
+ clear();
+
+ };
+
+private:
+ std::vector<boost::shared_ptr<filters::FilterReference<T> > > reference_pointers_;
+
+ T buffer0_; ///<! A temporary intermediate buffer
+ T buffer1_; ///<! A temporary intermediate buffer
+ //std::vector<T> buffer0_; ///<! A temporary intermediate buffer
+ // std::vector<T> buffer1_; ///<! A temporary intermediate buffer
+ bool configured_; ///<! whether the system is configured
+
+};
+
+template <typename T>
+bool FilterChain<T>::update (const T& data_in, T& data_out)
+{
+ unsigned int list_size = reference_pointers_.size();
+ bool result;
+ if (list_size == 0)
+ {
+ memcpy(&data_out, &data_in, sizeof(data_in));
+ result = true;
+ }
+ else if (list_size == 1)
+ result = reference_pointers_[0]->filter_->update(data_in, data_out);
+ else if (list_size == 2)
+ {
+ result = reference_pointers_[0]->filter_->update(data_in, buffer0_);
+ result = result && reference_pointers_[1]->filter_->update(buffer0_, data_out);
+ }
+ else
+ {
+ result = reference_pointers_[0]->filter_->update(data_in, buffer0_); //first copy in
+ for (unsigned int i = 1; i < reference_pointers_.size() - 1; i++) // all but first and last
+ {
+ if (i %2 == 1)
+ result = result && reference_pointers_[i]->filter_->update(buffer0_, buffer1_);
+ else
+ result = result && reference_pointers_[i]->filter_->update(buffer1_, buffer0_);
+
+ }
+ if (list_size % 2 == 1) // odd number last deposit was in buffer0
+ result = result && reference_pointers_.back()->filter_->update(buffer0_, data_out);
+ else
+ result = result && reference_pointers_.back()->filter_->update(buffer1_, data_out);
+ }
+ return result;
+
+};
+
+
+}
+
+
+
+#endif //#ifndef FILTERS_FILTER_CHAIN_H_
Modified: pkg/trunk/util/filters/include/filters/mean.h
===================================================================
--- pkg/trunk/util/filters/include/filters/mean.h 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/util/filters/include/filters/mean.h 2009-02-04 22:46:30 UTC (rev 10573)
@@ -35,6 +35,7 @@
#include <stdio.h>
#include "filters/filter_base.h"
+#include "ros/assert.h"
namespace filters
@@ -48,49 +49,76 @@
{
public:
/** \brief Construct the filter with the expected width and height */
- MeanFilter(uint32_t number_of_observations, uint32_t elements_per_observation);
+ MeanFilter();
/** \brief Destructor to clean up
*/
~MeanFilter();
+ virtual bool configure(unsigned int width, const std::string& number_of_observations);
+
/** \brief Update the filter and return the data seperately
- * \param data_in T array with length elements_per_observation
- * \param data_out T array with length elements_per_observation
+ * \param data_in T array with length width
+ * \param data_out T array with length width
*/
- virtual bool update(T const * const data_in, T* data_out);
+ virtual bool update( const T & data_in, T& data_out);
protected:
- T * data_storage_; ///< Storage for data between updates
+ T data_storage_; ///< Storage for data between updates
uint32_t last_updated_row_; ///< The last row to have been updated by the filter
uint32_t iterations_; ///< Number of iterations up to number of observations
uint32_t number_of_observations_; ///< Number of observations over which to filter
- uint32_t elements_per_observation_; ///< Number of elements per observation
+ uint32_t width_; ///< Number of elements per observation
+ bool configured_;
};
+ROS_REGISTER_FILTER(MeanFilter, std_vector_double)
+ROS_REGISTER_FILTER(MeanFilter, std_vector_float)
+
+
template <typename T>
-MeanFilter<T>::MeanFilter(uint32_t number_of_observations, uint32_t elements_per_observation):
- last_updated_row_(number_of_observations),
+MeanFilter<T>::MeanFilter():
+ last_updated_row_(0),
iterations_(0),
- number_of_observations_(number_of_observations),
- elements_per_observation_(elements_per_observation)
+ number_of_observations_(0),
+ width_(0),
+ configured_(false)
{
- data_storage_ = new T[number_of_observations_ * elements_per_observation];
}
template <typename T>
+bool MeanFilter<T>::configure(unsigned int width, const std::string& number_of_observations)
+{
+ if (configured_)
+ return false;
+ width_ = width;
+ std::stringstream ss;
+ ss << number_of_observations;
+ ss >> number_of_observations_;
+ last_updated_row_ = number_of_observations_;
+
+ data_storage_.resize(number_of_observations_ * width_);
+ configured_ = true;
+ return true;
+}
+
+template <typename T>
MeanFilter<T>::~MeanFilter()
{
- delete [] data_storage_;
}
template <typename T>
-bool MeanFilter<T>::update(T const* const data_in, T* data_out)
+bool MeanFilter<T>::update(const T & data_in, T& data_out)
{
+ // ROS_ASSERT(data_in.size() == width_);
+ //ROS_ASSERT(data_out.size() == width_);
+ if (data_in.size() != width_ || data_out.size() != width_)
+ return false;
+
//update active row
if (last_updated_row_ >= number_of_observations_ - 1)
last_updated_row_ = 0;
@@ -98,9 +126,9 @@
last_updated_row_++;
//copy incoming data into perminant storage
- memcpy(&data_storage_[elements_per_observation_ * last_updated_row_],
- data_in,
- sizeof(T) * elements_per_observation_);
+ memcpy(&data_storage_[width_ * last_updated_row_],
+ &data_in[0],
+ sizeof(data_in[0]) * width_);
//Return values
@@ -117,12 +145,12 @@
}
//Return each value
- for (uint32_t i = 0; i < elements_per_observation_; i++)
+ for (uint32_t i = 0; i < width_; i++)
{
data_out[i] = 0;
for (uint32_t row = 0; row < length; row ++)
{
- data_out[i] += data_storage_[i + row * elements_per_observation_];
+ data_out[i] += data_storage_[i + row * width_];
}
data_out[i] /= length;
}
Modified: pkg/trunk/util/filters/include/filters/median.h
===================================================================
--- pkg/trunk/util/filters/include/filters/median.h 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/util/filters/include/filters/median.h 2009-02-04 22:46:30 UTC (rev 10573)
@@ -31,9 +31,9 @@
#define FILTERS_MEDIAN_H_
#include <stdint.h>
-
+#include <sstream>
#include "filters/filter_base.h"
-
+#include "ros/assert.h"
/*
* Algorithm from N. Wirth's book, implementation by N. Devillard.
* This code in public domain.
@@ -85,55 +85,82 @@
*
*/
template <typename T>
-class MedianFilter: public FilterBase <T>
+class MedianFilter: public filters::FilterBase <T>
{
public:
/** \brief Construct the filter with the expected width and height */
- MedianFilter(uint32_t number_of_observations, uint32_t elements_per_observation);
+ MedianFilter();
/** \brief Destructor to clean up
*/
- ~MedianFilter()
- {
- delete [] data_storage_;
- delete [] temp_storage_;
- }
+ ~MedianFilter();
+ virtual bool configure(unsigned int elements, const std::string& arguments);
/** \brief Update the filter and return the data seperately
- * \param data_in double array with length elements_per_observation
- * \param data_out double array with length elements_per_observation
+ * \param data_in double array with length width
+ * \param data_out double array with length width
*/
- virtual bool update(T const * const data_in, T* data_out);
+ virtual bool update(const T& data_in, T& data_out);
protected:
- T * temp_storage_; ///< Preallocated storage for the list to sort
- T * data_storage_; ///< Storage for data between updates
+ ///\todo change to vector
+ T temp_storage_; ///< Preallocated storage for the list to sort
+ T data_storage_; ///< Storage for data between updates
uint32_t last_updated_row_; ///< The last row to have been updated by the filter
uint32_t iterations_; ///< Number of iterations up to number of observations
uint32_t number_of_observations_; ///< Number of observations over which to filter
- uint32_t elements_per_observation_; ///< Number of elements per observation
+ uint32_t width_; ///< Number of elements per observation
+ bool configured_; ///< Whether the filter has been configured
+
};
template <typename T>
-MedianFilter<T>::MedianFilter(uint32_t number_of_observations, uint32_t elements_per_observation):
- last_updated_row_(number_of_observations),
+MedianFilter<T>::MedianFilter():
+ last_updated_row_(0),
iterations_(0),
- number_of_observations_(number_of_observations),
- elements_per_observation_(elements_per_observation)
+ number_of_observations_(0),
+ width_(0),
+ configured_(false)
{
- data_storage_ = new T[number_of_observations_ * elements_per_observation];
- temp_storage_ = new T[elements_per_observation];
+
+};
+template <typename T>
+MedianFilter<T>::~MedianFilter()
+{
};
template <typename T>
-bool MedianFilter<T>::update(T const* const data_in, T* data_out)
+bool MedianFilter<T>::configure(uint32_t width, const std::string& number_of_observations)
{
+ if (configured_)
+ return false;
+ width_ = width;
+ std::stringstream ss;
+ ss << number_of_observations;
+ ss >> number_of_observations_;
+ data_storage_.resize(number_of_observations_ * width_);
+ temp_storage_.resize(width_);
+ last_updated_row_ = number_of_observations_;
+ configured_ = true;
+ return true;
+};
+
+template <typename T>
+bool MedianFilter<T>::update(const T& data_in, T& data_out)
+{
+ // printf("Expecting width %d, got %d and %d\n", width_, data_in.size(),data_out.size());
+ // ROS_ASSERT(data_in.size() == width_);
+ // ROS_ASSERT(data_out.size() == width_);
+ if (data_in.size() != width_ || data_out.size() != width_)
+ return false;
+ if (!configured_)
+ return false;
//update active row
if (last_updated_row_ >= number_of_observations_ - 1)
last_updated_row_ = 0;
@@ -141,9 +168,9 @@
last_updated_row_++;
//copy incoming data into perminant storage
- memcpy(&data_storage_[elements_per_observation_ * last_updated_row_],
- data_in,
- sizeof(T) * elements_per_observation_);
+ memcpy(&data_storage_[width_ * last_updated_row_],
+ &data_in[0],
+ sizeof(data_in[0]) * width_);
//Return values
@@ -160,18 +187,21 @@
}
//Return each value
- for (uint32_t i = 0; i < elements_per_observation_; i++)
+ for (uint32_t i = 0; i < width_; i++)
{
for (uint32_t row = 0; row < length; row ++)
{
- temp_storage_[row] = data_storage_[i + row * elements_per_observation_];
+ temp_storage_[row] = data_storage_[i + row * width_];
}
- data_out[i] = median(temp_storage_, length);
+ data_out[i] = median(&temp_storage_[0], length);
}
return true;
+};
+
+ROS_REGISTER_FILTER(MedianFilter, std_vector_double)
+ROS_REGISTER_FILTER(MedianFilter, std_vector_float)
}
-}
#endif //#ifndef FILTERS_MEDIAN_H_
Modified: pkg/trunk/util/filters/include/filters/transfer_function.h
===================================================================
--- pkg/trunk/util/filters/include/filters/transfer_function.h 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/util/filters/include/filters/transfer_function.h 2009-02-04 22:46:30 UTC (rev 10573)
@@ -84,8 +84,11 @@
* \param data_in vector<T> with n elements
* \param data_out vector<T> with n elements
*/
- virtual bool update(std::vector<T> const * const data_in, std::vector<T>* data_out) ;
+ virtual bool update(const std::vector<T> & data_in, std::vector<T>& data_out) ;
+
+ //virtual bool configure(unsigned int number_of_elements, const std::string & arguments)=0;
+
protected:
unsigned int number_of_channels_;
@@ -125,29 +128,29 @@
template <typename T>
-bool TransferFunctionFilter<T>::update(std::vector<T> const* const data_in, std::vector<T>* data_out)
+bool TransferFunctionFilter<T>::update(const std::vector<T> & data_in, std::vector<T>& data_out)
{
// Ensure the correct number of inputs
- assert(data_in->size() == number_of_channels_);
+ assert(data_in.size() == number_of_channels_);
// Copy data to prevent mutation if in and out are the same ptr
- std::vector<T> current_input = *data_in;
+ std::vector<T> current_input = data_in;
for (uint32_t i = 0; i < current_input.size(); i++)
{
- (*data_out)[i]=b_[0] * current_input[i];
+ data_out[i]=b_[0] * current_input[i];
for (uint32_t row = 0; row < input_buffer_.size(); row++)
{
- (*data_out)[i] += b_[row+1] * input_buffer_[row][i];
+ (data_out)[i] += b_[row+1] * input_buffer_[row][i];
}
for (uint32_t row = 0; row < output_buffer_.size(); row++)
{
- (*data_out)[i] -= a_[row+1] * output_buffer_[row][i];
+ (data_out)[i] -= a_[row+1] * output_buffer_[row][i];
}
}
input_buffer_.push(current_input);
- output_buffer_.push(*data_out);
+ output_buffer_.push(data_out);
return true;
}
Modified: pkg/trunk/util/filters/manifest.xml
===================================================================
--- pkg/trunk/util/filters/manifest.xml 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/util/filters/manifest.xml 2009-02-04 22:46:30 UTC (rev 10573)
@@ -7,9 +7,11 @@
</description>
<author>Tully Foote/tf...@wi..., Melonee Wise/mw...@wi...</author>
<license>BSD</license>
-<review status="unreviewed" notes=""/>
+<review status="API conditionally cleared" notes=""/>
<url>http://pr.willowgarage.com</url>
<depend package="misc_utils" />
+<depend package="loki" />
+<depend package="rosconsole" />
<export>
<cpp cflags="-I${prefix}/include"/> <!-- lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lfilter"/-->
</export>
Added: pkg/trunk/util/filters/test/test_chain.cpp
===================================================================
--- pkg/trunk/util/filters/test/test_chain.cpp (rev 0)
+++ pkg/trunk/util/filters/test/test_chain.cpp 2009-02-04 22:46:30 UTC (rev 10573)
@@ -0,0 +1,125 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "gtest/gtest.h"
+#include "filters/filter_chain.h"
+#include "filters/median.h"
+#include "filters/mean.h"
+
+template <typename T>
+class TestFilter : public filters::FilterBase<T>
+{
+public:
+ TestFilter() { printf("Constructor\n");};
+
+ ~TestFilter() { printf("Destructor\n");};
+
+ virtual bool configure(unsigned int number_of_elements, const std::string & arguments)
+ {
+ printf("Configured with %d %s\n", number_of_elements, arguments.c_str());
+
+ return true;
+ };
+
+ virtual bool update(const T & data_in, T& data_out)
+ {
+ printf("Update called\n");
+ return true;
+ };
+
+
+
+};
+
+ROS_REGISTER_FILTER(TestFilter, double)
+ROS_REGISTER_FILTER(TestFilter, float)
+
+
+
+TEST(FilterChain, configuring){
+ double epsilon = 1e-9;
+ printf("Chain test starting\n");
+ filters::FilterChain<std_vector_float > chain;
+ //filters::FilterChain<float> chain;
+
+
+ // chain.add("TestFilter", "");
+ chain.add("MeanFilter", "5");
+ chain.add("MedianFilter", "5");
+
+ chain.configure(5);
+
+ float input1[] = {1,2,3,4,5};
+ float input1a[] = {9,9,9,9,9};//seed w/incorrect values
+ std::vector<float> v1 (input1, input1 + sizeof(input1) / sizeof(float));
+ std::vector<float> v1a (input1a, input1a + sizeof(input1a) / sizeof(float));
+
+
+ EXPECT_TRUE(chain.update(v1, v1a));
+
+ chain.clear();
+
+ for (int i = 1; i < v1.size(); i++)
+ {
+ EXPECT_NEAR(input1[i], v1a[i], epsilon);
+ }
+
+}
+
+TEST(FilterChain, Misconfigured){
+ printf("Chain test starting\n");
+ filters::FilterChain<std_vector_float > chain;
+ //filters::FilterChain<float> chain;
+
+
+ // chain.add("TestFilter", "");
+ chain.add("MeanFilter", "5");
+ chain.add("MedianFilter", "5");
+
+ chain.configure(10);
+
+ float input1[] = {1,2,3,4,5};
+ float input1a[] = {1,2,3,4,5};
+ std::vector<float> v1 (input1, input1 + sizeof(input1) / sizeof(float));
+ std::vector<float> v1a (input1a, input1a + sizeof(input1a) / sizeof(float));
+
+
+ EXPECT_FALSE(chain.update(v1, v1a));
+
+ chain.clear();
+
+}
+
+
+
+
+int main(int argc, char **argv){
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
Added: pkg/trunk/util/filters/test/test_factory.cpp
===================================================================
--- pkg/trunk/util/filters/test/test_factory.cpp (rev 0)
+++ pkg/trunk/util/filters/test/test_factory.cpp 2009-02-04 22:46:30 UTC (rev 10573)
@@ -0,0 +1,93 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "gtest/gtest.h"
+#include "filters/filter_base.h"
+
+template <typename T>
+class TestFilter : public filters::FilterBase<T>
+{
+public:
+ TestFilter() { printf("Constructor\n");};
+
+ ~TestFilter() { printf("Destructor\n");};
+
+ virtual bool configure(unsigned int number_of_elements, const std::string & arguments)
+ {
+ printf("Configured with %d %s\n", number_of_elements, arguments.c_str());
+ return true;
+ };
+
+ virtual bool update(const T& data_in, T& data_out)
+ {
+ printf("Update called\n");
+ return true;
+ };
+
+
+
+};
+
+ROS_REGISTER_FILTER(TestFilter, double)
+ROS_REGISTER_FILTER(TestFilter, float)
+ROS_REGISTER_FILTER(TestFilter, int)
+ROS_REGISTER_FILTER(TestFilter, std_vector_float)
+
+
+TEST(FilterChain, configuring)
+{
+ std::string name = filters::getFilterID("TestFilter", "float");
+ filters::FilterBase<float> * a_filter = filters::FilterFactory<float>::Instance().CreateObject(name);
+ name = filters::getFilterID("TestFilter", "std_vector_float");
+ filters::FilterBase<std::vector<float> > * a1_filter = filters::FilterFactory<std::vector<float> >::Instance().CreateObject(name);
+ //filters::FilterBase<int> * a1_filter = filters::FilterFactory<int>::Instance().CreateObject("TestFilter<int>");
+ name = filters::getFilterID("TestFilter", "double");
+ filters::FilterBase<double> * b_filter = filters::FilterFactory<double>::Instance().CreateObject(name);
+
+ printf("a is of type: %s\na1 is of type: %s\n b is of type: %s \n",
+ a_filter->getType().c_str(),
+ a1_filter->getType().c_str(),
+ b_filter->getType().c_str());
+
+ a_filter->configure(4, "");
+
+ delete a_filter;
+ delete a1_filter;
+ delete b_filter;
+
+ EXPECT_TRUE("SUCCESS");
+};
+
+
+
+
+int main(int argc, char **argv){
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
Added: pkg/trunk/util/filters/test/test_mean.cpp
===================================================================
--- pkg/trunk/util/filters/test/test_mean.cpp (rev 0)
+++ pkg/trunk/util/filters/test/test_mean.cpp 2009-02-04 22:46:30 UTC (rev 10573)
@@ -0,0 +1,115 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <gtest/gtest.h>
+#include <sys/time.h>
+
+#include "filters/median.h"
+#include "filters/mean.h"
+
+using namespace filters ;
+
+void seed_rand()
+{
+ //Seed random number generator with current microseond count
+ timeval temp_time_struct;
+ gettimeofday(&temp_time_struct,NULL);
+ srand(temp_time_struct.tv_usec);
+};
+
+void generate_rand_vectors(double scale, uint64_t runs, std::vector<double>& xvalues, std::vector<double>& yvalues, std::vector<double>&zvalues)
+{
+ seed_rand();
+ for ( uint64_t i = 0; i < runs ; i++ )
+ {
+ xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
+ yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
+ zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
+ }
+}
+
+TEST(MeanFilter, ConfirmIdentityNRows)
+{
+ double epsilon = 1e-6;
+ int length = 5;
+ int rows = 5;
+ FilterBase<std::vector<double> > * filter = new MeanFilter<std::vector<double> > ();
+ filter->configure(rows, "5");
+
+ double input1[] = {1,2,3,4,5};
+ double input1a[] = {1,2,3,4,5};
+ std::vector<double> v1 (input1, input1 + sizeof(input1) / sizeof(double));
+ std::vector<double> v1a (input1a, input1a + sizeof(input1a) / sizeof(double));
+
+
+ for (int32_t i =0; i < rows*10; i++)
+ {
+ filter->update(v1, v1a);
+
+ for (int i = 1; i < length; i++)
+ {
+ EXPECT_NEAR(v1[i], v1a[i], epsilon);
+ }
+ }
+}
+
+TEST(MeanFilter, ThreeRows)
+{
+ double epsilon = 1e-6;
+ int length = 5;
+ int rows = 5;
+ FilterBase<std::vector<double> > * filter = new MeanFilter<std::vector<double> > ();
+ filter->configure(rows, "5");
+
+ double input1[] = {0,1,2,3,4};
+ std::vector<double> v1 (input1, input1 + sizeof(input1) / sizeof(double));
+ double input2[] = {1,2,3,4,5};
+ std::vector<double> v2 (input2, input2 + sizeof(input2) / sizeof(double));
+ double input3[] = {2,3,4,5,6};
+ std::vector<double> v3 (input3, input3 + sizeof(input3) / sizeof(double));
+ double input1a[] = {1,2,3,4,5};
+ std::vector<double> v1a (input1a, input1a + sizeof(input1a) / sizeof(double));
+
+
+ filter->update(v1, v1a);
+ filter->update(v2, v1a);
+ filter->update(v3, v1a);
+
+ for (int i = 1; i < length; i++)
+ {
+ EXPECT_NEAR(v2[i], v1a[i], epsilon);
+ }
+
+}
+
+
+int main(int argc, char **argv){
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
Modified: pkg/trunk/util/filters/test/test_median.cpp
===================================================================
--- pkg/trunk/util/filters/test/test_median.cpp 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/util/filters/test/test_median.cpp 2009-02-04 22:46:30 UTC (rev 10573)
@@ -59,19 +59,24 @@
double epsilon = 1e-6;
int length = 5;
int rows = 5;
- MedianFilter<float> filter(rows,length);
+ FilterBase<std::vector<float> > * filter = new MedianFilter<std_vector_float>();
+ EXPECT_TRUE(filter->configure(rows,"5"));
float input1[] = {1,2,3,4,5};
float input1a[] = {1,2,3,4,5};
+ std::vector<float> v1 (input1, input1 + sizeof(input1) / sizeof(float));
+ std::vector<float> v1a (input1a, input1a + sizeof(input1a) / sizeof(float));
for (int i =0; i < rows*10; i++)
{
- filter.update(input1, input1a);
+ filter->update(v1, v1a);
for (int i = 1; i < length; i++)
{
- EXPECT_NEAR(input1[i], input1a[i], epsilon);
+ EXPECT_NEAR(input1[i], v1a[i], epsilon);
}
}
+
+ delete filter;
}
TEST(MedianFilter, ThreeRows)
@@ -79,66 +84,30 @@
double epsilon = 1e-6;
int length = 5;
int rows = 5;
- MedianFilter<float> filter(rows,length);
+ FilterBase<std::vector<float> > * filter = new MedianFilter<std_vector_float>();
+ filter->configure(rows,"5");
float input1[] = {0,1,2,3,4};
+ std::vector<float> v1 (input1, input1 + sizeof(input1) / sizeof(float));
float input2[] = {1,2,3,4,5};
+ std::vector<float> v2 (input2, input2 + sizeof(input2) / sizeof(float));
float input3[] = {2,3,4,5,6};
+ std::vector<float> v3 (input3, input3 + sizeof(input3) / sizeof(float));
float input1a[] = {1,2,3,4,5};
+ std::vector<float> v1a (input1a, input1a + sizeof(input1a) / sizeof(float));
- filter.update(input1, input1a);
- filter.update(input2, input1a);
- filter.update(input3, input1a);
+ filter->update(v1, v1a);
+ filter->update(v2, v1a);
+ filter->update(v3, v1a);
for (int i = 1; i < length; i++)
{
- EXPECT_NEAR(input2[i], input1a[i], epsilon);
+ EXPECT_NEAR(v2[i], v1a[i], epsilon);
}
}
-TEST(MeanFilter, ConfirmIdentityNRows)
-{
- double epsilon = 1e-6;
- int length = 5;
- int rows = 5;
- MeanFilter<double> filter(rows,length);
- double input1[] = {1,2,3,4,5};
- double input1a[] = {1,2,3,4,5};
- for (int32_t i =0; i < rows*10; i++)
- {
- filter.update(input1, input1a);
- for (int i = 1; i < length; i++)
- {
- EXPECT_NEAR(input1[i], input1a[i], epsilon);
- }
- }
-}
-
-TEST(MeanFilter, ThreeRows)
-{
- double epsilon = 1e-6;
- int length = 5;
- int rows = 5;
- MeanFilter<double> filter(rows,length);
- double input1[] = {0,1,2,3,4};
- double input2[] = {1,2,3,4,5};
- double input3[] = {2,3,4,5,6};
- double input1a[] = {1,2,3,4,5};
-
- filter.update(input1, input1a);
- filter.update(input2, input1a);
- filter.update(input3, input1a);
-
- for (int i = 1; i < length; i++)
- {
- EXPECT_NEAR(input2[i], input1a[i], epsilon);
- }
-
-}
-
-
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
Modified: pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h
===================================================================
--- pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h 2009-02-04 22:46:30 UTC (rev 10573)
@@ -65,8 +65,8 @@
boost::mutex data_lock; /// Protection from multi threaded programs
std_msgs::LaserScan temp_scan_; /** \todo cache only shallow info not full scan */
- filters::MedianFilter<float> * range_filter_;
- filters::MedianFilter<float> * intensity_filter_;
+ filters::MedianFilter<std::vector<float> > * range_filter_;
+ filters::MedianFilter<std::vector<float> > * intensity_filter_;
};
Modified: pkg/trunk/util/laser_scan/src/median_filter.cpp
===================================================================
--- pkg/trunk/util/laser_scan/src/median_filter.cpp 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/util/laser_scan/src/median_filter.cpp 2009-02-04 22:46:30 UTC (rev 10573)
@@ -38,8 +38,13 @@
filter_length_(filter_length),
num_ranges_(1)
{
- range_filter_ = new MedianFilter<float>(filter_length_, num_ranges_);
- intensity_filter_ = new MedianFilter<float>(filter_length_, num_ranges_);
+ range_filter_ = new MedianFilter<std::vector<float> >();
+ std::stringstream ss;
+ ss << filter_length_;
+ range_filter_->configure(num_ranges_, ss.str());
+
+ intensity_filter_ = new MedianFilter<std::vector<float> >();//(filter_length_, num_ranges_);
+ intensity_filter_->configure(num_ranges_, ss.str());
};
LaserMedianFilter::~LaserMedianFilter()
@@ -61,14 +66,19 @@
num_ranges_ = scan_in.get_ranges_size();
- range_filter_ = new MedianFilter<float>(filter_length_, num_ranges_);
- intensity_filter_ = new MedianFilter<float>(filter_length_, num_ranges_);
-
+ range_filter_ = new MedianFilter<std::vector<float> >();
+ std::stringstream ss;
+ ss << filter_length_;
+ range_filter_->configure(num_ranges_, ss.str());
+
+ intensity_filter_ = new MedianFilter<std::vector<float> >();//(filter_length_, num_ranges_);
+ intensity_filter_->configure(num_ranges_, ss.str());
+
}
/** \todo check for length of intensities too */
- range_filter_->update(&scan_in.ranges[0], &scan_out.ranges[0]);
- intensity_filter_->update(&scan_in.intensities[0], &scan_out.intensities[0]);
+ range_filter_->update(scan_in.ranges, scan_out.ranges);
+ intensity_filter_->update(scan_in.intensities, scan_out.intensities);
return true;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
