23 #include "box_filter.h"
25 #include <core/exception.h>
26 #include <utils/math/coord.h>
27 #include <utils/time/time.h>
51 unsigned int in_data_size,
52 std::vector<LaserDataFilter::Buffer *> &in,
59 tf_listener_ = tf_listener;
62 frame_map_ = config_->
get_string(
"/frames/fixed");
63 cfg_occupied_thresh_ = std::numeric_limits<float>::max();
75 LaserBoxFilterDataFilter::point_in_rectangle(
float x,
float y)
87 bool is_in_rect =
false;
89 std::vector<Box>::iterator it;
90 for (it = boxes_.begin(); is_in_rect ==
false && it != boxes_.end(); ++it) {
91 Vector AB = d_vec(it->a, it->b);
92 Vector AM = d_vec(it->a, point);
93 Vector BC = d_vec(it->b, it->c);
94 Vector BM = d_vec(it->b, point);
95 double dotABAM = dot(AB, AM);
96 double dotABAB = dot(AB, AB);
97 double dotBCBM = dot(BC, BM);
98 double dotBCBC = dot(BC, BC);
99 is_in_rect = 0 <= dotABAM && dotABAM <= dotABAB && 0 <= dotBCBM && dotBCBM <= dotBCBC;
104 LaserBoxFilterDataFilter::Vector
105 LaserBoxFilterDataFilter::d_vec(LaserBoxFilterDataFilter::Vector p1,
106 LaserBoxFilterDataFilter::Vector p2)
108 LaserBoxFilterDataFilter::Vector ret_val;
109 ret_val.x = (p2.x - p1.x);
110 ret_val.y = (p2.y - p1.y);
115 LaserBoxFilterDataFilter::dot(LaserBoxFilterDataFilter::Vector u,
116 LaserBoxFilterDataFilter::Vector v)
118 return u.x * v.x + u.y * v.y;
129 new_box.a.x = msg->
p1(0);
130 new_box.a.y = msg->
p1(1);
131 new_box.b.x = msg->
p2(0);
132 new_box.b.y = msg->
p2(1);
133 new_box.c.x = msg->
p3(0);
134 new_box.c.y = msg->
p3(1);
135 new_box.d.x = msg->
p4(0);
136 new_box.d.y = msg->
p4(1);
138 boxes_.push_back(new_box);
140 box_filter_if_->
write();
146 const unsigned int vecsize =
in.size();
150 for (
unsigned int a = 0; a < vecsize; ++a) {
166 "Can't transform laser-data (%s -> %s)",
168 in[a]->frame.c_str());
173 out[a]->frame =
in[a]->frame;
174 out[a]->timestamp =
in[a]->timestamp;
179 if (std::isfinite(
in[a]->values[i])) {
188 p.setValue(x, y, 0.);
191 add = !point_in_rectangle(p.getX(), p.getY());
194 out[a]->values[i] =
in[a]->values[i];
196 out[a]->values[i] = std::numeric_limits<float>::quiet_NaN();