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