00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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 }