23 #include "projection.h" 25 #include <core/exception.h> 26 #include <utils/math/angle.h> 30 #include <sys/types.h> 58 const std::string filter_name,
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)
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)
73 for (
unsigned int i = 0; i < 360; ++i) {
74 sin_angles360[i] = sinf(
deg2rad(i));
75 cos_angles360[i] = cosf(
deg2rad(i));
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.));
85 LaserProjectionDataFilter::~LaserProjectionDataFilter()
98 LaserProjectionDataFilter::set_output(
float *outbuf, fawkes::tf::Point &p)
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_) )
110 float phi = atan2f(p.y(), p.x());
116 if (outbuf[j] == 0.) {
117 outbuf[j] = (float)p.length();
119 outbuf[j] = std::min(outbuf[j], (
float)p.length());
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;
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.);
146 set_output(outbuf, p);
149 for (
unsigned int i = 0; i < 720; ++i) {
150 if (inbuf[i] == 0.)
continue;
152 p.setValue(inbuf[i] * cos_angles720[i], inbuf[i] * sin_angles720[i], 0.);
155 set_output(outbuf, p);
159 if (inbuf[i] == 0.)
continue;
161 float a =
deg2rad(360.f / (
float)i);
162 p.setValue(inbuf[i] * cos(a), inbuf[i] * sin(a), 0.);
165 set_output(outbuf, p);
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
std::vector< Buffer * > out
Vector of output arrays.
Fawkes library namespace.
A class for handling time.
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.
float rad2deg(float rad)
Convert an angle given in radians to degrees.
unsigned int out_data_size
Number of entries in output arrays.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
void filter()
Filter the incoming data.
std::vector< Buffer * > in
Vector of input arrays.
unsigned int in_data_size
Number of entries in input arrays.