Fawkes API  Fawkes Development Version
1080to360.cpp
1 
2 /***************************************************************************
3  * 1080to360.cpp - Laser data data filter to downsample 1080 to 360 values
4  *
5  * Created: Mon Jun 01 16:11:39 2015
6  * Copyright 2006-2015 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 "1080to360.h"
23 
24 #include <core/exception.h>
25 #include <utils/math/angle.h>
26 #include <utils/time/time.h>
27 #include <cstdlib>
28 
29 /** @class Laser1080to360DataFilter "1080to360.h"
30  * Downsample filter from 1080 to 360 values.
31  * @author Tim Niemueller
32  */
33 
34 /** Constructor.
35  * @param filter_name name of this filter instance
36  * @param average if true, beams will be averaged by left and right neighbours,
37  * otherwise every second beam will be used
38  * @param in_data_size number of entries input value arrays
39  * @param in vector of input arrays
40  */
42  bool average,
43  unsigned int in_data_size,
44  std::vector<LaserDataFilter::Buffer *> &in)
45  : LaserDataFilter(filter_name, in_data_size, in, in.size())
46 {
47  if (in_data_size != 1080) {
48  throw fawkes::Exception("1080to360 filter needs input array size of "
49  "1080 entries");
50  }
51  set_out_data_size(360);
52  __average = average;
53 }
54 
55 void
57 {
58  const unsigned int vecsize = std::min(in.size(), out.size());
59  for (unsigned int a = 0; a < vecsize; ++a) {
60  out[a]->frame = in[a]->frame;
61  out[a]->timestamp->set_time(in[a]->timestamp);
62  float *inbuf = in[a]->values;
63  float *outbuf = out[a]->values;
64 
65  if (__average) {
66  for (unsigned int i = 0; i < 360; ++i) {
67  outbuf[i] = (inbuf[i * 3] + inbuf[i * 2 + 1] + inbuf[i * 2 + 2]) / 2.0;
68  }
69  } else {
70  for (unsigned int i = 0; i < 360; ++i) {
71  outbuf[i] = inbuf[i * 3 + 1];
72  }
73  }
74  }
75 }
std::vector< Buffer * > out
Vector of output arrays.
Definition: filter.h:76
void filter()
Filter the incoming data.
Definition: 1080to360.cpp:56
Base class for exceptions in Fawkes.
Definition: exception.h:36
Laser1080to360DataFilter(std::string filter_name, bool average, unsigned int in_data_size, std::vector< LaserDataFilter::Buffer *> &in)
Constructor.
Definition: 1080to360.cpp:41
virtual void set_out_data_size(unsigned int data_size)
Resize output arrays.
Definition: filter.cpp:157
std::vector< Buffer * > in
Vector of input arrays.
Definition: filter.h:75
Laser data filter.
Definition: filter.h:32