00001
00002 /***************************************************************************
00003  *  bulb.cpp - implements class that defines a light bulb as mirror
00004  *
00005  *  Created: Wed Jul 27 16:19:00 2005
00006  *  Copyright  2005-2007 Tim Niemueller [www.niemueller.de]
00007  *             2005      Martin Heracles
00008  *
00009  ****************************************************************************/
00010
00011 /*  This program is free software; you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation; either version 2 of the License, or
00014  *  (at your option) any later version. A runtime exception applies to
00015  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
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_WRE file in the doc directory.
00023  */
00024
00025 #include <core/exception.h>
00026
00027 #include <models/mirror/bulb.h>
00028 #include <utils/system/console_colors.h>
00029 #include <fvutils/ipc/shm_lut.h>
00030
00031
00032 #include <cmath>
00033 #include <string>
00034 #include <cstring>
00035 #include <cstdio>
00036 #include <cerrno>
00037 #include <cstdlib>
00038 #include <iostream>
00039 #include <sys/utsname.h>
00040
00041 using namespace std;
00042 using namespace fawkes;
00043 
00044 /** @class Bulb <models/mirror/bulb.h>
00045  * Bulb mirror lookup table.
00046  * This mirror model is based on a LUT that will map image pixels to radial
00047  * coordinates in relative radial coordinates.
00048  * @author Tim Niemueller
00049  * @author Martin Heracles
00050  */
00051
00052 
00053 /** Constructor.
00054  * Load bulb LUT from file.
00055  * @param filename filename of bulb file to load.
00056  */
00057 Bulb::Bulb(const char *filename)
00058 {
00059   init();
00060   load(filename);
00061 }
00062
00063 
00064 /** Constructor.
00065  * Load bulb LUT from file and expose LUT via shared memory.
00066  * @param filename filename of bulb file to load.
00067  * @param lut_id LUT ID
00068  * @param destroy_on_delete destroy LUT on delete
00069  * @see SharedMemoryLookupTable
00070  */
00071 Bulb::Bulb(const char *filename,
00072            const char *lut_id, bool destroy_on_delete)
00073 {
00074   init();
00075
00076   this->lut_id            = strdup(lut_id);
00077   this->destroy_on_delete = destroy_on_delete;
00078
00079   create();
00080   load(filename);
00081 }
00082
00083 
00084 /** Constructor.
00085  * Create new empty bulb LUT and expose LUT via shared memory.
00086  * @param width width of LUT
00087  * @param height height of LUT
00088  * @param lut_id LUT ID
00089  * @param destroy_on_delete destroy LUT on delete
00090  * @see SharedMemoryLookupTable
00091  */
00092 Bulb::Bulb(unsigned int width, unsigned int height,
00093            const char *lut_id, bool destroy_on_delete)
00094 {
00095   init();
00096
00097   this->width             = width;
00098   this->height            = height;
00099   this->lut_id            = strdup(lut_id);
00100   this->destroy_on_delete = destroy_on_delete;
00101
00102   valid = ((width > 0) && (height > 0));
00103
00104   image_center_x = width / 2;
00105   image_center_y = height / 2;
00106
00107   create();
00108 }
00109
00110 
00111 /** Constructor.
00112  * Create new empty bulb LUT.
00113  * @param width width of LUT
00114  * @param height height of LUT
00115  */
00116 Bulb::Bulb(unsigned int width, unsigned int height)
00117 {
00118   init();
00119
00120   this->width  = width;
00121   this->height = height;
00122   this->lut_id = NULL;
00123
00124   valid = ((width > 0) && (height > 0));
00125
00126   image_center_x = width / 2;
00127   image_center_y = height / 2;
00128
00129   create();
00130 }
00131
00132 
00133 /** Copy constructor.
00134  * @param bulb bulb LUT to copy
00135  */
00136 Bulb::Bulb(const Bulb &bulb)
00137 {
00138   init();
00139
00140   this->valid = bulb.valid;
00141
00142   this->width = bulb.width;
00143   this->height = bulb.height;
00144
00145   this->image_center_x = bulb.image_center_x;
00146   this->image_center_y = bulb.image_center_y;
00147
00148   this->orientation = bulb.orientation;
00149
00150   this->distance_min = distance_min;
00151   this->distance_max = distance_max;
00152
00153   create();
00154
00155   memcpy(lut, bulb.lut, lut_bytes);
00156 }
00157
00158 
00159 /** Initializer. */
00160 void
00161 Bulb::init()
00162 {
00163   valid = false;
00164   width = 0;
00165   height = 0;
00166   lut_id = NULL;
00167   image_center_x = 0;
00168   image_center_y = 0;
00169
00170   // by default, set orientation to 0 rad
00171   orientation = 0.0;
00172
00173   // set to the opposite, for later comparison
00174   distance_min = 999999.0;
00175   distance_max = 0.0;
00176
00177   image_center_x = width / 2;
00178   image_center_y = height / 2;
00179
00180   shm_lut = 0;
00181   lut = NULL;
00182   lut_bytes = 0;
00183
00184 }
00185
00186 
00187 /** Destructor.
00188  * Erases LUT memory. */
00189 Bulb::~Bulb()
00190 {
00191   erase();
00192   if ( lut_id != NULL ) {
00193     free(lut_id);
00194   }
00195 }
00196
00197 
00198 /** Create memory for LUT.
00199  * This creates the memory segment for the LUT. If a valid LUT ID
00200  * is set the LUT is exposed via shared memory. Otherwise the LUT is
00201  * created on the heap.
00202  */
00203 void
00204 Bulb::create()
00205 {
00206   bytes_per_sample = sizeof(polar_coord_2d_t);
00207
00208   if ( lut_id != NULL ) {
00209     shm_lut   = new SharedMemoryLookupTable( lut_id,
00210                                              width, height, /* depth */ 1,
00211                                              bytes_per_sample);
00212     shm_lut->set_destroy_on_delete( destroy_on_delete );
00213     lut       = (polar_coord_2d_t *)shm_lut->buffer();
00214     lut_bytes = shm_lut->data_size();
00215   } else {
00216     lut_bytes = width * height * bytes_per_sample;
00217     lut = (polar_coord_2d_t *)malloc( lut_bytes );
00218   }
00219   memset(lut, 0, lut_bytes);
00220 }
00221
00222 
00223 /** Erase LUT memory. */
00224 void
00225 Bulb::erase()
00226 {
00227   if ( lut_id != NULL ) {
00228     delete shm_lut;
00229     shm_lut = NULL;
00230     lut = NULL;
00231     lut_bytes = 0;
00232   } else {
00233     if (lut != NULL) {
00234       free(lut);
00235     }
00236     lut = NULL;
00237     lut_bytes = 0;
00238   }
00239 }
00240
00241 
00242 /** Load LUT from file.
00243  * @param filename name of LUT file
00244  */
00245 void
00246 Bulb::load(const char *filename)
00247 {
00248   FILE *f = fopen(filename, "r");
00249   if (f == NULL) {
00250     throw Exception("Cannot open bulb file");
00251   }
00252
00253   bulb_file_header_t h;
00254   if ( (fread(&h, sizeof(h), 1, f) == 0) && (! feof(f)) && (ferror(f) != 0)) {
00255     throw Exception("Bulb file header invalid");
00256   }
00257
00258   width          = h.width;
00259   height         = h.height;
00260   image_center_x = h.center_x;
00261   image_center_y = h.center_y;
00262   orientation    = h.orientation;
00263   distance_min   = h.dist_min;
00264   distance_max   = h.dist_max;
00265
00266   erase();
00267   create();
00268
00269   if ( (fread(lut, lut_bytes, 1, f) == 0) && (! feof(f)) && (ferror(f) != 0)) {
00270     erase();
00271     throw Exception("Could not read bulb data from file");
00272   }
00273
00274   fclose(f);
00275 }
00276
00277 
00278 /** Save LUT from file.
00279  * @param filename name of LUT file
00280  */
00281 void
00282 Bulb::save(const char *filename)
00283 {
00284   if (! valid) {
00285     throw Exception("Bulb is not valid");
00286   }
00287
00288   FILE *f = fopen(filename, "w");
00289
00290   if (f == NULL) {
00291     throw Exception("Could not open bulb file for writing");
00292   }
00293
00294   bulb_file_header_t h;
00295
00296   h.width        = width;
00297   h.height       = height;
00298   h.center_x     = image_center_x;
00299   h.center_y     = image_center_y;
00300   h.orientation  = orientation;
00301   h.dist_min     = distance_min;
00302   h.dist_max     = distance_max;
00303
00304   if ( fwrite(&h, sizeof(h), 1, f) == 0 ) {
00305     throw Exception("Cannot write bulb file header");
00306   }
00307
00308
00309   if ( fwrite(lut, lut_bytes, 1, f) == 0 ) {
00310     throw Exception("Cannot write bulb file data");
00311   }
00312
00313   fclose(f);
00314 }
00315
00316
00317 void
00318 Bulb::warp2unwarp(unsigned int warp_x, unsigned int warp_y,
00319                   unsigned int *unwarp_x, unsigned int *unwarp_y) {
00320   /*
00321   // check if image pixel (warp_x, warp_y) maps to something
00322   if ( this->lut->isNonZero(warp_x, warp_y) ) {
00323     // get corresponding world point (polar coordinates)
00324     polar_coord_2d_t worldPoint = this->lut->getWorldPointRelative(warp_x, warp_y);
00325 
00326     // convert to cartesian coordinates
00327     *unwarp_x = (unsigned int) ( worldPoint.r * cos(worldPoint.phi) );
00328     *unwarp_y = (unsigned int) ( worldPoint.r * sin(worldPoint.phi) );
00329   }
00330   */
00331 }
00332
00333
00334 void
00335 Bulb::unwarp2warp(unsigned int unwarp_x, unsigned int unwarp_y,
00336                   unsigned int *warp_x, unsigned int *warp_y    )
00337 {
00338
00339 }
00340
00341
00342 const char *
00343 Bulb::getName() {
00344   return "Mirrormodel::Bulb";
00345 }
00346
00347 
00348 /** Check if a valid LUT has been loaded.
00349  * @return true if a valid LUT has been loaded and can be used, false otherwise
00350  */
00351 bool
00352 Bulb::isValid()
00353 {
00354   return valid;
00355 }
00356
00357
00358 polar_coord_2d_t
00359 Bulb::getWorldPointRelative(unsigned int image_x,
00360                             unsigned int image_y) const
00361 {
00362   if ( (image_x > width) || (image_y > height) ) {
00363     polar_coord_2d_t rv;
00364     rv.r = rv.phi = 0;
00365     return rv;
00366   } else {
00367     // will be tuned
00368     polar_coord_2d_t rv;
00369     rv.r   = lut[image_y * width + image_x].r;
00370     rv.phi = lut[image_y * width + image_x].phi;
00371     return rv;
00372
00373   }
00374 }
00375
00376
00377 cart_coord_2d_t
00378 Bulb::getWorldPointGlobal(unsigned int image_x,
00379                           unsigned int image_y,
00380                           float pose_x,
00381                           float pose_y,
00382                           float pose_ori        ) const
00383 {
00384
00385   cart_coord_2d_t rv;
00386   rv.x = 0;
00387   rv.y = 0;
00388
00389   if (image_x > width) return rv;
00390   if (image_y > height) return rv;
00391
00392
00393   // get relative world point (polar coordinates)
00394   polar_coord_2d_t pointRelative;
00395   pointRelative = getWorldPointRelative( image_x, image_y );
00396
00397   // convert relative angle "pointRelative.phi" to global angle "globalPhi"
00398   // (depends on "robOri")
00399   float globalPhi;
00400   if ( pose_ori                   >= 0.0  &&
00401        pointRelative.phi          >= 0.0  &&
00402        pointRelative.phi + pose_ori >  M_PI    ) {
00403     globalPhi = -( 2*M_PI - (pointRelative.phi + pose_ori) );
00404   } else if ( pose_ori                     < 0.0   &&
00405               pointRelative.phi          < 0.0   &&
00406               pointRelative.phi + pose_ori < -M_PI    ) {
00407     globalPhi = 2*M_PI - fabs( pointRelative.phi + pose_ori );
00408   } else {
00409     globalPhi = pointRelative.phi + pose_ori;
00410   }
00411
00412   // convert relative world point to global world point
00413   // (using global angle "globalPhi" instead of relative angle "pointRelative.phi")
00414   rv.x = pointRelative.r * cos( globalPhi ) + pose_x;
00415   rv.y = pointRelative.r * sin( globalPhi ) + pose_y;
00416
00417   return rv;
00418 }
00419
00420 
00421 /** Set a world point mapping.
00422  * This modifies the mapping in the LUT. An exception is thrown if the coordinates
00423  * are out of range or the distance is zero.
00424  * @param image_x x coordinate of point in image in pixels
00425  * @param image_y y coordinate of point in image in pixels
00426  * @param world_r distance to real object from camera center in meters
00427  * @param world_phi angle to real object
00428  */
00429 void
00430 Bulb::setWorldPoint(unsigned int image_x,
00431                     unsigned int image_y,
00432                     float        world_r,
00433                     float        world_phi)
00434 {
00435   if (image_x > width) {
00436     throw Exception("MirrorModel::Bulb::setWorldPoint(): image_x out of bounds");
00437   }
00438   if (image_y > height) {
00439     throw Exception("MirrorModel::Bulb::setWorldPoint(): image_x out of bounds");
00440   }
00441   if (world_r == 0.f) {
00442     throw Exception("MirrorModel::Bulb::setWorldPoint(): radius cannot be zero");
00443   }
00444
00445   // set world point
00446   lut[image_y * width + image_x].r   = world_r;
00447   lut[image_y * width + image_x].phi = world_phi; //convertAngleI2W( world_phi );
00448
00449   // update distances
00450   float dist_new = getDistanceInImage( image_x, image_y,
00451                                        image_center_x, image_center_y );
00452   if (dist_new > distance_max) {
00453     distance_max = dist_new;
00454   }
00455   if (dist_new < distance_min) {
00456     distance_min = dist_new;
00457   }
00458 }
00459
00460
00461 void
00462 Bulb::reset()
00463 {
00464   memset(lut, 0, lut_bytes);
00465 }
00466
00467
00468 point_t
00469 Bulb::getCenter() const
00470 {
00471   point_t center;
00472
00473   center.x = image_center_x;
00474   center.y = image_center_y;
00475
00476   return center;
00477 }
00478
00479
00480 void
00481 Bulb::setCenter(unsigned int image_x,
00482                 unsigned int image_y  )
00483 {
00484   if (image_x > width) {
00485     throw Exception("MirrorModel::Bulb::setCenter(): image_x out of bounds");
00486   }
00487   if (image_y > height) {
00488     throw Exception("MirrorModel::Bulb::setCenter(): image_y out of bounds");
00489   }
00490
00491   image_center_x = image_x;
00492   image_center_y = image_y;
00493
00494   // caution: the distance_min and distance_max values are not correct afterwards!
00495 }
00496
00497
00498 void
00499 Bulb::setOrientation(float angle)
00500 {
00501   if (angle >= -M_PI &&
00502       angle <=  M_PI    ) {
00503     // angle is valid
00504     orientation = angle;
00505   } else {
00506     // angle not valid
00507     throw Exception("MirrorModel::Bulb::setOrientation(): angle is invalid");
00508   }
00509 }
00510
00511
00512 float
00513 Bulb::getOrientation() const
00514 {
00515   return orientation;
00516 }
00517
00518
00519 bool
00520 Bulb::isValidPoint(unsigned int image_x, unsigned int image_y) const
00521 {
00522   return isNonZero(image_x, image_y);
00523 }
00524
00525 
00526 /** Check if pixel maps to valid world point.
00527  * @param image_x x coordinate in image
00528  * @param image_y y coordinate in image
00529  * @return true, iff image pixel (imagePointX, imagePointY) is not zero
00530  * (checks distances "r" only, not the angles "phi") i.e. if it maps to a
00531  * real-world position
00532  */
00533 bool
00534 Bulb::isNonZero(unsigned int image_x,
00535                 unsigned int image_y  ) const
00536 {
00537   if (image_x > width) return false;
00538   if (image_y > height) return false;
00539
00540   return (lut[image_y * width + image_x].r != 0.0);
00541 }
00542
00543 
00544 /** Get number of non-zero entries.
00545  * @return number of non-zero entries.
00546  */
00547 unsigned int
00548 Bulb::numNonZero() const
00549 {
00550   unsigned int num_nonzero = 0;
00551   for (unsigned int h = 0; h < height; ++h) {
00552     for (unsigned int w = 0; w < width; ++w) {
00553       if ( lut[h * width + w].r != 0.0 ) {
00554         ++num_nonzero;
00555       }
00556     }
00557   }
00558
00559   return num_nonzero;
00560 }
00561 
00562 /** Angle between direction to point and "to the right".
00563  * @param image_x x coordinate in image
00564  * @param image_y y coordinate in image
00565  * @return angle between direction "to point (px, py)" and direction "to the right",
00566  * with respect to center point. (Angle is in radians; clockwise is positive,
00567  * counter-clockwise is negative.)
00568  */
00569 float
00570 Bulb::getAngle(unsigned int image_x,
00571                unsigned int image_y  ) const
00572 {
00573   return atan2f((float(image_y) - float(image_center_y)),
00574                 (float(image_x) - float(image_center_x)));
00575 }
00576
00577 
00578 /** Euklidean distance between to image points.
00579  * @return the (euklidian) distance between two image points
00580  * @param image_p1_x x coordinate in image of point 1
00581  * @param image_p1_y y coordinate in image of point 1
00582  * @param image_p2_x x coordinate in image of point 2
00583  * @param image_p2_y y coordinate in image of point 2
00584  */
00585 float
00586 Bulb::getDistanceInImage(unsigned int image_p1_x, unsigned int image_p1_y,
00587                          unsigned int image_p2_x, unsigned int image_p2_y  )
00588 {
00589   float diffX = float(image_p1_x) - float(image_p2_x);
00590   float diffY = float(image_p1_y) - float(image_p2_y);
00591
00592   return sqrt( diffX * diffX +
00593                diffY * diffY   );
00594 }
00595
00596 
00597 /** convertAngleI2W
00598  * @return If you have a (ball-) direction in the omni-image,
00599  * at which direction is the ball in the world,
00600  * relative to the robot?
00601  * @param angle_in_image angle to be converted
00602  */
00603 float
00604 Bulb::convertAngleI2W (float angle_in_image) const
00605 {
00606   // get rid of impact of "orientation" on angle_in_image
00607   if (angle_in_image - orientation >= -M_PI &&
00608       angle_in_image - orientation <=  M_PI   ) {
00609     angle_in_image = angle_in_image - orientation;
00610   }
00611   else if (angle_in_image - orientation > M_PI) {
00612     angle_in_image = -( M_PI - ((angle_in_image - orientation) - M_PI) );
00613   }
00614   else { // "angle_in_image - orientation < -M_PI"
00615     angle_in_image = M_PI - ( (-(angle_in_image - orientation)) - M_PI );
00616   }
00617
00618   // turn around by PI
00619   // (this is taking the angle that points to the opposite direction)
00620   if (angle_in_image + M_PI >= -M_PI &&
00621       angle_in_image + M_PI <= M_PI    ) {
00622     angle_in_image = angle_in_image + M_PI;
00623   }
00624   else if (angle_in_image + M_PI > M_PI) {
00625     angle_in_image = -( M_PI - angle_in_image );
00626   }
00627   else { // "angle_in_image + M_PI < -M_PI"
00628     angle_in_image = M_PI - ( (-(angle_in_image + M_PI)) - M_PI );
00629   }
00630
00631   // convert without taking into consideration "orientation"
00632   // (flipping at vertical axis)
00633   if (angle_in_image >= 0.0 &&
00634       angle_in_image <= M_PI  ) {
00635     angle_in_image = (-angle_in_image + M_PI);
00636   } else if (angle_in_image >= -M_PI &&
00637              angle_in_image <= 0.0     ) {
00638     angle_in_image = (-angle_in_image - M_PI);
00639   } else if (angle_in_image > M_PI) {
00640     // Clip
00641     angle_in_image = M_PI;
00642   } else if (angle_in_image < -M_PI) {
00643     // Clip
00644     angle_in_image = -M_PI;
00645   } else {                      // should not occurr
00646     cout << "Bulb::convertAngleI2W: ERROR! An invalid angle occurred (angle="
00647          << angle_in_image << ")." << endl;
00648     return 0.0;
00649   }
00650
00651   return angle_in_image;
00652 }
00653
00654 
00655 /** Compose a filename matching the given format.
00656  * In the format %h is replaced by the hostname.
00657  * @param format format of file name
00658  * @return filename based on the given format
00659  */
00660 string
00661 Bulb::composeFilename(const char *format)
00662 {
00663   string rv = format;
00664
00665   struct utsname uname_info;
00666   uname( &uname_info );
00667
00668   size_t loc = rv.find( "%h" );
00669   if (loc != string::npos) {
00670     rv.replace( loc, 2, uname_info.nodename );
00671   }
00672
00673   return rv;
00674 }