Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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 #include <geometry/hom_polar.h>
28 #include <geometry/hom_vector.h>
29 
30 #include <cstdlib>
31 #include <cstring>
32 #include <sys/types.h>
33 
34 using namespace fawkes;
35 
36 /** @class LaserProjectionDataFilter "filters/projection.h"
37  * Projects one laser into another laser's plane.
38  * This first transforms the laser value into the target frame, and then
39  * projects it into the X-Y plane (which is assumed to be the laser plane
40  * of another (virtual) laser. Additionally some sanity filtering in all
41  * three axes is applied after the transformation, but before the
42  * projection.
43  * @author Tim Niemueller, Christoph Schwering
44  */
45 
46 /** Constructor.
47  * @param tf_listener transform listener to get transform from
48  * @param target_frame target coordinate fram to project into
49  * @param not_from_x lower X boundary of ignored rectangle
50  * @param not_to_x upper X boundary of ignored rectangle
51  * @param not_from_y lower Y boundary of ignored rectangle
52  * @param not_to_y upper Y boundary of ignored rectangle
53  * @param only_from_z minimum Z value for accepted points
54  * @param only_to_z maximum Z value for accepted points
55  * @param in_data_size number of entries input value arrays
56  * @param in vector of input arrays
57  */
59  tf::TransformListener *tf_listener,
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(in_data_size, in, in.size()),
67  tf_listener_(tf_listener), 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  float* inbuf = in[a]->values;
130  float* outbuf = out[a]->values;
131  memset(outbuf, 0, sizeof(float) * out_data_size);
132 
134  tf::Point p;
135 
136  tf_listener_->lookup_transform(target_frame_, in[a]->frame,
137  fawkes::Time(0, 0), t);
138 
139  if (in_data_size == 360) {
140  for (unsigned int i = 0; i < 360; ++i) {
141  if (inbuf[i] == 0.) continue;
142  p.setValue(inbuf[i] * cos_angles360[i], inbuf[i] * sin_angles360[i], 0.);
143  p = t * p;
144 
145  set_output(outbuf, p);
146  }
147  } else if (in_data_size == 720) {
148  for (unsigned int i = 0; i < 720; ++i) {
149  if (inbuf[i] == 0.) continue;
150 
151  p.setValue(inbuf[i] * cos_angles720[i], inbuf[i] * sin_angles720[i], 0.);
152  p = t * p;
153 
154  set_output(outbuf, p);
155  }
156  } else {
157  for (unsigned int i = 0; i < in_data_size; ++i) {
158  if (inbuf[i] == 0.) continue;
159 
160  float a = deg2rad(360.f / (float)i);
161  p.setValue(inbuf[i] * cos(a), inbuf[i] * sin(a), 0.);
162  p = t * p;
163 
164  set_output(outbuf, p);
165  }
166  }
167  }
168 }
169 
Receive transforms and answer queries.
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:67
A class for handling time.
Definition: time.h:91
Transform that contains a timestamp and frame IDs.
Definition: types.h:91
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:48
LaserProjectionDataFilter(fawkes::tf::TransformListener *tf_listener, 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:58
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:64
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
void filter()
Filter the incoming data.
Definition: projection.cpp:124
std::vector< Buffer * > in
Vector of input arrays.
Definition: filter.h:66
unsigned int in_data_size
Number of entries in input arrays.
Definition: filter.h:65
Laser data filter.
Definition: filter.h:28