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
32using 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
91LaserProjectionDataFilter::~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 */
102inline void
103LaserProjectionDataFilter::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
127void
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}
Laser data filter.
Definition: filter.h:33
unsigned int out_data_size
Number of entries in output arrays.
Definition: filter.h:87
unsigned int in_data_size
Number of entries in input arrays.
Definition: filter.h:88
std::vector< Buffer * > out
Vector of output arrays.
Definition: filter.h:90
std::vector< Buffer * > in
Vector of input arrays.
Definition: filter.h:89
void filter()
Filter the incoming data.
Definition: projection.cpp:128
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
A class for handling time.
Definition: time.h:93
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
Definition: angle.h:90
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:46