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 <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 namespace firevision {
00045 #if 0 /* just to make Emacs auto-indent happy */
00046 }
00047 #endif
00048 
00049 /** @class Bulb <models/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 /** Set a world point mapping.
00427  * This modifies the mapping in the LUT. An exception is thrown if the coordinates
00428  * are out of range or the distance is zero.
00429  * @param image_x x coordinate of point in image in pixels
00430  * @param image_y y coordinate of point in image in pixels
00431  * @param world_r distance to real object from camera center in meters
00432  * @param world_phi angle to real object
00433  */
00434 void
00435 Bulb::setWorldPoint(unsigned int image_x,
00436                     unsigned int image_y,
00437                     float        world_r,
00438                     float        world_phi)
00439 {
00440   if (image_x > width) {
00441     throw Exception("MirrorModel::Bulb::setWorldPoint(): image_x out of bounds");
00442   }
00443   if (image_y > height) {
00444     throw Exception("MirrorModel::Bulb::setWorldPoint(): image_x out of bounds");
00445   }
00446   if (world_r == 0.f) {
00447     throw Exception("MirrorModel::Bulb::setWorldPoint(): radius cannot be zero");
00448   }
00449 
00450   // set world point
00451   lut[image_y * width + image_x].r   = world_r;
00452   lut[image_y * width + image_x].phi = world_phi; //convertAngleI2W( world_phi );
00453 
00454   // update distances
00455   float dist_new = getDistanceInImage( image_x, image_y,
00456                                        image_center_x, image_center_y );
00457   if (dist_new > distance_max) {
00458     distance_max = dist_new;
00459   }
00460   if (dist_new < distance_min) {
00461     distance_min = dist_new;
00462   }
00463 }
00464 
00465 
00466 void
00467 Bulb::reset()
00468 {
00469   memset(lut, 0, lut_bytes);
00470 }
00471 
00472 
00473 point_t
00474 Bulb::getCenter() const
00475 {
00476   point_t center;
00477 
00478   center.x = image_center_x;
00479   center.y = image_center_y;
00480 
00481   return center;
00482 }
00483 
00484 
00485 void
00486 Bulb::setCenter(unsigned int image_x,
00487                 unsigned int image_y  )
00488 {
00489   if (image_x > width) {
00490     throw Exception("MirrorModel::Bulb::setCenter(): image_x out of bounds");
00491   }
00492   if (image_y > height) {
00493     throw Exception("MirrorModel::Bulb::setCenter(): image_y out of bounds");
00494   }
00495 
00496   image_center_x = image_x;
00497   image_center_y = image_y;
00498 
00499   // caution: the distance_min and distance_max values are not correct afterwards!
00500 }
00501 
00502 
00503 void
00504 Bulb::setOrientation(float angle)
00505 {
00506   if (angle >= -M_PI &&
00507       angle <=  M_PI    ) {
00508     // angle is valid
00509     orientation = angle;
00510   } else {
00511     // angle not valid
00512     throw Exception("MirrorModel::Bulb::setOrientation(): angle is invalid");
00513   }
00514 }
00515 
00516 
00517 float
00518 Bulb::getOrientation() const
00519 {
00520   return orientation;
00521 }
00522 
00523 
00524 bool
00525 Bulb::isValidPoint(unsigned int image_x, unsigned int image_y) const
00526 {
00527   return isNonZero(image_x, image_y);
00528 }
00529 
00530 
00531 /** Check if pixel maps to valid world point.
00532  * @param image_x x coordinate in image
00533  * @param image_y y coordinate in image
00534  * @return true, iff image pixel (imagePointX, imagePointY) is not zero
00535  * (checks distances "r" only, not the angles "phi") i.e. if it maps to a
00536  * real-world position
00537  */
00538 bool
00539 Bulb::isNonZero(unsigned int image_x,
00540                 unsigned int image_y  ) const
00541 {
00542   if (image_x > width) return false;
00543   if (image_y > height) return false;
00544 
00545   return (lut[image_y * width + image_x].r != 0.0);
00546 }
00547 
00548 
00549 /** Get number of non-zero entries.
00550  * @return number of non-zero entries.
00551  */
00552 unsigned int
00553 Bulb::numNonZero() const
00554 {
00555   unsigned int num_nonzero = 0;
00556   for (unsigned int h = 0; h < height; ++h) {
00557     for (unsigned int w = 0; w < width; ++w) {
00558       if ( lut[h * width + w].r != 0.0 ) {
00559         ++num_nonzero;
00560       }
00561     }
00562   }
00563 
00564   return num_nonzero;
00565 }
00566 
00567 /** Angle between direction to point and "to the right".
00568  * @param image_x x coordinate in image
00569  * @param image_y y coordinate in image
00570  * @return angle between direction "to point (px, py)" and direction "to the right",
00571  * with respect to center point. (Angle is in radians; clockwise is positive,
00572  * counter-clockwise is negative.)
00573  */
00574 float
00575 Bulb::getAngle(unsigned int image_x,
00576                unsigned int image_y  ) const
00577 {
00578   return atan2f((float(image_y) - float(image_center_y)),
00579                 (float(image_x) - float(image_center_x)));
00580 }
00581 
00582 
00583 /** Euklidean distance between to image points.
00584  * @return the (euklidian) distance between two image points
00585  * @param image_p1_x x coordinate in image of point 1
00586  * @param image_p1_y y coordinate in image of point 1
00587  * @param image_p2_x x coordinate in image of point 2
00588  * @param image_p2_y y coordinate in image of point 2
00589  */
00590 float
00591 Bulb::getDistanceInImage(unsigned int image_p1_x, unsigned int image_p1_y,
00592                          unsigned int image_p2_x, unsigned int image_p2_y  )
00593 {
00594   float diffX = float(image_p1_x) - float(image_p2_x);
00595   float diffY = float(image_p1_y) - float(image_p2_y);
00596 
00597   return sqrt( diffX * diffX +
00598                diffY * diffY   );
00599 }
00600 
00601 
00602 /** convertAngleI2W
00603  * @return If you have a (ball-) direction in the omni-image,
00604  * at which direction is the ball in the world,
00605  * relative to the robot?
00606  * @param angle_in_image angle to be converted
00607  */
00608 float
00609 Bulb::convertAngleI2W (float angle_in_image) const
00610 {
00611   // get rid of impact of "orientation" on angle_in_image
00612   if (angle_in_image - orientation >= -M_PI &&
00613       angle_in_image - orientation <=  M_PI   ) {
00614     angle_in_image = angle_in_image - orientation;
00615   }
00616   else if (angle_in_image - orientation > M_PI) {
00617     angle_in_image = -( M_PI - ((angle_in_image - orientation) - M_PI) );
00618   }
00619   else { // "angle_in_image - orientation < -M_PI"
00620     angle_in_image = M_PI - ( (-(angle_in_image - orientation)) - M_PI );
00621   }
00622 
00623   // turn around by PI
00624   // (this is taking the angle that points to the opposite direction)
00625   if (angle_in_image + M_PI >= -M_PI &&
00626       angle_in_image + M_PI <= M_PI    ) {
00627     angle_in_image = angle_in_image + M_PI;
00628   }
00629   else if (angle_in_image + M_PI > M_PI) {
00630     angle_in_image = -( M_PI - angle_in_image );
00631   }
00632   else { // "angle_in_image + M_PI < -M_PI"
00633     angle_in_image = M_PI - ( (-(angle_in_image + M_PI)) - M_PI );
00634   }
00635 
00636   // convert without taking into consideration "orientation"
00637   // (flipping at vertical axis)
00638   if (angle_in_image >= 0.0 &&
00639       angle_in_image <= M_PI  ) {
00640     angle_in_image = (-angle_in_image + M_PI);
00641   } else if (angle_in_image >= -M_PI &&
00642              angle_in_image <= 0.0     ) {
00643     angle_in_image = (-angle_in_image - M_PI);
00644   } else if (angle_in_image > M_PI) {
00645     // Clip
00646     angle_in_image = M_PI;
00647   } else if (angle_in_image < -M_PI) {
00648     // Clip
00649     angle_in_image = -M_PI;
00650   } else {                      // should not occurr
00651     cout << "Bulb::convertAngleI2W: ERROR! An invalid angle occurred (angle="
00652          << angle_in_image << ")." << endl;
00653     return 0.0;
00654   }
00655 
00656   return angle_in_image;
00657 }
00658 
00659 
00660 /** Compose a filename matching the given format.
00661  * In the format %h is replaced by the hostname.
00662  * @param format format of file name
00663  * @return filename based on the given format
00664  */
00665 string
00666 Bulb::composeFilename(const char *format)
00667 {
00668   string rv = format;
00669 
00670   struct utsname uname_info;
00671   uname( &uname_info );
00672 
00673   size_t loc = rv.find( "%h" );
00674   if (loc != string::npos) {
00675     rv.replace( loc, 2, uname_info.nodename );
00676   }
00677 
00678   return rv;
00679 }
00680 
00681 } // end namespace firevision

Generated on Tue Feb 22 13:31:17 2011 for Fawkes API by  doxygen 1.4.7