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
39using namespace std;
40using namespace fawkes;
41
42namespace 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 */
56Bulb::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 */
69Bulb::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 */
88Bulb::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 */
110Bulb::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 */
129Bulb::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 */
155Bulb &
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. */
183void
184Bulb::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. */
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 */
223void
224Bulb::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. */
241void
242Bulb::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 */
261void
262Bulb::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 */
296void
297Bulb::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
330void
331Bulb::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
349void
350Bulb::unwarp2warp(unsigned int unwarp_x,
351 unsigned int unwarp_y,
352 unsigned int *warp_x,
353 unsigned int *warp_y)
354{
355}
356
357const char *
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 */
366bool
368{
369 return valid;
370}
371
373Bulb::getWorldPointRelative(unsigned int image_x, unsigned int image_y) const
374{
375 if ((image_x > width) || (image_y > height)) {
377 rv.r = rv.phi = 0;
378 return rv;
379 } else {
380 // will be tuned
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
389Bulb::getWorldPointGlobal(unsigned int image_x,
390 unsigned int image_y,
391 float pose_x,
392 float pose_y,
393 float pose_ori) const
394{
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 */
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 */
446void
447Bulb::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
473void
475{
476 memset(lut, 0, lut_bytes);
477}
478
481{
482 upoint_t center;
483
484 center.x = image_center_x;
485 center.y = image_center_y;
486
487 return center;
488}
489
490void
491Bulb::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
506void
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
518float
520{
521 return orientation;
522}
523
524bool
525Bulb::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 */
537bool
538Bulb::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 */
551unsigned int
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 */
573float
574Bulb::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 */
586float
587Bulb::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 */
604float
605Bulb::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 */
652string
653Bulb::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
size_t data_size() const
Get the size of the data-segment.
Definition: shm.cpp:745
void set_destroy_on_delete(bool destroy)
Set deletion behaviour.
Definition: shm.cpp:839
Bulb mirror lookup table.
Definition: bulb.h:37
Bulb & operator=(const Bulb &bulb)
Assignment operator.
Definition: bulb.cpp:156
virtual fawkes::cart_coord_2d_t getWorldPointGlobal(unsigned int image_x, unsigned int image_y, float pose_x, float pose_y, float pose_ori) const
Get global coordinate based on image coordinates.
Definition: bulb.cpp:389
virtual bool isValidPoint(unsigned int image_x, unsigned int image_y) const
Check if the given point is valid.
Definition: bulb.cpp:525
virtual float getOrientation() const
Get orientation of the omni-camera.
Definition: bulb.cpp:519
static std::string composeFilename(const char *format)
Compose a filename matching the given format.
Definition: bulb.cpp:653
unsigned int numNonZero() const
Get number of non-zero entries.
Definition: bulb.cpp:552
float getDistanceInImage(unsigned int image_p1_x, unsigned int image_p1_y, unsigned int image_p2_x, unsigned int image_p2_y)
Euklidean distance between to image points.
Definition: bulb.cpp:587
void load(const char *filename)
Load LUT from file.
Definition: bulb.cpp:262
virtual fawkes::polar_coord_2d_t getWorldPointRelative(unsigned int image_x, unsigned int image_y) const
Get relative coordinate based on image coordinates.
Definition: bulb.cpp:373
virtual void setWorldPoint(unsigned int image_x, unsigned int image_y, float world_r, float world_phi)
Set a world point mapping.
Definition: bulb.cpp:447
Bulb(const char *filename)
Constructor.
Definition: bulb.cpp:56
float getAngle(unsigned int image_x, unsigned int image_y) const
Angle between direction to point and "to the right".
Definition: bulb.cpp:574
virtual fawkes::upoint_t getCenter() const
Get the image pixel that is the center of the omni-camera.
Definition: bulb.cpp:480
virtual ~Bulb()
Destructor.
Definition: bulb.cpp:210
void save(const char *filename)
Save LUT from file.
Definition: bulb.cpp:297
virtual void setOrientation(float angle)
Set orientation of the omni-camera device.
Definition: bulb.cpp:507
float convertAngleI2W(float angle_in_image) const
convertAngleI2W
Definition: bulb.cpp:605
virtual void setCenter(unsigned int image_x, unsigned int image_y)
Set center of omni-camera to given image pixel.
Definition: bulb.cpp:491
const fawkes::polar_coord_2d_t * get_lut() const
Get the raw lookup table.
Definition: bulb.cpp:433
virtual void unwarp2warp(unsigned int unwarp_x, unsigned int unwarp_y, unsigned int *warp_x, unsigned int *warp_y)
Transform unwarped to warped point.
Definition: bulb.cpp:350
virtual void reset()
Reset model.
Definition: bulb.cpp:474
virtual void warp2unwarp(unsigned int warp_x, unsigned int warp_y, unsigned int *unwarp_x, unsigned int *unwarp_y)
Transform warped to unwarped point.
Definition: bulb.cpp:331
virtual const char * getName()
Get name of model.
Definition: bulb.cpp:358
bool isNonZero(unsigned int image_x, unsigned int image_y) const
Check if pixel maps to valid world point.
Definition: bulb.cpp:538
virtual bool isValid()
Check if a valid LUT has been loaded.
Definition: bulb.cpp:367
Shared memory lookup table.
Definition: shm_lut.h:113
unsigned char * buffer() const
Get LUT buffer.
Definition: shm_lut.cpp:141
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