Fawkes API Fawkes Development Version
|
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