30 #include <gtest/gtest.h> 40 timeval temp_time_struct;
41 gettimeofday(&temp_time_struct,NULL);
42 srand(temp_time_struct.tv_usec);
45 void generate_rand_vectors(
double scale, uint64_t runs, std::vector<double>& xvalues, std::vector<double>& yvalues, std::vector<double>&zvalues)
48 for ( uint64_t i = 0; i < runs ; i++ )
50 xvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
51 yvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
52 zvalues[i] = 1.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
56 TEST(MultiChannelMedianFilterDouble, ConfirmIdentityNRows)
63 EXPECT_TRUE(filter->
configure(rows,
"MultiChannelMedianFilterDouble5"));
66 double input1[] = {1,2,3,4,5};
67 double input1a[] = {11,12,13,14,15};
68 std::vector<double> v1 (input1, input1 +
sizeof(input1) /
sizeof(
double));
69 std::vector<double> v1a (input1a, input1a +
sizeof(input1a) /
sizeof(
double));
71 for (
int i =0; i < rows*10; i++)
73 EXPECT_TRUE(filter->
update(v1, v1a));
75 for (
int j = 1; j < length; j++)
77 EXPECT_NEAR(input1[j], v1a[j], epsilon);
84 TEST(MultiChannelMedianFilterDouble, ThreeRows)
91 EXPECT_TRUE(filter->
configure(rows,
"MultiChannelMedianFilterDouble5" ));
93 double input1[] = {0,1,2,3,4};
94 std::vector<double> v1 (input1, input1 +
sizeof(input1) /
sizeof(
double));
95 double input2[] = {1,2,3,4,5};
96 std::vector<double> v2 (input2, input2 +
sizeof(input2) /
sizeof(
double));
97 double input3[] = {2,3,4,5,6};
98 std::vector<double> v3 (input3, input3 +
sizeof(input3) /
sizeof(
double));
99 double input1a[] = {1,2,3,4,5};
100 std::vector<double> v1a (input1a, input1a +
sizeof(input1a) /
sizeof(
double));
102 EXPECT_TRUE(filter->
update(v1, v1a));
103 EXPECT_TRUE(filter->
update(v2, v1a));
104 EXPECT_TRUE(filter->
update(v3, v1a));
106 for (
int i = 1; i < length; i++)
108 EXPECT_NEAR(v2[i], v1a[i], epsilon);
113 TEST(MultiChannelMedianFilterFloat, ConfirmIdentityNRows)
120 EXPECT_TRUE(filter->
configure(rows,
"MultiChannelMedianFilterFloat5" ));
122 float input1[] = {1,2,3,4,5};
123 float input1a[] = {1,2,3,4,5};
124 std::vector<float> v1 (input1, input1 +
sizeof(input1) /
sizeof(
float));
125 std::vector<float> v1a (input1a, input1a +
sizeof(input1a) /
sizeof(
float));
127 for (
int i =0; i < rows*10; i++)
129 EXPECT_TRUE(filter->
update(v1, v1a));
131 for (
int j = 1; j < length; j++)
133 EXPECT_NEAR(input1[j], v1a[j], epsilon);
140 TEST(MultiChannelMedianFilterFloat, ThreeRows)
147 EXPECT_TRUE(filter->
configure(rows,
"MultiChannelMedianFilterFloat5"));
149 float input1[] = {0,1,2,3,4};
150 std::vector<float> v1 (input1, input1 +
sizeof(input1) /
sizeof(
float));
151 float input2[] = {1,2,3,4,5};
152 std::vector<float> v2 (input2, input2 +
sizeof(input2) /
sizeof(
float));
153 float input3[] = {2,3,4,5,6};
154 std::vector<float> v3 (input3, input3 +
sizeof(input3) /
sizeof(
float));
155 float input1a[] = {1,2,3,4,5};
156 std::vector<float> v1a (input1a, input1a +
sizeof(input1a) /
sizeof(
float));
158 EXPECT_TRUE(filter->
update(v1, v1a));
159 EXPECT_TRUE(filter->
update(v2, v1a));
160 EXPECT_TRUE(filter->
update(v3, v1a));
162 for (
int i = 1; i < length; i++)
164 EXPECT_NEAR(v2[i], v1a[i], epsilon);
170 int main(
int argc,
char **argv){
171 testing::InitGoogleTest(&argc, argv);
173 return RUN_ALL_TESTS();
virtual bool update(const std::vector< T > &data_in, std::vector< T > &data_out)=0
Update the filter and return the data seperately.
bool configure(unsigned int number_of_channels, const std::string ¶m_name, ros::NodeHandle node_handle=ros::NodeHandle())
Configure the filter from the parameter server.
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)