Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
720to360.cpp
1 
2 /***************************************************************************
3  * 720to360.cpp - Laser data data filter to downsample 720 to 360 values
4  *
5  * Created: Tue Jun 23 14:37:36 2009
6  * Copyright 2006-2009 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "720to360.h"
24 
25 #include <core/exception.h>
26 #include <utils/math/angle.h>
27 #include <cstdlib>
28 
29 /** @class Laser720to360DataFilter "720to360.h"
30  * Downsample filter from 720 to 360 values.
31  * @author Tim Niemueller
32  */
33 
34 /** Constructor.
35  * @param average if true, beams will be averaged by left and right neighbours,
36  * otherwise every second beam will be used
37  * @param in_data_size number of entries input value arrays
38  * @param in vector of input arrays
39  */
41  unsigned int in_data_size,
42  std::vector<LaserDataFilter::Buffer *> &in)
43  : LaserDataFilter(in_data_size, in, in.size())
44 {
45  if (in_data_size != 720) {
46  throw fawkes::Exception("720to360 filter needs input array size of "
47  "720 entries");
48  }
49  set_out_data_size(360);
50  __average = average;
51 }
52 
53 void
55 {
56  const unsigned int vecsize = std::min(in.size(), out.size());
57  for (unsigned int a = 0; a < vecsize; ++a) {
58  out[a]->frame = in[a]->frame;
59  float *inbuf = in[a]->values;
60  float *outbuf = out[a]->values;
61 
62  if (__average) {
63  outbuf[0] = (inbuf[719] + inbuf[0]) / 2.0;
64  for (unsigned int i = 1; i < 360; ++i) {
65  outbuf[i] = (inbuf[i * 2 - 1] + inbuf[i * 2 + 1]) / 2.0;
66  }
67  } else {
68  for (unsigned int i = 0; i < 360; ++i) {
69  outbuf[i] = inbuf[i * 2];
70  }
71  }
72  }
73 }
std::vector< Buffer * > out
Vector of output arrays.
Definition: filter.h:67
void filter()
Filter the incoming data.
Definition: 720to360.cpp:54
Base class for exceptions in Fawkes.
Definition: exception.h:36
Laser720to360DataFilter(bool average, unsigned int in_data_size, std::vector< LaserDataFilter::Buffer * > &in)
Constructor.
Definition: 720to360.cpp:40
virtual void set_out_data_size(unsigned int data_size)
Resize output arrays.
Definition: filter.cpp:151
std::vector< Buffer * > in
Vector of input arrays.
Definition: filter.h:66
Laser data filter.
Definition: filter.h:28