Fawkes API  Fawkes Development Version
bulb.cpp
1 
2 /***************************************************************************
3  * bulb.cpp - implements class that defines a light bulb as mirror
4  *
5  * Created: Wed Jul 27 16:19:00 2005
6  * Copyright 2005-2007 Tim Niemueller [www.niemueller.de]
7  * 2005 Martin Heracles
8  *
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version. A runtime exception applies to
15  * this software (see LICENSE.GPL_WRE file mentioned below for details).
16  *
17  * This program is distributed in the hope that it will be useful,
18  * but WITHOUT ANY WARRANTY; without even the implied warranty of
19  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20  * GNU Library General Public License for more details.
21  *
22  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
23  */
24 
25 #include <core/exception.h>
26 
27 #include <fvmodels/mirror/bulb.h>
28 #include <utils/system/console_colors.h>
29 #include <fvutils/ipc/shm_lut.h>
30 
31 
32 #include <cmath>
33 #include <string>
34 #include <cstring>
35 #include <cstdio>
36 #include <cerrno>
37 #include <cstdlib>
38 #include <iostream>
39 #include <sys/utsname.h>
40 
41 using namespace std;
42 using namespace fawkes;
43 
44 namespace firevision {
45 #if 0 /* just to make Emacs auto-indent happy */
46 }
47 #endif
48 
49 /** @class Bulb <fvmodels/mirror/bulb.h>
50  * Bulb mirror lookup table.
51  * This mirror model is based on a LUT that will map image pixels to radial
52  * coordinates in relative radial coordinates.
53  * @author Tim Niemueller
54  * @author Martin Heracles
55  */
56 
57 
58 /** Constructor.
59  * Load bulb LUT from file.
60  * @param filename filename of bulb file to load.
61  */
62 Bulb::Bulb(const char *filename)
63 {
64  init();
65  load(filename);
66 }
67 
68 
69 /** Constructor.
70  * Load bulb LUT from file and expose LUT via shared memory.
71  * @param filename filename of bulb file to load.
72  * @param lut_id LUT ID
73  * @param destroy_on_delete destroy LUT on delete
74  * @see SharedMemoryLookupTable
75  */
76 Bulb::Bulb(const char *filename,
77  const char *lut_id, bool destroy_on_delete)
78 {
79  init();
80 
81  this->lut_id = strdup(lut_id);
82  this->destroy_on_delete = destroy_on_delete;
83 
84  create();
85  load(filename);
86 }
87 
88 
89 /** Constructor.
90  * Create new empty bulb LUT and expose LUT via shared memory.
91  * @param width width of LUT
92  * @param height height of LUT
93  * @param lut_id LUT ID
94  * @param destroy_on_delete destroy LUT on delete
95  * @see SharedMemoryLookupTable
96  */
97 Bulb::Bulb(unsigned int width, unsigned int height,
98  const char *lut_id, bool destroy_on_delete)
99 {
100  init();
101 
102  this->width = width;
103  this->height = height;
104  this->lut_id = strdup(lut_id);
105  this->destroy_on_delete = destroy_on_delete;
106 
107  valid = ((width > 0) && (height > 0));
108 
109  image_center_x = width / 2;
110  image_center_y = height / 2;
111 
112  create();
113 }
114 
115 
116 /** Constructor.
117  * Create new empty bulb LUT.
118  * @param width width of LUT
119  * @param height height of LUT
120  */
121 Bulb::Bulb(unsigned int width, unsigned int height)
122 {
123  init();
124 
125  this->width = width;
126  this->height = height;
127  this->lut_id = NULL;
128 
129  valid = ((width > 0) && (height > 0));
130 
131  image_center_x = width / 2;
132  image_center_y = height / 2;
133 
134  create();
135 }
136 
137 
138 /** Copy constructor.
139  * @param bulb bulb LUT to copy
140  */
141 Bulb::Bulb(const Bulb &bulb)
142 {
143  init();
144 
145  this->valid = bulb.valid;
146 
147  this->width = bulb.width;
148  this->height = bulb.height;
149 
150  this->image_center_x = bulb.image_center_x;
151  this->image_center_y = bulb.image_center_y;
152 
153  this->orientation = bulb.orientation;
154 
155  this->distance_min = bulb.distance_min;
156  this->distance_max = bulb.distance_max;
157 
158  create();
159 
160  memcpy(lut, bulb.lut, lut_bytes);
161 }
162 
163 
164 /** Initializer. */
165 void
166 Bulb::init()
167 {
168  valid = false;
169  width = 0;
170  height = 0;
171  lut_id = NULL;
172  image_center_x = 0;
173  image_center_y = 0;
174 
175  // by default, set orientation to 0 rad
176  orientation = 0.0;
177 
178  // set to the opposite, for later comparison
179  distance_min = 999999.0;
180  distance_max = 0.0;
181 
182  image_center_x = width / 2;
183  image_center_y = height / 2;
184 
185  shm_lut = 0;
186  lut = NULL;
187  lut_bytes = 0;
188 
189 }
190 
191 
192 /** Destructor.
193  * Erases LUT memory. */
194 Bulb::~Bulb()
195 {
196  erase();
197  if ( lut_id != NULL ) {
198  free(lut_id);
199  }
200 }
201 
202 
203 /** Create memory for LUT.
204  * This creates the memory segment for the LUT. If a valid LUT ID
205  * is set the LUT is exposed via shared memory. Otherwise the LUT is
206  * created on the heap.
207  */
208 void
209 Bulb::create()
210 {
211  bytes_per_sample = sizeof(polar_coord_2d_t);
212 
213  if ( lut_id != NULL ) {
214  shm_lut = new SharedMemoryLookupTable( lut_id,
215  width, height, /* depth */ 1,
216  bytes_per_sample);
217  shm_lut->set_destroy_on_delete( destroy_on_delete );
218  lut = (polar_coord_2d_t *)shm_lut->buffer();
219  lut_bytes = shm_lut->data_size();
220  } else {
221  lut_bytes = width * height * bytes_per_sample;
222  lut = (polar_coord_2d_t *)malloc( lut_bytes );
223  }
224  memset(lut, 0, lut_bytes);
225 }
226 
227 
228 /** Erase LUT memory. */
229 void
230 Bulb::erase()
231 {
232  if ( lut_id != NULL ) {
233  delete shm_lut;
234  shm_lut = NULL;
235  lut = NULL;
236  lut_bytes = 0;
237  } else {
238  if (lut != NULL) {
239  free(lut);
240  }
241  lut = NULL;
242  lut_bytes = 0;
243  }
244 }
245 
246 
247 /** Load LUT from file.
248  * @param filename name of LUT file
249  */
250 void
251 Bulb::load(const char *filename)
252 {
253  FILE *f = fopen(filename, "r");
254  if (f == NULL) {
255  throw Exception("Cannot open bulb file");
256  }
257 
259  if ( (fread(&h, sizeof(h), 1, f) == 0) && (! feof(f)) && (ferror(f) != 0)) {
260  throw Exception("Bulb file header invalid");
261  }
262 
263  width = h.width;
264  height = h.height;
265  image_center_x = h.center_x;
266  image_center_y = h.center_y;
267  orientation = h.orientation;
268  distance_min = h.dist_min;
269  distance_max = h.dist_max;
270 
271  erase();
272  create();
273 
274  if ( (fread(lut, lut_bytes, 1, f) == 0) && (! feof(f)) && (ferror(f) != 0)) {
275  erase();
276  throw Exception("Could not read bulb data from file");
277  }
278 
279  fclose(f);
280 }
281 
282 
283 /** Save LUT from file.
284  * @param filename name of LUT file
285  */
286 void
287 Bulb::save(const char *filename)
288 {
289  if (! valid) {
290  throw Exception("Bulb is not valid");
291  }
292 
293  FILE *f = fopen(filename, "w");
294 
295  if (f == NULL) {
296  throw Exception("Could not open bulb file for writing");
297  }
298 
300 
301  h.width = width;
302  h.height = height;
303  h.center_x = image_center_x;
304  h.center_y = image_center_y;
305  h.orientation = orientation;
306  h.dist_min = distance_min;
307  h.dist_max = distance_max;
308 
309  if ( fwrite(&h, sizeof(h), 1, f) == 0 ) {
310  throw Exception("Cannot write bulb file header");
311  }
312 
313 
314  if ( fwrite(lut, lut_bytes, 1, f) == 0 ) {
315  throw Exception("Cannot write bulb file data");
316  }
317 
318  fclose(f);
319 }
320 
321 
322 void
323 Bulb::warp2unwarp(unsigned int warp_x, unsigned int warp_y,
324  unsigned int *unwarp_x, unsigned int *unwarp_y) {
325  /*
326  // check if image pixel (warp_x, warp_y) maps to something
327  if ( this->lut->isNonZero(warp_x, warp_y) ) {
328  // get corresponding world point (polar coordinates)
329  polar_coord_2d_t worldPoint = this->lut->getWorldPointRelative(warp_x, warp_y);
330 
331  // convert to cartesian coordinates
332  *unwarp_x = (unsigned int) ( worldPoint.r * cos(worldPoint.phi) );
333  *unwarp_y = (unsigned int) ( worldPoint.r * sin(worldPoint.phi) );
334  }
335  */
336 }
337 
338 
339 void
340 Bulb::unwarp2warp(unsigned int unwarp_x, unsigned int unwarp_y,
341  unsigned int *warp_x, unsigned int *warp_y )
342 {
343 
344 }
345 
346 
347 const char *
348 Bulb::getName() {
349  return "Mirrormodel::Bulb";
350 }
351 
352 
353 /** Check if a valid LUT has been loaded.
354  * @return true if a valid LUT has been loaded and can be used, false otherwise
355  */
356 bool
357 Bulb::isValid()
358 {
359  return valid;
360 }
361 
362 
364 Bulb::getWorldPointRelative(unsigned int image_x,
365  unsigned int image_y) const
366 {
367  if ( (image_x > width) || (image_y > height) ) {
368  polar_coord_2d_t rv;
369  rv.r = rv.phi = 0;
370  return rv;
371  } else {
372  // will be tuned
373  polar_coord_2d_t rv;
374  rv.r = lut[image_y * width + image_x].r;
375  rv.phi = lut[image_y * width + image_x].phi;
376  return rv;
377 
378  }
379 }
380 
381 
383 Bulb::getWorldPointGlobal(unsigned int image_x,
384  unsigned int image_y,
385  float pose_x,
386  float pose_y,
387  float pose_ori ) const
388 {
389 
390  cart_coord_2d_t rv;
391  rv.x = 0;
392  rv.y = 0;
393 
394  if (image_x > width) return rv;
395  if (image_y > height) return rv;
396 
397 
398  // get relative world point (polar coordinates)
399  polar_coord_2d_t pointRelative;
400  pointRelative = getWorldPointRelative( image_x, image_y );
401 
402  // convert relative angle "pointRelative.phi" to global angle "globalPhi"
403  // (depends on "robOri")
404  float globalPhi;
405  if ( pose_ori >= 0.0 &&
406  pointRelative.phi >= 0.0 &&
407  pointRelative.phi + pose_ori > M_PI ) {
408  globalPhi = -( 2*M_PI - (pointRelative.phi + pose_ori) );
409  } else if ( pose_ori < 0.0 &&
410  pointRelative.phi < 0.0 &&
411  pointRelative.phi + pose_ori < -M_PI ) {
412  globalPhi = 2*M_PI - fabs( pointRelative.phi + pose_ori );
413  } else {
414  globalPhi = pointRelative.phi + pose_ori;
415  }
416 
417  // convert relative world point to global world point
418  // (using global angle "globalPhi" instead of relative angle "pointRelative.phi")
419  rv.x = pointRelative.r * cos( globalPhi ) + pose_x;
420  rv.y = pointRelative.r * sin( globalPhi ) + pose_y;
421 
422  return rv;
423 }
424 
425 
426 /** Get the raw lookup table.
427  * Returns a pointer to the raw lookup table buffer ordered in row-major
428  * mappings from pixels to polar coordinates.
429  * @return raw lookup table
430  */
432 Bulb::get_lut() const
433 {
434  return lut;
435 }
436 
437 
438 /** Set a world point mapping.
439  * This modifies the mapping in the LUT. An exception is thrown if the coordinates
440  * are out of range or the distance is zero.
441  * @param image_x x coordinate of point in image in pixels
442  * @param image_y y coordinate of point in image in pixels
443  * @param world_r distance to real object from camera center in meters
444  * @param world_phi angle to real object
445  */
446 void
447 Bulb::setWorldPoint(unsigned int image_x,
448  unsigned int image_y,
449  float world_r,
450  float world_phi)
451 {
452  if (image_x > width) {
453  throw Exception("MirrorModel::Bulb::setWorldPoint(): image_x out of bounds");
454  }
455  if (image_y > height) {
456  throw Exception("MirrorModel::Bulb::setWorldPoint(): image_x out of bounds");
457  }
458  if (world_r == 0.f) {
459  throw Exception("MirrorModel::Bulb::setWorldPoint(): radius cannot be zero");
460  }
461 
462  // set world point
463  lut[image_y * width + image_x].r = world_r;
464  lut[image_y * width + image_x].phi = world_phi; //convertAngleI2W( world_phi );
465 
466  // update distances
467  float dist_new = getDistanceInImage( image_x, image_y,
468  image_center_x, image_center_y );
469  if (dist_new > distance_max) {
470  distance_max = dist_new;
471  }
472  if (dist_new < distance_min) {
473  distance_min = dist_new;
474  }
475 }
476 
477 
478 void
479 Bulb::reset()
480 {
481  memset(lut, 0, lut_bytes);
482 }
483 
484 
485 upoint_t
486 Bulb::getCenter() const
487 {
488  upoint_t center;
489 
490  center.x = image_center_x;
491  center.y = image_center_y;
492 
493  return center;
494 }
495 
496 
497 void
498 Bulb::setCenter(unsigned int image_x,
499  unsigned int image_y )
500 {
501  if (image_x > width) {
502  throw Exception("MirrorModel::Bulb::setCenter(): image_x out of bounds");
503  }
504  if (image_y > height) {
505  throw Exception("MirrorModel::Bulb::setCenter(): image_y out of bounds");
506  }
507 
508  image_center_x = image_x;
509  image_center_y = image_y;
510 
511  // caution: the distance_min and distance_max values are not correct afterwards!
512 }
513 
514 
515 void
516 Bulb::setOrientation(float angle)
517 {
518  if (angle >= -M_PI &&
519  angle <= M_PI ) {
520  // angle is valid
521  orientation = angle;
522  } else {
523  // angle not valid
524  throw Exception("MirrorModel::Bulb::setOrientation(): angle is invalid");
525  }
526 }
527 
528 
529 float
530 Bulb::getOrientation() const
531 {
532  return orientation;
533 }
534 
535 
536 bool
537 Bulb::isValidPoint(unsigned int image_x, unsigned int image_y) const
538 {
539  return isNonZero(image_x, image_y);
540 }
541 
542 
543 /** Check if pixel maps to valid world point.
544  * @param image_x x coordinate in image
545  * @param image_y y coordinate in image
546  * @return true, iff image pixel (imagePointX, imagePointY) is not zero
547  * (checks distances "r" only, not the angles "phi") i.e. if it maps to a
548  * real-world position
549  */
550 bool
551 Bulb::isNonZero(unsigned int image_x,
552  unsigned int image_y ) const
553 {
554  if (image_x > width) return false;
555  if (image_y > height) return false;
556 
557  return (lut[image_y * width + image_x].r != 0.0);
558 }
559 
560 
561 /** Get number of non-zero entries.
562  * @return number of non-zero entries.
563  */
564 unsigned int
565 Bulb::numNonZero() const
566 {
567  unsigned int num_nonzero = 0;
568  for (unsigned int h = 0; h < height; ++h) {
569  for (unsigned int w = 0; w < width; ++w) {
570  if ( lut[h * width + w].r != 0.0 ) {
571  ++num_nonzero;
572  }
573  }
574  }
575 
576  return num_nonzero;
577 }
578 
579 /** Angle between direction to point and "to the right".
580  * @param image_x x coordinate in image
581  * @param image_y y coordinate in image
582  * @return angle between direction "to point (px, py)" and direction "to the right",
583  * with respect to center point. (Angle is in radians; clockwise is positive,
584  * counter-clockwise is negative.)
585  */
586 float
587 Bulb::getAngle(unsigned int image_x,
588  unsigned int image_y ) const
589 {
590  return atan2f((float(image_y) - float(image_center_y)),
591  (float(image_x) - float(image_center_x)));
592 }
593 
594 
595 /** Euklidean distance between to image points.
596  * @return the (euklidian) distance between two image points
597  * @param image_p1_x x coordinate in image of point 1
598  * @param image_p1_y y coordinate in image of point 1
599  * @param image_p2_x x coordinate in image of point 2
600  * @param image_p2_y y coordinate in image of point 2
601  */
602 float
603 Bulb::getDistanceInImage(unsigned int image_p1_x, unsigned int image_p1_y,
604  unsigned int image_p2_x, unsigned int image_p2_y )
605 {
606  float diffX = float(image_p1_x) - float(image_p2_x);
607  float diffY = float(image_p1_y) - float(image_p2_y);
608 
609  return sqrt( diffX * diffX +
610  diffY * diffY );
611 }
612 
613 
614 /** convertAngleI2W
615  * @return If you have a (ball-) direction in the omni-image,
616  * at which direction is the ball in the world,
617  * relative to the robot?
618  * @param angle_in_image angle to be converted
619  */
620 float
621 Bulb::convertAngleI2W (float angle_in_image) const
622 {
623  // get rid of impact of "orientation" on angle_in_image
624  if (angle_in_image - orientation >= -M_PI &&
625  angle_in_image - orientation <= M_PI ) {
626  angle_in_image = angle_in_image - orientation;
627  }
628  else if (angle_in_image - orientation > M_PI) {
629  angle_in_image = -( M_PI - ((angle_in_image - orientation) - M_PI) );
630  }
631  else { // "angle_in_image - orientation < -M_PI"
632  angle_in_image = M_PI - ( (-(angle_in_image - orientation)) - M_PI );
633  }
634 
635  // turn around by PI
636  // (this is taking the angle that points to the opposite direction)
637  if (angle_in_image + M_PI >= -M_PI &&
638  angle_in_image + M_PI <= M_PI ) {
639  angle_in_image = angle_in_image + M_PI;
640  }
641  else if (angle_in_image + M_PI > M_PI) {
642  angle_in_image = -( M_PI - angle_in_image );
643  }
644  else { // "angle_in_image + M_PI < -M_PI"
645  angle_in_image = M_PI - ( (-(angle_in_image + M_PI)) - M_PI );
646  }
647 
648  // convert without taking into consideration "orientation"
649  // (flipping at vertical axis)
650  if (angle_in_image >= 0.0 &&
651  angle_in_image <= M_PI ) {
652  angle_in_image = (-angle_in_image + M_PI);
653  } else if (angle_in_image >= -M_PI &&
654  angle_in_image <= 0.0 ) {
655  angle_in_image = (-angle_in_image - M_PI);
656  } else if (angle_in_image > M_PI) {
657  // Clip
658  angle_in_image = M_PI;
659  } else if (angle_in_image < -M_PI) {
660  // Clip
661  angle_in_image = -M_PI;
662  } else { // should not occurr
663  cout << "Bulb::convertAngleI2W: ERROR! An invalid angle occurred (angle="
664  << angle_in_image << ")." << endl;
665  return 0.0;
666  }
667 
668  return angle_in_image;
669 }
670 
671 
672 /** Compose a filename matching the given format.
673  * In the format %h is replaced by the hostname.
674  * @param format format of file name
675  * @return filename based on the given format
676  */
677 string
678 Bulb::composeFilename(const char *format)
679 {
680  string rv = format;
681 
682  struct utsname uname_info;
683  uname( &uname_info );
684 
685  size_t loc = rv.find( "%h" );
686  if (loc != string::npos) {
687  rv.replace( loc, 2, uname_info.nodename );
688  }
689 
690  return rv;
691 }
692 
693 } // end namespace firevision
Cartesian coordinates (2D).
Definition: types.h:59
unsigned int center_y
y coordinate of mirror center in image
Definition: bulb.h:120
Fawkes library namespace.
unsigned int y
y coordinate
Definition: types.h:36
STL namespace.
unsigned int x
x coordinate
Definition: types.h:35
float dist_min
minimum distance from mirror center
Definition: bulb.h:122
Polar coordinates.
Definition: types.h:85
Base class for exceptions in Fawkes.
Definition: exception.h:36
Bulb mirror lookup table.
Definition: bulb.h:38
Point with cartesian coordinates as unsigned integers.
Definition: types.h:34
float y
y coordinate
Definition: types.h:61
float dist_max
maximum distance from mirror center
Definition: bulb.h:123
float r
distance
Definition: types.h:86
unsigned int width
width of LUT
Definition: bulb.h:117
float phi
angle
Definition: types.h:87
float orientation
orientation of camera in image
Definition: bulb.h:121
unsigned int center_x
x coordinate of mirror center in image
Definition: bulb.h:119
Shared memory lookup table.
Definition: shm_lut.h:113
unsigned int height
height of LUT
Definition: bulb.h:118
float x
x coordinate
Definition: types.h:60