test_median.cpp
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 #include <gtest/gtest.h>
00031 #include <sys/time.h>
00032 
00033 #include "filters/median.h"
00034 
00035 using namespace filters ;
00036 
00037 void seed_rand()
00038 {
00039   //Seed random number generator with current microseond count
00040   timeval temp_time_struct;
00041   gettimeofday(&temp_time_struct,NULL);
00042   srand(temp_time_struct.tv_usec);
00043 };
00044 
00045 void generate_rand_vectors(double scale, uint64_t runs, std::vector<double>& xvalues, std::vector<double>& yvalues, std::vector<double>&zvalues)
00046 {
00047   seed_rand();
00048   for ( uint64_t i = 0; i < runs ; i++ )
00049   {
00050     xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00051     yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00052     zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
00053   }
00054 }
00055 
00056 TEST(MultiChannelMedianFilterDouble, ConfirmIdentityNRows)
00057 {
00058   double epsilon = 1e-6;
00059   int length = 5;
00060   int rows = 5;
00061   
00062   MultiChannelFilterBase<double > * filter = new filters::MultiChannelMedianFilter<double>();
00063   EXPECT_TRUE(filter->configure(rows, "MultiChannelMedianFilterDouble5"));
00064   
00065 
00066   double input1[] = {1,2,3,4,5};
00067   double input1a[] = {11,12,13,14,15};
00068   std::vector<double> v1 (input1, input1 + sizeof(input1) / sizeof(double));
00069   std::vector<double> v1a (input1a, input1a + sizeof(input1a) / sizeof(double));
00070 
00071   for (int i =0; i < rows*10; i++)
00072   {
00073     EXPECT_TRUE(filter->update(v1, v1a));
00074 
00075     for (int j = 1; j < length; j++)
00076     {
00077        EXPECT_NEAR(input1[j], v1a[j], epsilon);
00078     }
00079   }
00080 
00081   delete filter;
00082 }
00083 
00084 TEST(MultiChannelMedianFilterDouble, ThreeRows)
00085 {
00086   double epsilon = 1e-6;
00087   int length = 5;
00088   int rows = 5;
00089 
00090   MultiChannelFilterBase<double > * filter = new MultiChannelMedianFilter<double>();
00091   EXPECT_TRUE(filter->configure(rows, "MultiChannelMedianFilterDouble5" ));
00092   
00093   double input1[] = {0,1,2,3,4};
00094   std::vector<double> v1 (input1, input1 + sizeof(input1) / sizeof(double));
00095   double input2[] = {1,2,3,4,5};
00096   std::vector<double> v2 (input2, input2 + sizeof(input2) / sizeof(double));
00097   double input3[] = {2,3,4,5,6};
00098   std::vector<double> v3 (input3, input3 + sizeof(input3) / sizeof(double));
00099   double input1a[] = {1,2,3,4,5};
00100   std::vector<double> v1a (input1a, input1a + sizeof(input1a) / sizeof(double));
00101 
00102   EXPECT_TRUE(filter->update(v1, v1a));
00103   EXPECT_TRUE(filter->update(v2, v1a));
00104   EXPECT_TRUE(filter->update(v3, v1a));
00105 
00106   for (int i = 1; i < length; i++)
00107   {
00108     EXPECT_NEAR(v2[i], v1a[i], epsilon);
00109   }
00110 
00111 }
00112 
00113 TEST(MultiChannelMedianFilterFloat, ConfirmIdentityNRows)
00114 {
00115   float epsilon = 1e-6;
00116   int length = 5;
00117   int rows = 5;
00118   
00119   MultiChannelFilterBase<float > * filter = new filters::MultiChannelMedianFilter<float>();
00120   EXPECT_TRUE(filter->configure(rows, "MultiChannelMedianFilterFloat5" ));
00121 
00122   float input1[] = {1,2,3,4,5};
00123   float input1a[] = {1,2,3,4,5};
00124   std::vector<float> v1 (input1, input1 + sizeof(input1) / sizeof(float));
00125   std::vector<float> v1a (input1a, input1a + sizeof(input1a) / sizeof(float));
00126 
00127   for (int i =0; i < rows*10; i++)
00128   {
00129     EXPECT_TRUE(filter->update(v1, v1a));
00130 
00131     for (int j = 1; j < length; j++)
00132     {
00133        EXPECT_NEAR(input1[j], v1a[j], epsilon);
00134     }
00135   }
00136 
00137   delete filter;
00138 }
00139 
00140 TEST(MultiChannelMedianFilterFloat, ThreeRows)
00141 {
00142   float epsilon = 1e-6;
00143   int length = 5;
00144   int rows = 5;
00145   
00146   MultiChannelFilterBase<float > * filter = new MultiChannelMedianFilter<float>();
00147   EXPECT_TRUE(filter->configure(rows, "MultiChannelMedianFilterFloat5"));
00148   
00149   float input1[] = {0,1,2,3,4};
00150   std::vector<float> v1 (input1, input1 + sizeof(input1) / sizeof(float));
00151   float input2[] = {1,2,3,4,5};
00152   std::vector<float> v2 (input2, input2 + sizeof(input2) / sizeof(float));
00153   float input3[] = {2,3,4,5,6};
00154   std::vector<float> v3 (input3, input3 + sizeof(input3) / sizeof(float));
00155   float input1a[] = {1,2,3,4,5};
00156   std::vector<float> v1a (input1a, input1a + sizeof(input1a) / sizeof(float));
00157 
00158   EXPECT_TRUE(filter->update(v1, v1a));
00159   EXPECT_TRUE(filter->update(v2, v1a));
00160   EXPECT_TRUE(filter->update(v3, v1a));
00161 
00162   for (int i = 1; i < length; i++)
00163   {
00164     EXPECT_NEAR(v2[i], v1a[i], epsilon);
00165   }
00166 
00167 }
00168 
00169 
00170 int main(int argc, char **argv){
00171   testing::InitGoogleTest(&argc, argv);
00172   ros::init(argc, argv, "test_median");
00173   return RUN_ALL_TESTS();
00174 }
 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