Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
sensproc_thread.cpp
1 
2 /***************************************************************************
3  * sensproc_thread.cpp - Laser HT sensor processing thread
4  *
5  * Created: Sat Jul 04 21:35:37 2009 (RoboCup 2009, Graz)
6  * Copyright 2006-2009 Tim Niemueller [www.niemueller.de]
7  *
8  * $Id: sensor_thread.cpp 2627 2009-06-25 18:08:09Z tim $
9  *
10  ****************************************************************************/
11 
12 /* This program is free software; you can redistribute it and/or modify
13  * it under the terms of the GNU General Public License as published by
14  * the Free Software Foundation; either version 2 of the License, or
15  * (at your option) any later version.
16  *
17  * This program is distributed in the hope that it will be useful,
18  * but WITHOUT ANY WARRANTY; without even the implied warranty of
19  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20  * GNU Library General Public License for more details.
21  *
22  * Read the full text in the LICENSE.GPL file in the doc directory.
23  */
24 
25 #include "sensproc_thread.h"
26 #include "hough_transform.h"
27 
28 #include <interfaces/Laser360Interface.h>
29 #include <interfaces/ObjectPositionInterface.h>
30 #include <interfaces/VisualDisplay2DInterface.h>
31 
32 #include <utils/math/angle.h>
33 #include <utils/math/coord.h>
34 #ifdef LASERHT_TIMETRACKER
35 # include <utils/time/tracker.h>
36 #endif
37 
38 #include <cstdlib>
39 
40 using namespace fawkes;
41 
42 /** @class LaserHtSensorProcThread "sensproc_thread.h"
43  * Laser Hough Transform sensor processing thread.
44  * This thread integrates into the Fawkes main loop at the sensor processing
45  * hook and uses the Hough Transform to extract shapes.
46  * @author Tim Niemueller
47  */
48 
49 
50 /** Constructor. */
52  : Thread("LaserHtSensorProcThread", Thread::OPMODE_WAITFORWAKEUP),
53  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
54 {
55 }
56 
57 
58 void
60 {
61  __laser360_if = NULL;
62  __visdisp_if = NULL;
63  __line_if = NULL;
64 
65  __cfg_num_samples = config->get_uint("/plugins/laserht/line/num_samples");
66  __cfg_r_scale = config->get_float("/plugins/laserht/line/r_scale");
67  __cfg_laser_ifid = config->get_string("/plugins/laserht/laser_interface_id");
68  __cfg_enable_disp = config->get_bool("/plugins/laserht/line/enable_display");
69  __cfg_vote_threshold = config->get_uint("/plugins/laserht/line/vote_threshold");
70  __cfg_dist_threshold = config->get_float("/plugins/laserht/line/dist_threshold");
71  __cfg_fitting_error_threshold = config->get_float("/plugins/laserht/line/fitting_error_threshold");
72 
73  __laser360_if = NULL;
74  __line_if = NULL;
75  try {
76  __laser360_if = blackboard->open_for_reading<Laser360Interface>(__cfg_laser_ifid.c_str());
77  if (__cfg_enable_disp) {
78  __visdisp_if = blackboard->open_for_reading<VisualDisplay2DInterface>("LaserGUI");
79  }
80  __line_if = blackboard->open_for_writing<ObjectPositionInterface>("LaserLine");
81  __line_if->set_object_type(ObjectPositionInterface::TYPE_LINE);
82  } catch (Exception &e) {
83  blackboard->close(__laser360_if);
84  blackboard->close(__line_if);
85  throw;
86  }
87 
88  __ht = new HoughTransform(2);
89 
90  __num_vals = __cfg_num_samples;
91  __angle_step = 180.f / __num_vals;
92  __r_scale = __cfg_r_scale;
93  __values = new int*[__num_vals];
94  for (unsigned int i = 0; i < __num_vals; ++i) {
95  __values[i] = new int[2];
96  }
97 
98 #ifdef LASERHT_TIMETRACKER
99  __tt = new TimeTracker();
100  __tt_loop = 0;
101  __ttc_reset = __tt->add_class("Reset");
102  __ttc_process = __tt->add_class("Processing");
103  __ttc_fitting = __tt->add_class("Fitting");
104  __ttc_total = __tt->add_class("Total");
105 #endif
106 }
107 
108 
109 void
111 {
112  __line_if->set_valid(false);
113  __line_if->write();
114 
115  blackboard->close(__laser360_if);
116  blackboard->close(__visdisp_if);
117  blackboard->close(__line_if);
118 
119  delete __ht;
120  for (unsigned int i = 0; i < __num_vals; ++i) {
121  delete[] __values[i];
122  }
123  delete[] __values;
124 }
125 
126 
127 void
128 LaserHtSensorProcThread::line_points_from_params(float r, float phi,
129  float &x1, float &y1,
130  float &x2, float &y2)
131 {
132  float phi_rad = deg2rad(phi);
133  float phi_mod = phi - (floorf(phi / 90.) * 90);
134  float r_scaled = r * __r_scale;
135  float tx, ty;
136  polar2cart2d(phi_rad, r_scaled, &tx, &ty);
137  x1 = tx;
138  y1 = ty;
139 
140  float alpha, y_factor = 1;
141  if ( ((phi >= 0) && (phi < 90)) ||
142  (phi >= 270) ) {
143  y_factor = -1;
144  alpha = deg2rad(90 - phi_mod);
145  } else {
146  alpha = deg2rad(phi_mod);
147  }
148  float dx = 1 * cos(alpha);
149  float dy = 1 * y_factor * sin(alpha);
150  x2 = x1 + dx;
151  y2 = y1 + dy;
152 }
153 
154 
155 void
157 {
158  __laser360_if->read();
159  float *distances = __laser360_if->distances();
160  const size_t num_dist = __laser360_if->maxlenof_distances();
161 
162 #ifdef LASERHT_TIMETRACKER
163  __tt->ping_start(__ttc_total);
164  __tt->ping_start(__ttc_reset);
165 #endif
166  __ht->reset();
167 #ifdef LASERHT_TIMETRACKER
168  __tt->ping_end(__ttc_reset);
169  __tt->ping_start(__ttc_process);
170 #endif
171 
172  for (size_t i = 0; i < num_dist; ++i) {
173  // generate candidates
174  if (distances[i] > 0) {
175  for (unsigned int j = 0; j < __num_vals; ++j) {
176  float phi = deg2rad(i);
177  float theta = deg2rad(j * __angle_step);
178  float x, y;
179  polar2cart2d(phi, distances[i], &x, &y);
180  float r = x * cos(theta) + y * sin(theta);
181  r /= __r_scale;
182  __values[j][0] = (int)roundf(r);
183  __values[j][1] = (int)roundf(j * __angle_step);
184  }
185  __ht->process(__values, __num_vals);
186  }
187  }
188 #ifdef LASERHT_TIMETRACKER
189  __tt->ping_end(__ttc_process);
190 #endif
191 
192  int max_values[2];
193  unsigned int max_count = __ht->max(max_values);
194 
195  if (max_count >= __cfg_vote_threshold) {
196  float x1, y1, x2, y2;
197  line_points_from_params(max_values[0], max_values[1], x1, y1, x2, y2);
198 
199  try {
200  if (__cfg_enable_disp && __visdisp_if->has_writer()) {
202  float x[2] = {x1, x2};
203  float y[2] = {y1, y2};
204  unsigned char color[4] = {0, 255, 0, 255};
207  VisualDisplay2DInterface::LS_SOLID, color);
208  __visdisp_if->msgq_enqueue(lm);
209 
210  /*
211  color[0] = 0;
212  color[1] = 255;
213 
214  int *values;
215  unsigned int num_v = __ht->filter(&values, __cfg_vote_threshold);
216  for (unsigned int i = 0; i < num_v; ++i) {
217  line_points_from_params(values[i * 2 + 0], values[i * 2 + 1], x1, y1, x2, y2);
218  float x[2] = {x1, x2};
219  float y[2] = {y1, y2};
220  lm = new VisualDisplay2DInterface::AddCartLineMessage(x, y,
221  VisualDisplay2DInterface::LS_SOLID, color);
222  __visdisp_if->msgq_enqueue(lm);
223  }
224  free(values);
225  */
226  }
227  } catch (Exception &e) {} // ignored
228 
229  // Calculate points contributing to the primary line
230  float theta = deg2rad(max_values[1]);
231  float alpha = 0.5 * M_PI - theta;
232  float r_scaled = max_values[0] * __r_scale;
233  float cos_alpha = cos(alpha);
234  float sin_alpha = sin(alpha);
235  float threshold = __cfg_dist_threshold;
236  float r_min = r_scaled - threshold;
237  float r_max = r_scaled + threshold;
238 
239  bool first_x_minmax = true;
240  float x_min = 0, x_max = 0;
241 
242  std::vector<laser_reading_t> readings;
243 
244  for (size_t i = 0; i < num_dist; ++i) {
245  // calculate r with r(theta) = x_i * cos(theta) + y_i * sin(theta)
246  if (distances[i] > 0) {
247  float x, y;
248  float phi = deg2rad(i);
249  polar2cart2d(phi, distances[i], &x, &y);
250  float r = x * cos(theta) + y * sin(theta);
251 
252  if ( (r >= r_min) && (r <= r_max) ) {
253  // valid!
254  /* generally too much, might be useful for debugging
255  if (__cfg_enable_disp && __visdisp_if->has_writer()) {
256  float xp[2] = {0, x};
257  float yp[2] = {0, y};
258  unsigned char color[4] = {0, 0, 255, 255};
259  VisualDisplay2DInterface::AddCartLineMessage *lm;
260  lm = new VisualDisplay2DInterface::AddCartLineMessage(xp, yp,
261  VisualDisplay2DInterface::LS_SOLID, color);
262  __visdisp_if->msgq_enqueue(lm);
263  }
264  */
265 
266  // now rotate all values to get a horizontal line, otherwise
267  // line fitting could fail (for vertical lines)
268 
269  // note: x_rot = x * cos_alpha - y * sin_alpha
270  // y_rot = x * sin_alpha + y * cos_alpha
271 
272  // Therefore, to rotate line to horizontal position, i.e. by alpha
273  float x_rot = x * cos_alpha - y * sin_alpha;
274  float y_rot = x * sin_alpha + y * cos_alpha;
275 
276  laser_reading_t l = {phi, distances[i], x_rot, y_rot};
277  readings.push_back(l);
278  if (first_x_minmax) {
279  first_x_minmax = false;
280  x_min = x_rot;
281  x_max = x_rot;
282  } else {
283  if (x_rot < x_min) x_min = x_rot;
284  if (x_rot > x_max) x_max = x_rot;
285  }
286  }
287  }
288  }
289 
290 #ifdef LASERHT_TIMETRACKER
291  __tt->ping_start(__ttc_fitting);
292 #endif
293  // fit line through determined readings
294  float a = 0, b = 0, e = 0;
295  fit_line(readings, 0, a, b, e);
296 #ifdef LASERHT_TIMETRACKER
297  __tt->ping_end(__ttc_fitting);
298 #endif
299 
300  if ( e <= __cfg_fitting_error_threshold ) {
301  // calculate y values of min and max point
302  float y_min = a * x_min + b;
303  float y_max = a * x_max + b;
304 
305  // rotate min/max points back, remember rotation from above,
306  // and note: sin(-alpha) = - sin(alpha)
307  // cos(-alpha) = cos(alpha)
308  float x_min_rot = x_min * cos_alpha + y_min * sin_alpha;
309  float y_min_rot = y_min * cos_alpha - x_min * sin_alpha;
310  float x_max_rot = x_max * cos_alpha + y_max * sin_alpha;
311  float y_max_rot = y_max * cos_alpha - x_max * sin_alpha;
312 
313  // calculate parameters of fitted line
314  float alpha_fit = atan2f(y_max_rot - y_min_rot,
315  x_max_rot - x_min_rot);
316  if ( (theta <= 0.5 * M_PI) ||
317  ((theta >= M_PI) && (theta <= 1.5 * M_PI)) ) {
318  alpha_fit = 0.5 * M_PI + alpha_fit;
319  }
320  float theta_fit = floorf(theta / (0.5*M_PI)) * 0.5*M_PI + alpha_fit;
321  float r_fit = x_min_rot * cos(theta_fit) + y_min_rot * sin(theta_fit);
322 
323  if (__cfg_enable_disp && __visdisp_if->has_writer()) {
324  float x1, y1, x2, y2;
325  line_points_from_params(r_fit / __r_scale, rad2deg(theta_fit),
326  x1, y1, x2, y2);
327 
328  try {
330  float x[2] = {x1, x2};
331  float y[2] = {y1, y2};
332  unsigned char color[4] = {0, 0, 255, 255};
335  VisualDisplay2DInterface::LS_SOLID, color);
336  __visdisp_if->msgq_enqueue(lm);
337  } catch (Exception &e) {} // ignored
338  }
339 
340 
341  // write data to interface
342  __line_if->set_world_x(x_min_rot);
343  __line_if->set_world_y(y_min_rot);
344 
345  __line_if->set_relative_x(x_max_rot);
346  __line_if->set_relative_y(y_max_rot);
347 
348  __line_if->set_bearing(theta_fit);
349  __line_if->set_distance(r_fit);
350 
351  __line_if->set_roll(e);
352  __line_if->set_visible(true);
353  } else {
354  logger->log_debug(name(), "Fitting error above threshold: %f > %f",
355  e, __cfg_fitting_error_threshold);
356  __line_if->set_roll(e);
357  __line_if->set_visible(false);
358  }
359  } else {
360  logger->log_debug(name(), "Votes below threshold: %u < %u",
361  max_count, __cfg_vote_threshold);
362  __line_if->set_visible(false);
363  }
364 
365  __line_if->set_valid(true);
366  __line_if->write();
367 #ifdef LASERHT_TIMETRACKER
368  __tt->ping_end(__ttc_total);
369  if (++__tt_loop >= 100) {
370  __tt->print_to_stdout();
371  __tt_loop = 0;
372  }
373 #endif
374 }
375 
376 #define sqr(x) ((x) * (x))
377 
378 void
379 LaserHtSensorProcThread::fit_line(const std::vector<laser_reading_t> &points,
380  const unsigned int first_index,
381  float &a, float &b, float &least_square_error)
382 {
383  const size_t n = points.size();
384  float sum_x = 0.0, sum_xy = 0.0, sum_y = 0.0, sum_xx = 0.0;
385 
386  float x, y;
387  register float e = 0.0;
388  for (size_t i = first_index; i < n; ++i ) {
389  x = points[i].x;
390  y = points[i].y;
391 
392  sum_y += y;
393  sum_x += x;
394  sum_xy += x*y;
395  sum_xx += x*x;
396 
397  }
398 
399  b = ( sum_y * sum_xx - sum_x * sum_xy ) / ( n * sum_xx - sum_x * sum_x );
400  a = ( n * sum_xy - sum_x * sum_y ) / ( n * sum_xx - sum_x * sum_x );
401 
402 
403  for (size_t i = first_index; i < n; ++i ) {
404  // Compute least-square error if desired
405  e += sqr( points[i].y - (points[i].x*a + b) );
406  }
407 
408  least_square_error = e;
409 }