Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * sensproc_thread.cpp - Laser HT sensor processing thread 00004 * 00005 * Created: Sat Jul 04 21:35:37 2009 (RoboCup 2009, Graz) 00006 * Copyright 2006-2009 Tim Niemueller [www.niemueller.de] 00007 * 00008 * $Id: sensor_thread.cpp 2627 2009-06-25 18:08:09Z tim $ 00009 * 00010 ****************************************************************************/ 00011 00012 /* This program is free software; you can redistribute it and/or modify 00013 * it under the terms of the GNU General Public License as published by 00014 * the Free Software Foundation; either version 2 of the License, or 00015 * (at your option) any later version. 00016 * 00017 * This program is distributed in the hope that it will be useful, 00018 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00019 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00020 * GNU Library General Public License for more details. 00021 * 00022 * Read the full text in the LICENSE.GPL file in the doc directory. 00023 */ 00024 00025 #include "sensproc_thread.h" 00026 #include "hough_transform.h" 00027 00028 #include <interfaces/Laser360Interface.h> 00029 #include <interfaces/ObjectPositionInterface.h> 00030 #include <interfaces/VisualDisplay2DInterface.h> 00031 00032 #include <utils/math/angle.h> 00033 #include <utils/math/coord.h> 00034 #ifdef LASERHT_TIMETRACKER 00035 # include <utils/time/tracker.h> 00036 #endif 00037 00038 #include <cstdlib> 00039 00040 using namespace fawkes; 00041 00042 /** @class LaserHtSensorProcThread "sensproc_thread.h" 00043 * Laser Hough Transform sensor processing thread. 00044 * This thread integrates into the Fawkes main loop at the sensor processing 00045 * hook and uses the Hough Transform to extract shapes. 00046 * @author Tim Niemueller 00047 */ 00048 00049 00050 /** Constructor. */ 00051 LaserHtSensorProcThread::LaserHtSensorProcThread() 00052 : Thread("LaserHtSensorProcThread", Thread::OPMODE_WAITFORWAKEUP), 00053 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS) 00054 { 00055 } 00056 00057 00058 void 00059 LaserHtSensorProcThread::init() 00060 { 00061 __laser360_if = NULL; 00062 __visdisp_if = NULL; 00063 __line_if = NULL; 00064 00065 __cfg_num_samples = config->get_uint("/plugins/laserht/line/num_samples"); 00066 __cfg_r_scale = config->get_float("/plugins/laserht/line/r_scale"); 00067 __cfg_laser_ifid = config->get_string("/plugins/laserht/laser_interface_id"); 00068 __cfg_enable_disp = config->get_bool("/plugins/laserht/line/enable_display"); 00069 __cfg_vote_threshold = config->get_uint("/plugins/laserht/line/vote_threshold"); 00070 __cfg_dist_threshold = config->get_float("/plugins/laserht/line/dist_threshold"); 00071 __cfg_fitting_error_threshold = config->get_float("/plugins/laserht/line/fitting_error_threshold"); 00072 00073 __laser360_if = NULL; 00074 __line_if = NULL; 00075 try { 00076 __laser360_if = blackboard->open_for_reading<Laser360Interface>(__cfg_laser_ifid.c_str()); 00077 if (__cfg_enable_disp) { 00078 __visdisp_if = blackboard->open_for_reading<VisualDisplay2DInterface>("LaserGUI"); 00079 } 00080 __line_if = blackboard->open_for_writing<ObjectPositionInterface>("LaserLine"); 00081 __line_if->set_object_type(ObjectPositionInterface::TYPE_LINE); 00082 } catch (Exception &e) { 00083 blackboard->close(__laser360_if); 00084 blackboard->close(__line_if); 00085 throw; 00086 } 00087 00088 __ht = new HoughTransform(2); 00089 00090 __num_vals = __cfg_num_samples; 00091 __angle_step = 180.f / __num_vals; 00092 __r_scale = __cfg_r_scale; 00093 __values = new int*[__num_vals]; 00094 for (unsigned int i = 0; i < __num_vals; ++i) { 00095 __values[i] = new int[2]; 00096 } 00097 00098 #ifdef LASERHT_TIMETRACKER 00099 __tt = new TimeTracker(); 00100 __tt_loop = 0; 00101 __ttc_reset = __tt->add_class("Reset"); 00102 __ttc_process = __tt->add_class("Processing"); 00103 __ttc_fitting = __tt->add_class("Fitting"); 00104 __ttc_total = __tt->add_class("Total"); 00105 #endif 00106 } 00107 00108 00109 void 00110 LaserHtSensorProcThread::finalize() 00111 { 00112 __line_if->set_valid(false); 00113 __line_if->write(); 00114 00115 blackboard->close(__laser360_if); 00116 blackboard->close(__visdisp_if); 00117 blackboard->close(__line_if); 00118 00119 delete __ht; 00120 for (unsigned int i = 0; i < __num_vals; ++i) { 00121 delete[] __values[i]; 00122 } 00123 delete[] __values; 00124 } 00125 00126 00127 void 00128 LaserHtSensorProcThread::line_points_from_params(float r, float phi, 00129 float &x1, float &y1, 00130 float &x2, float &y2) 00131 { 00132 float phi_rad = deg2rad(phi); 00133 float phi_mod = phi - (floorf(phi / 90.) * 90); 00134 float r_scaled = r * __r_scale; 00135 float tx, ty; 00136 polar2cart2d(phi_rad, r_scaled, &tx, &ty); 00137 x1 = tx; 00138 y1 = ty; 00139 00140 float alpha, y_factor = 1; 00141 if ( ((phi >= 0) && (phi < 90)) || 00142 (phi >= 270) ) { 00143 y_factor = -1; 00144 alpha = deg2rad(90 - phi_mod); 00145 } else { 00146 alpha = deg2rad(phi_mod); 00147 } 00148 float dx = 1 * cos(alpha); 00149 float dy = 1 * y_factor * sin(alpha); 00150 x2 = x1 + dx; 00151 y2 = y1 + dy; 00152 } 00153 00154 00155 void 00156 LaserHtSensorProcThread::loop() 00157 { 00158 __laser360_if->read(); 00159 float *distances = __laser360_if->distances(); 00160 const size_t num_dist = __laser360_if->maxlenof_distances(); 00161 00162 #ifdef LASERHT_TIMETRACKER 00163 __tt->ping_start(__ttc_total); 00164 __tt->ping_start(__ttc_reset); 00165 #endif 00166 __ht->reset(); 00167 #ifdef LASERHT_TIMETRACKER 00168 __tt->ping_end(__ttc_reset); 00169 __tt->ping_start(__ttc_process); 00170 #endif 00171 00172 for (size_t i = 0; i < num_dist; ++i) { 00173 // generate candidates 00174 if (distances[i] > 0) { 00175 for (unsigned int j = 0; j < __num_vals; ++j) { 00176 float phi = deg2rad(i); 00177 float theta = deg2rad(j * __angle_step); 00178 float x, y; 00179 polar2cart2d(phi, distances[i], &x, &y); 00180 float r = x * cos(theta) + y * sin(theta); 00181 r /= __r_scale; 00182 __values[j][0] = (int)roundf(r); 00183 __values[j][1] = (int)roundf(j * __angle_step); 00184 } 00185 __ht->process(__values, __num_vals); 00186 } 00187 } 00188 #ifdef LASERHT_TIMETRACKER 00189 __tt->ping_end(__ttc_process); 00190 #endif 00191 00192 int max_values[2]; 00193 unsigned int max_count = __ht->max(max_values); 00194 00195 if (max_count >= __cfg_vote_threshold) { 00196 float x1, y1, x2, y2; 00197 line_points_from_params(max_values[0], max_values[1], x1, y1, x2, y2); 00198 00199 try { 00200 if (__cfg_enable_disp && __visdisp_if->has_writer()) { 00201 __visdisp_if->msgq_enqueue(new VisualDisplay2DInterface::DeleteAllMessage()); 00202 float x[2] = {x1, x2}; 00203 float y[2] = {y1, y2}; 00204 unsigned char color[4] = {0, 255, 0, 255}; 00205 VisualDisplay2DInterface::AddCartLineMessage *lm; 00206 lm = new VisualDisplay2DInterface::AddCartLineMessage(x, y, 00207 VisualDisplay2DInterface::LS_SOLID, color); 00208 __visdisp_if->msgq_enqueue(lm); 00209 00210 /* 00211 color[0] = 0; 00212 color[1] = 255; 00213 00214 int *values; 00215 unsigned int num_v = __ht->filter(&values, __cfg_vote_threshold); 00216 for (unsigned int i = 0; i < num_v; ++i) { 00217 line_points_from_params(values[i * 2 + 0], values[i * 2 + 1], x1, y1, x2, y2); 00218 float x[2] = {x1, x2}; 00219 float y[2] = {y1, y2}; 00220 lm = new VisualDisplay2DInterface::AddCartLineMessage(x, y, 00221 VisualDisplay2DInterface::LS_SOLID, color); 00222 __visdisp_if->msgq_enqueue(lm); 00223 } 00224 free(values); 00225 */ 00226 } 00227 } catch (Exception &e) {} // ignored 00228 00229 // Calculate points contributing to the primary line 00230 float theta = deg2rad(max_values[1]); 00231 float alpha = 0.5 * M_PI - theta; 00232 float r_scaled = max_values[0] * __r_scale; 00233 float cos_alpha = cos(alpha); 00234 float sin_alpha = sin(alpha); 00235 float threshold = __cfg_dist_threshold; 00236 float r_min = r_scaled - threshold; 00237 float r_max = r_scaled + threshold; 00238 00239 bool first_x_minmax = true; 00240 float x_min = 0, x_max = 0; 00241 00242 std::vector<laser_reading_t> readings; 00243 00244 for (size_t i = 0; i < num_dist; ++i) { 00245 // calculate r with r(theta) = x_i * cos(theta) + y_i * sin(theta) 00246 if (distances[i] > 0) { 00247 float x, y; 00248 float phi = deg2rad(i); 00249 polar2cart2d(phi, distances[i], &x, &y); 00250 float r = x * cos(theta) + y * sin(theta); 00251 00252 if ( (r >= r_min) && (r <= r_max) ) { 00253 // valid! 00254 /* generally too much, might be useful for debugging 00255 if (__cfg_enable_disp && __visdisp_if->has_writer()) { 00256 float xp[2] = {0, x}; 00257 float yp[2] = {0, y}; 00258 unsigned char color[4] = {0, 0, 255, 255}; 00259 VisualDisplay2DInterface::AddCartLineMessage *lm; 00260 lm = new VisualDisplay2DInterface::AddCartLineMessage(xp, yp, 00261 VisualDisplay2DInterface::LS_SOLID, color); 00262 __visdisp_if->msgq_enqueue(lm); 00263 } 00264 */ 00265 00266 // now rotate all values to get a horizontal line, otherwise 00267 // line fitting could fail (for vertical lines) 00268 00269 // note: x_rot = x * cos_alpha - y * sin_alpha 00270 // y_rot = x * sin_alpha + y * cos_alpha 00271 00272 // Therefore, to rotate line to horizontal position, i.e. by alpha 00273 float x_rot = x * cos_alpha - y * sin_alpha; 00274 float y_rot = x * sin_alpha + y * cos_alpha; 00275 00276 laser_reading_t l = {phi, distances[i], x_rot, y_rot}; 00277 readings.push_back(l); 00278 if (first_x_minmax) { 00279 first_x_minmax = false; 00280 x_min = x_rot; 00281 x_max = x_rot; 00282 } else { 00283 if (x_rot < x_min) x_min = x_rot; 00284 if (x_rot > x_max) x_max = x_rot; 00285 } 00286 } 00287 } 00288 } 00289 00290 #ifdef LASERHT_TIMETRACKER 00291 __tt->ping_start(__ttc_fitting); 00292 #endif 00293 // fit line through determined readings 00294 float a = 0, b = 0, e = 0; 00295 fit_line(readings, 0, a, b, e); 00296 #ifdef LASERHT_TIMETRACKER 00297 __tt->ping_end(__ttc_fitting); 00298 #endif 00299 00300 if ( e <= __cfg_fitting_error_threshold ) { 00301 // calculate y values of min and max point 00302 float y_min = a * x_min + b; 00303 float y_max = a * x_max + b; 00304 00305 // rotate min/max points back, remember rotation from above, 00306 // and note: sin(-alpha) = - sin(alpha) 00307 // cos(-alpha) = cos(alpha) 00308 float x_min_rot = x_min * cos_alpha + y_min * sin_alpha; 00309 float y_min_rot = y_min * cos_alpha - x_min * sin_alpha; 00310 float x_max_rot = x_max * cos_alpha + y_max * sin_alpha; 00311 float y_max_rot = y_max * cos_alpha - x_max * sin_alpha; 00312 00313 // calculate parameters of fitted line 00314 float alpha_fit = atan2f(y_max_rot - y_min_rot, 00315 x_max_rot - x_min_rot); 00316 if ( (theta <= 0.5 * M_PI) || 00317 ((theta >= M_PI) && (theta <= 1.5 * M_PI)) ) { 00318 alpha_fit = 0.5 * M_PI + alpha_fit; 00319 } 00320 float theta_fit = floorf(theta / (0.5*M_PI)) * 0.5*M_PI + alpha_fit; 00321 float r_fit = x_min_rot * cos(theta_fit) + y_min_rot * sin(theta_fit); 00322 00323 if (__cfg_enable_disp && __visdisp_if->has_writer()) { 00324 float x1, y1, x2, y2; 00325 line_points_from_params(r_fit / __r_scale, rad2deg(theta_fit), 00326 x1, y1, x2, y2); 00327 00328 try { 00329 __visdisp_if->msgq_enqueue(new VisualDisplay2DInterface::DeleteAllMessage()); 00330 float x[2] = {x1, x2}; 00331 float y[2] = {y1, y2}; 00332 unsigned char color[4] = {0, 0, 255, 255}; 00333 VisualDisplay2DInterface::AddCartLineMessage *lm; 00334 lm = new VisualDisplay2DInterface::AddCartLineMessage(x, y, 00335 VisualDisplay2DInterface::LS_SOLID, color); 00336 __visdisp_if->msgq_enqueue(lm); 00337 } catch (Exception &e) {} // ignored 00338 } 00339 00340 00341 // write data to interface 00342 __line_if->set_world_x(x_min_rot); 00343 __line_if->set_world_y(y_min_rot); 00344 00345 __line_if->set_relative_x(x_max_rot); 00346 __line_if->set_relative_y(y_max_rot); 00347 00348 __line_if->set_bearing(theta_fit); 00349 __line_if->set_distance(r_fit); 00350 00351 __line_if->set_roll(e); 00352 __line_if->set_visible(true); 00353 } else { 00354 logger->log_debug(name(), "Fitting error above threshold: %f > %f", 00355 e, __cfg_fitting_error_threshold); 00356 __line_if->set_roll(e); 00357 __line_if->set_visible(false); 00358 } 00359 } else { 00360 logger->log_debug(name(), "Votes below threshold: %u < %u", 00361 max_count, __cfg_vote_threshold); 00362 __line_if->set_visible(false); 00363 } 00364 00365 __line_if->set_valid(true); 00366 __line_if->write(); 00367 #ifdef LASERHT_TIMETRACKER 00368 __tt->ping_end(__ttc_total); 00369 if (++__tt_loop >= 100) { 00370 __tt->print_to_stdout(); 00371 __tt_loop = 0; 00372 } 00373 #endif 00374 } 00375 00376 #define sqr(x) ((x) * (x)) 00377 00378 void 00379 LaserHtSensorProcThread::fit_line(const std::vector<laser_reading_t> &points, 00380 const unsigned int first_index, 00381 float &a, float &b, float &least_square_error) 00382 { 00383 const size_t n = points.size(); 00384 float sum_x = 0.0, sum_xy = 0.0, sum_y = 0.0, sum_xx = 0.0; 00385 00386 float x, y; 00387 register float e = 0.0; 00388 for (size_t i = first_index; i < n; ++i ) { 00389 x = points[i].x; 00390 y = points[i].y; 00391 00392 sum_y += y; 00393 sum_x += x; 00394 sum_xy += x*y; 00395 sum_xx += x*x; 00396 00397 } 00398 00399 b = ( sum_y * sum_xx - sum_x * sum_xy ) / ( n * sum_xx - sum_x * sum_x ); 00400 a = ( n * sum_xy - sum_x * sum_y ) / ( n * sum_xx - sum_x * sum_x ); 00401 00402 00403 for (size_t i = first_index; i < n; ++i ) { 00404 // Compute least-square error if desired 00405 e += sqr( points[i].y - (points[i].x*a + b) ); 00406 } 00407 00408 least_square_error = e; 00409 }