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