test_chain.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include "gtest/gtest.h"
31 #include "filters/filter_chain.h"
32 
33 
34 
35 TEST(MultiChannelFilterChain, configuring){
36  double epsilon = 1e-9;
38 
39  EXPECT_TRUE(chain.configure(5, "MultiChannelMeanFilterDouble5"));
40 
41  double input1[] = {1,2,3,4,5};
42  double input1a[] = {9,9,9,9,9};//seed w/incorrect values
43  std::vector<double> v1 (input1, input1 + sizeof(input1) / sizeof(double));
44  std::vector<double> v1a (input1a, input1a + sizeof(input1a) / sizeof(double));
45 
46 
47  EXPECT_TRUE(chain.update(v1, v1a));
48 
49  chain.clear();
50 
51  for (unsigned int i = 1; i < v1.size(); i++)
52  {
53  EXPECT_NEAR(input1[i], v1a[i], epsilon);
54  }
55 }
56 TEST(FilterChain, configuring){
57  double epsilon = 1e-9;
58  filters::FilterChain<float> chain("float");
59 
60  EXPECT_TRUE(chain.configure("MeanFilterFloat5"));
61 
62  float v1 = 1;
63  float v1a = 9;
64 
65  EXPECT_TRUE(chain.update(v1, v1a));
66 
67  chain.clear();
68 
69  EXPECT_NEAR(v1, v1a, epsilon);
70 
71  }
72 
73 TEST(MultiChannelFilterChain, MisconfiguredNumberOfChannels){
75 
76  EXPECT_TRUE(chain.configure(10, "MultiChannelMedianFilterDouble5"));
77 
78  // EXPECT_TRUE(chain.configure(10));
79 
80  double input1[] = {1,2,3,4,5};
81  double input1a[] = {1,2,3,4,5};
82  std::vector<double> v1 (input1, input1 + sizeof(input1) / sizeof(double));
83  std::vector<double> v1a (input1a, input1a + sizeof(input1a) / sizeof(double));
84 
85 
86  EXPECT_FALSE(chain.update(v1, v1a));
87 
88  chain.clear();
89 
90 }
91 
92 TEST(MultiChannelFilterChain, TwoFilters){
93  double epsilon = 1e-9;
95 
96  EXPECT_TRUE(chain.configure(5, "TwoFilters"));
97 
98  double input1[] = {1,2,3,4,5};
99  double input1a[] = {9,9,9,9,9};//seed w/incorrect values
100  std::vector<double> v1 (input1, input1 + sizeof(input1) / sizeof(double));
101  std::vector<double> v1a (input1a, input1a + sizeof(input1a) / sizeof(double));
102 
103 
104  EXPECT_TRUE(chain.update(v1, v1a));
105 
106  chain.clear();
107 
108  for (unsigned int i = 1; i < v1.size(); i++)
109  {
110  EXPECT_NEAR(input1[i], v1a[i], epsilon);
111  }
112 }
113 
114 
115 TEST(MultiChannelFilterChain, TransferFunction){
116  double epsilon = 1e-4;
117 
119  EXPECT_TRUE(chain.configure(3, "TransferFunction" ));
120 
121  std::vector<double> in1,in2,in3,in4,in5,in6,in7;
122  std::vector<double> out1;
123 
124  in1.push_back(10.0);
125  in1.push_back(10.0);
126  in1.push_back(10.0);
127  //
128  in2.push_back(70.0);
129  in2.push_back(30.0);
130  in2.push_back(8.0);
131  //
132  in3.push_back(-1.0);
133  in3.push_back(5.0);
134  in3.push_back(22.0);
135  //
136  in4.push_back(44.0);
137  in4.push_back(23.0);
138  in4.push_back(8.0);
139  //
140  in5.push_back(10.0);
141  in5.push_back(10.0);
142  in5.push_back(10.0);
143  //
144  in6.push_back(5.0);
145  in6.push_back(-1.0);
146  in6.push_back(5.0);
147  //
148  in7.push_back(6.0);
149  in7.push_back(-30.0);
150  in7.push_back(2.0);
151  //
152  out1.push_back(17.1112);
153  out1.push_back(9.0285);
154  out1.push_back(8.3102);
155  EXPECT_TRUE(chain.update(in1, in1));
156  EXPECT_TRUE(chain.update(in2, in2));
157  EXPECT_TRUE(chain.update(in3, in3));
158  EXPECT_TRUE(chain.update(in4, in4));
159  EXPECT_TRUE(chain.update(in5, in5));
160  EXPECT_TRUE(chain.update(in6, in6));
161  EXPECT_TRUE(chain.update(in7, in7));
162 
163  chain.clear();
164 
165  for(unsigned int i=0; i<out1.size(); i++)
166  {
167  EXPECT_NEAR(out1[i], in7[i], epsilon);
168  }
169 }
170 
171 /*
172 TEST(MultiChannelFilterChain, OverlappingNames){
173  filters::MultiChannelFilterChain<double> chain("double");
174 
175 
176  std::string bad_xml = "<filters> <filter type=\"MultiChannelMeanFilterDouble\" name=\"mean_test\"> <params number_of_observations=\"5\"/></filter><filter type=\"MedianFilter\" name=\"mean_test\"> <params number_of_observations=\"5\"/></filter></filters>";
177 
178  TiXmlDocument chain_def = TiXmlDocument();
179  chain_def.Parse(bad_xml.c_str());
180  TiXmlElement * config = chain_def.RootElement();
181 
182  EXPECT_FALSE(chain.configure(5, config));
183 
184 }
185 */
186 
187 TEST(FilterChain, ReconfiguringChain){
188  filters::FilterChain<int> chain("int");
189 
190  int v1 = 1;
191  int v1a = 9;
192 
193  EXPECT_TRUE(chain.configure("OneIncrements"));
194  EXPECT_TRUE(chain.update(v1, v1a));
195  EXPECT_EQ(2, v1a);
196  chain.clear();
197 
198  EXPECT_TRUE(chain.configure("TwoIncrements"));
199  EXPECT_TRUE(chain.update(v1, v1a));
200  EXPECT_EQ(3, v1a);
201  chain.clear();
202 
203 }
204 
205 TEST(FilterChain, ThreeIncrementChains){
206  filters::FilterChain<int> chain("int");
207  int v1 = 1;
208  int v1a = 9;
209 
210  EXPECT_TRUE(chain.configure("ThreeIncrements"));
211  EXPECT_TRUE(chain.update(v1, v1a));
212  EXPECT_EQ(4, v1a);
213  chain.clear();
214 
215 }
216 
217 TEST(FilterChain, TenIncrementChains){
218  filters::FilterChain<int> chain("int");
219  int v1 = 1;
220  int v1a = 9;
221 
222  EXPECT_TRUE(chain.configure("TenIncrements"));
223  EXPECT_TRUE(chain.update(v1, v1a));
224  EXPECT_EQ(11, v1a);
225  chain.clear();
226 
227 }
228 /*
229 TEST(MultiChannelFilterChain, ReconfiguringMultiChannelChain){
230  filters::MultiChannelFilterChain<int> chain("int");
231 
232  int v1 = 1;
233  int v1a = 9;
234 
235  EXPECT_TRUE(chain.configure("OneMultiChannelIncrements"));
236  EXPECT_TRUE(chain.update(v1, v1a));
237  EXPECT_EQ(2, v1a);
238  chain.clear();
239 
240  EXPECT_TRUE(chain.configure("TwoMultiChannelIncrements"));
241  EXPECT_TRUE(chain.update(v1, v1a));
242  EXPECT_EQ(3, v1a);
243  chain.clear();
244 
245 }
246 
247 TEST(MultiChannelFilterChain, ThreeMultiChannelIncrementChains){
248  filters::MultiChannelFilterChain<int> chain("int");
249  int v1 = 1;
250  int v1a = 9;
251 
252  EXPECT_TRUE(chain.configure("ThreeMultiChannelIncrements"));
253  EXPECT_TRUE(chain.update(v1, v1a));
254  EXPECT_EQ(4, v1a);
255  chain.clear();
256 
257 }
258 */
259 TEST(MultiChannelFilterChain, TenMultiChannelIncrementChains){
261  std::vector<int> v1;
262  v1.push_back(1);
263  v1.push_back(1);
264  v1.push_back(1);
265  std::vector<int> v1a = v1;
266 
267  EXPECT_TRUE(chain.configure(3, "TenMultiChannelIncrements"));
268  EXPECT_TRUE(chain.update(v1, v1a));
269  for (unsigned int i = 0; i < 3; i++)
270  {
271  EXPECT_EQ(11, v1a[i]);
272  }
273  chain.clear();
274 
275 }
276 
277 
278 int main(int argc, char **argv){
279  testing::InitGoogleTest(&argc, argv);
280  ros::init(argc, argv, "test_chain");
281  return RUN_ALL_TESTS();
282 }
double epsilon
bool clear()
Clear all filters from this chain.
Definition: filter_chain.h:362
A class which will construct and sequentially call Filters according to xml This is the primary way i...
Definition: filter_chain.h:274
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool update(const std::vector< T > &data_in, std::vector< T > &data_out)
process data through each of the filters added sequentially
Definition: filter_chain.h:316
int main(int argc, char **argv)
Definition: test_chain.cpp:278
A class which will construct and sequentially call Filters according to xml This is the primary way i...
Definition: filter_chain.h:47
TEST(MultiChannelFilterChain, configuring)
Definition: test_chain.cpp:35
bool clear()
Clear all filters from this chain.
Definition: filter_chain.h:130
bool update(const T &data_in, T &data_out)
process data through each of the filters added sequentially
Definition: filter_chain.h:92
bool configure(std::string param_name, ros::NodeHandle node=ros::NodeHandle())
Configure the filter chain from a configuration stored on the parameter server.
Definition: filter_chain.h:74
bool configure(unsigned int size, std::string param_name, ros::NodeHandle node=ros::NodeHandle())
Configure the filter chain from a configuration stored on the parameter server.
Definition: filter_chain.h:295


filters
Author(s):
autogenerated on Mon Jun 10 2019 13:15:08