Fawkes API  Fawkes Development Version
projection.cpp
1 
2 /***************************************************************************
3  * projection.cpp - Laser data projection filter
4  *
5  * Created: Tue Mar 22 16:30:51 2011
6  * Copyright 2011 Christoph Schwering
7  * 2011 Tim Niemueller
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 "projection.h"
24 
25 #include <core/exception.h>
26 #include <utils/math/angle.h>
27 
28 #include <cstdlib>
29 #include <cstring>
30 #include <sys/types.h>
31 
32 using namespace fawkes;
33 
34 /** @class LaserProjectionDataFilter "filters/projection.h"
35  * Projects one laser into another laser's plane.
36  * This first transforms the laser value into the target frame, and then
37  * projects it into the X-Y plane (which is assumed to be the laser plane
38  * of another (virtual) laser. Additionally some sanity filtering in all
39  * three axes is applied after the transformation, but before the
40  * projection.
41  * @author Tim Niemueller, Christoph Schwering
42  */
43 
44 /** Constructor.
45  * @param filter_name name of this filter
46  * @param tf transformer to get transform from
47  * @param target_frame target coordinate fram to project into
48  * @param not_from_x lower X boundary of ignored rectangle
49  * @param not_to_x upper X boundary of ignored rectangle
50  * @param not_from_y lower Y boundary of ignored rectangle
51  * @param not_to_y upper Y boundary of ignored rectangle
52  * @param only_from_z minimum Z value for accepted points
53  * @param only_to_z maximum Z value for accepted points
54  * @param in_data_size number of entries input value arrays
55  * @param in vector of input arrays
56  */
58  const std::string filter_name,
59  tf::Transformer *tf,
60  std::string target_frame,
61  float not_from_x, float not_to_x,
62  float not_from_y, float not_to_y,
63  float only_from_z, float only_to_z,
64  unsigned int in_data_size,
65  std::vector<LaserDataFilter::Buffer *> &in)
66  : LaserDataFilter(filter_name, in_data_size, in, in.size()),
67  tf_(tf), target_frame_(target_frame),
68  not_from_x_(not_from_x), not_to_x_(not_to_x),
69  not_from_y_(not_from_y), not_to_y_(not_to_y),
70  only_from_z_(only_from_z), only_to_z_(only_to_z)
71 {
72  // Generate lookup tables for sin and cos
73  for (unsigned int i = 0; i < 360; ++i) {
74  sin_angles360[i] = sinf(deg2rad(i));
75  cos_angles360[i] = cosf(deg2rad(i));
76  }
77  for (unsigned int i = 0; i < 720; ++i) {
78  sin_angles720[i] = sinf(deg2rad((float)i / 2.));
79  cos_angles720[i] = cosf(deg2rad((float)i / 2.));
80  }
81 
82  index_factor_ = out_data_size / 360.;
83 }
84 
85 LaserProjectionDataFilter::~LaserProjectionDataFilter()
86 {
87 }
88 
89 
90 /** Set the output buffer applying filtering.
91  * This checks the given point against the configured bounds. If and
92  * only if the point satisfies the given criteria it is set at the
93  * appropriate array index of the buffer.
94  * @param outbuf buffer in which to set the value conditionally
95  * @param p point to check and (maybe) set
96  */
97 inline void
98 LaserProjectionDataFilter::set_output(float *outbuf, fawkes::tf::Point &p)
99 {
100  if ( ((p.x() >= not_from_x_) && (p.x() <= not_to_x_) &&
101  (p.y() >= not_from_y_) && (p.y() <= not_to_y_)) ||
102  (p.z() < only_from_z_) || (p.z() > only_to_z_) )
103  {
104  // value is inside "forbidden" robot rectangle or
105  // below or above Z thresholds
106  return;
107  }
108 
109  p.setZ(0.);
110  float phi = atan2f(p.y(), p.x());
111 
112  unsigned int j =
113  (unsigned int)roundf(rad2deg(normalize_rad(phi)) * index_factor_);
114  if (j > out_data_size) j = 0; // might happen just at the boundary
115 
116  if (outbuf[j] == 0.) {
117  outbuf[j] = (float)p.length();
118  } else {
119  outbuf[j] = std::min(outbuf[j], (float)p.length());
120  }
121 }
122 
123 void
125 {
126  const unsigned int vecsize = std::min(in.size(), out.size());
127  for (unsigned int a = 0; a < vecsize; ++a) {
128  out[a]->frame = target_frame_;
129  out[a]->timestamp->set_time(in[a]->timestamp);
130  float* inbuf = in[a]->values;
131  float* outbuf = out[a]->values;
132  memset(outbuf, 0, sizeof(float) * out_data_size);
133 
135  tf::Point p;
136 
137  tf_->lookup_transform(target_frame_, in[a]->frame,
138  fawkes::Time(0, 0), t);
139 
140  if (in_data_size == 360) {
141  for (unsigned int i = 0; i < 360; ++i) {
142  if (inbuf[i] == 0.) continue;
143  p.setValue(inbuf[i] * cos_angles360[i], inbuf[i] * sin_angles360[i], 0.);
144  p = t * p;
145 
146  set_output(outbuf, p);
147  }
148  } else if (in_data_size == 720) {
149  for (unsigned int i = 0; i < 720; ++i) {
150  if (inbuf[i] == 0.) continue;
151 
152  p.setValue(inbuf[i] * cos_angles720[i], inbuf[i] * sin_angles720[i], 0.);
153  p = t * p;
154 
155  set_output(outbuf, p);
156  }
157  } else {
158  for (unsigned int i = 0; i < in_data_size; ++i) {
159  if (inbuf[i] == 0.) continue;
160 
161  float a = deg2rad(360.f / (float)i);
162  p.setValue(inbuf[i] * cos(a), inbuf[i] * sin(a), 0.);
163  p = t * p;
164 
165  set_output(outbuf, p);
166  }
167  }
168  }
169 }
170 
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
Definition: angle.h:93
std::vector< Buffer * > out
Vector of output arrays.
Definition: filter.h:76
Fawkes library namespace.
A class for handling time.
Definition: time.h:91
LaserProjectionDataFilter(std::string filter_name, fawkes::tf::Transformer *tf, std::string target_frame, float not_from_x, float not_to_x, float not_from_y, float not_to_y, float only_from_z, float only_to_z, unsigned int in_data_size, std::vector< LaserDataFilter::Buffer *> &in)
Constructor.
Definition: projection.cpp:57
Transform that contains a timestamp and frame IDs.
Definition: types.h:96
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:48
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
unsigned int out_data_size
Number of entries in output arrays.
Definition: filter.h:73
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
Coordinate transforms between any two frames in a system.
Definition: transformer.h:68
void filter()
Filter the incoming data.
Definition: projection.cpp:124
std::vector< Buffer * > in
Vector of input arrays.
Definition: filter.h:75
unsigned int in_data_size
Number of entries in input arrays.
Definition: filter.h:74
Laser data filter.
Definition: filter.h:32