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 <sys/types.h>
27 #include <utils/math/angle.h>
28 
29 #include <cstdlib>
30 #include <cstring>
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  tf::Transformer * tf,
59  std::string target_frame,
60  float not_from_x,
61  float not_to_x,
62  float not_from_y,
63  float not_to_y,
64  float only_from_z,
65  float only_to_z,
66  unsigned int in_data_size,
67  std::vector<LaserDataFilter::Buffer *> &in)
68 : LaserDataFilter(filter_name, in_data_size, in, in.size()),
69  tf_(tf),
70  target_frame_(target_frame),
71  not_from_x_(not_from_x),
72  not_to_x_(not_to_x),
73  not_from_y_(not_from_y),
74  not_to_y_(not_to_y),
75  only_from_z_(only_from_z),
76  only_to_z_(only_to_z)
77 {
78  // Generate lookup tables for sin and cos
79  for (unsigned int i = 0; i < 360; ++i) {
80  sin_angles360[i] = sinf(deg2rad(i));
81  cos_angles360[i] = cosf(deg2rad(i));
82  }
83  for (unsigned int i = 0; i < 720; ++i) {
84  sin_angles720[i] = sinf(deg2rad((float)i / 2.));
85  cos_angles720[i] = cosf(deg2rad((float)i / 2.));
86  }
87 
88  index_factor_ = out_data_size / 360.;
89 }
90 
91 LaserProjectionDataFilter::~LaserProjectionDataFilter()
92 {
93 }
94 
95 /** Set the output buffer applying filtering.
96  * This checks the given point against the configured bounds. If and
97  * only if the point satisfies the given criteria it is set at the
98  * appropriate array index of the buffer.
99  * @param outbuf buffer in which to set the value conditionally
100  * @param p point to check and (maybe) set
101  */
102 inline void
103 LaserProjectionDataFilter::set_output(float *outbuf, fawkes::tf::Point &p)
104 {
105  if (((p.x() >= not_from_x_) && (p.x() <= not_to_x_) && (p.y() >= not_from_y_)
106  && (p.y() <= not_to_y_))
107  || (p.z() < only_from_z_) || (p.z() > only_to_z_)) {
108  // value is inside "forbidden" robot rectangle or
109  // below or above Z thresholds
110  return;
111  }
112 
113  p.setZ(0.);
114  float phi = atan2f(p.y(), p.x());
115 
116  unsigned int j = (unsigned int)roundf(rad2deg(normalize_rad(phi)) * index_factor_);
117  if (j > out_data_size)
118  j = 0; // might happen just at the boundary
119 
120  if (outbuf[j] == 0.) {
121  outbuf[j] = (float)p.length();
122  } else {
123  outbuf[j] = std::min(outbuf[j], (float)p.length());
124  }
125 }
126 
127 void
129 {
130  const unsigned int vecsize = std::min(in.size(), out.size());
131  for (unsigned int a = 0; a < vecsize; ++a) {
132  out[a]->frame = target_frame_;
133  out[a]->timestamp->set_time(in[a]->timestamp);
134  float *inbuf = in[a]->values;
135  float *outbuf = out[a]->values;
136  memset(outbuf, 0, sizeof(float) * out_data_size);
137 
139  tf::Point p;
140 
141  tf_->lookup_transform(target_frame_, in[a]->frame, fawkes::Time(0, 0), t);
142 
143  if (in_data_size == 360) {
144  for (unsigned int i = 0; i < 360; ++i) {
145  if (inbuf[i] == 0.)
146  continue;
147  p.setValue((btScalar)inbuf[i] * cos_angles360[i],
148  (btScalar)inbuf[i] * sin_angles360[i],
149  0.);
150  p = t * p;
151 
152  set_output(outbuf, p);
153  }
154  } else if (in_data_size == 720) {
155  for (unsigned int i = 0; i < 720; ++i) {
156  if (inbuf[i] == 0.)
157  continue;
158 
159  p.setValue((btScalar)inbuf[i] * cos_angles720[i],
160  (btScalar)inbuf[i] * sin_angles720[i],
161  0.);
162  p = t * p;
163 
164  set_output(outbuf, p);
165  }
166  } else {
167  for (unsigned int i = 0; i < in_data_size; ++i) {
168  if (inbuf[i] == 0.)
169  continue;
170 
171  float a = deg2rad((float)i / ((float)in_data_size / 360.f));
172  p.setValue((btScalar)inbuf[i] * cos(a), (btScalar)inbuf[i] * sin(a), 0.);
173  p = t * p;
174 
175  set_output(outbuf, p);
176  }
177  }
178  }
179 }
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
Definition: angle.h:90
std::vector< Buffer * > out
Vector of output arrays.
Definition: filter.h:90
Fawkes library namespace.
A class for handling time.
Definition: time.h:92
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:46
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
LaserProjectionDataFilter(const 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
unsigned int out_data_size
Number of entries in output arrays.
Definition: filter.h:87
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
Coordinate transforms between any two frames in a system.
Definition: transformer.h:64
void filter()
Filter the incoming data.
Definition: projection.cpp:128
std::vector< Buffer * > in
Vector of input arrays.
Definition: filter.h:89
unsigned int in_data_size
Number of entries in input arrays.
Definition: filter.h:88
Laser data filter.
Definition: filter.h:32