Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
filter.cpp
1 
2 /***************************************************************************
3  * filter.cpp - Laser data filter interface
4  *
5  * Created: Fri Oct 10 17:12:29 2008
6  * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "filter.h"
23 #include <core/exception.h>
24 
25 #include <cstring>
26 #include <cstdlib>
27 
28 /** @class LaserDataFilter "filter.h"
29  * Laser data filter.
30  * With this interface laser filter are described. These filters take laser
31  * readings as input, mangle them and return a new array of filtered laser data.
32  * @author Tim Niemueller
33  *
34  * @fn void LaserDataFilter::filter() = 0
35  * Filter the incoming data.
36  * Function shall filter the data in the "in" member vector and write output
37  * to the "out" member vector.
38  */
39 
40 /** @var LaserDataFilter::in
41  * Vector of input arrays.
42  * Each entry in the vector is an array of data_size entries. It depends on
43  * the filter how multiple inputs are processed.
44  */
45 
46 /** @var LaserDataFilter::out
47  * Vector of output arrays.
48  * Each entry in the vector is an array of data_size entries. It depends on
49  * the filter how multiple outputs are generated.
50  */
51 
52 /** @var LaserDataFilter::in_data_size
53  * Number of entries in input arrays.
54  */
55 
56 /** @var LaserDataFilter::out_data_size
57  * Number of entries in output arrays.
58  */
59 
60 /** @class LaserDataFilter::Buffer "filter.h"
61  * Laser data buffer.
62  * A buffer comprises the value array and a reference frame ID.
63  */
64 
65 /** Constructor.
66  * @param in_data_size number of entries input value arrays
67  * @param in vector of input arrays
68  * @param out_size number of value arrays to generate in out vector
69  */
70 LaserDataFilter::LaserDataFilter(unsigned int in_data_size,
71  std::vector<Buffer *> &in, unsigned int out_size)
72 {
73  this->in = in;
74  this->in_data_size = in_data_size;
75  this->out_data_size = in_data_size; // yes, in_data_size!
76 
77  if (out_size > 0) out.resize(out_size);
78  for (unsigned int i = 0; i < out_size; ++i) {
79  out[i] = new Buffer(out_data_size);
80  }
81 
82  __own_in = false;
83  __own_out = true;
84 }
85 
86 
87 /** Virtual empty destructor. */
89 {
90  if (__own_in) {
91  for (unsigned int i = 0; i < in.size(); ++i) {
92  free(in[i]->values);
93  delete in[i];
94  }
95  }
96  if (__own_out) {
97  for (unsigned int i = 0; i < out.size(); ++i) {
98  free(out[i]->values);
99  delete out[i];
100  }
101  }
102 }
103 
104 
105 /** Get filtered data array
106  * @return a Buffer with an array of the same size as the last array
107  * given to filter() or NULL if filter() was never called.
108  */
109 std::vector<LaserDataFilter::Buffer *> &
111 {
112  return out;
113 }
114 
115 
116 /** Set filtered data array
117  * @param out vector of output values. The vector is only accepted if it has
118  * the same size as the current one. The filter will now longer assume
119  * ownership of the arrays in the vector. Either free the memory or call
120  * set_array_ownership().
121  */
122 void
123 LaserDataFilter::set_out_vector(std::vector<Buffer *> &out)
124 {
125  if (this->out.size() != out.size()) {
126  throw fawkes::Exception("Filter out vector size mismatch: %zu vs. %zu",
127  this->out.size(), out.size());
128  }
129 
130  if (__own_out) {
131  for (unsigned int i = 0; i < this->out.size(); ++i) {
132  free(this->out[i]->values);
133  delete this->out[i];
134  }
135  }
136  this->out.clear();
137 
138  this->out = out;
139  __own_out = false;
140 }
141 
142 
143 /** Resize output arrays.
144  * A side effect is that the output array size will be owned afterwards.
145  * Call this method only in constructors! Note that the output arrays are
146  * only recreated if own by the filter. If you passed an out vector you have
147  * to make sure the contained arrays fit (before calling set_out_vector()!).
148  * @param data_size number of entries in output arrays.
149  */
150 void
151 LaserDataFilter::set_out_data_size(unsigned int data_size)
152 {
153  if (out_data_size != data_size) {
154  if (__own_out) {
155  for (unsigned int i = 0; i < out.size(); ++i) {
156  free(out[i]->values);
157  out[i]->values = (float *)malloc(data_size * sizeof(float));
158  }
159  }
160  }
161 
162  out_data_size = data_size;
163 }
164 
165 
166 /** Get size of filtered data array
167  * @return size of filtered data array or 0 if filter() was never called.
168  */
169 unsigned int
171 {
172  return out_data_size;
173 }
174 
175 
176 /** Resets all readings in outbuf to 0.0
177  * @param outbuf array of out_data_size
178  */
179 void
181 {
182  memset(outbuf->values, 0, sizeof(float) * out_data_size);
183 }
184 
185 /** Copies the readings from inbuf to outbuf.
186  * Requires out_data_size to be equal to in_data_size.
187  * @param inbuf array of in_data_size (= out_data_size) readings
188  * @param outbuf array of out_data_size (= in_data_size) readings
189  */
190 void
192  const LaserDataFilter::Buffer *inbuf)
193 {
194  if (in_data_size != out_data_size) {
195  throw fawkes::Exception("copy_to_outbuf() requires equal "\
196  "input and output data size");
197  }
198  memcpy(outbuf->values, inbuf->values, sizeof(float) * out_data_size);
199 }
200 
201 
202 /** Set input/output array ownership.
203  * Owned arrays will be freed on destruction or when setting new arrays.
204  * @param own_in true to assign ownership of input arrays, false otherwise
205  * @param own_out true to assign ownership of output arrays, false otherwise
206  */
207 void
208 LaserDataFilter::set_array_ownership(bool own_in, bool own_out)
209 {
210  __own_in = own_in;
211  __own_out = own_out;
212 }
213 
214 
215 /** Constructor.
216  * @param num_values if not zero allocates the values arrays with the
217  * given number of elements
218  */
220 {
221  if (num_values > 0) {
222  values = (float *)malloc(num_values * sizeof(float));
223  }
224 }