sensproc_thread.cpp

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 }

Generated on Tue Feb 22 13:32:30 2011 for Fawkes API by  doxygen 1.4.7