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>
69Bulb::Bulb(
const char *filename,
const char *lut_id,
bool destroy_on_delete)
73 this->lut_id = strdup(lut_id);
74 this->destroy_on_delete = destroy_on_delete;
88Bulb::Bulb(
unsigned int width,
unsigned int height,
const char *lut_id,
bool destroy_on_delete)
93 this->height = height;
94 this->lut_id = strdup(lut_id);
95 this->destroy_on_delete = destroy_on_delete;
97 valid = ((width > 0) && (height > 0));
99 image_center_x = width / 2;
100 image_center_y = height / 2;
115 this->height = height;
118 valid = ((width > 0) && (height > 0));
120 image_center_x = width / 2;
121 image_center_y = height / 2;
133 this->valid = bulb.valid;
135 this->width = bulb.width;
136 this->height = bulb.height;
138 this->image_center_x = bulb.image_center_x;
139 this->image_center_y = bulb.image_center_y;
141 this->orientation = bulb.orientation;
143 this->distance_min = bulb.distance_min;
144 this->distance_max = bulb.distance_max;
148 memcpy(lut, bulb.lut, lut_bytes);
164 height = bulb.height;
165 bytes_per_sample = bulb.bytes_per_sample;
167 image_center_x = bulb.image_center_x;
168 image_center_y = bulb.image_center_y;
170 orientation = bulb.orientation;
172 distance_min = bulb.distance_min;
173 distance_max = bulb.distance_max;
177 memcpy(lut, bulb.lut, lut_bytes);
197 distance_min = 999999.0;
200 image_center_x = width / 2;
201 image_center_y = height / 2;
213 if (lut_id != NULL) {
228 if (lut_id != NULL) {
234 lut_bytes = width * height * bytes_per_sample;
237 memset(lut, 0, lut_bytes);
244 if (lut_id != NULL) {
264 FILE *f = fopen(filename,
"r");
266 throw Exception(
"Cannot open bulb file");
270 if ((fread(&h,
sizeof(h), 1, f) == 0) && (!feof(f)) && (ferror(f) != 0)) {
271 throw Exception(
"Bulb file header invalid");
285 if ((fread(lut, lut_bytes, 1, f) == 0) && (!feof(f)) && (ferror(f) != 0)) {
287 throw Exception(
"Could not read bulb data from file");
303 FILE *f = fopen(filename,
"w");
306 throw Exception(
"Could not open bulb file for writing");
319 if (fwrite(&h,
sizeof(h), 1, f) == 0) {
320 throw Exception(
"Cannot write bulb file header");
323 if (fwrite(lut, lut_bytes, 1, f) == 0) {
324 throw Exception(
"Cannot write bulb file data");
333 unsigned int *unwarp_x,
334 unsigned int *unwarp_y)
351 unsigned int unwarp_y,
352 unsigned int *warp_x,
353 unsigned int *warp_y)
360 return "Mirrormodel::Bulb";
375 if ((image_x > width) || (image_y > height)) {
382 rv.
r = lut[image_y * width + image_x].
r;
383 rv.
phi = lut[image_y * width + image_x].
phi;
390 unsigned int image_y,
393 float pose_ori)
const
401 if (image_y > height)
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);
416 globalPhi = pointRelative.
phi + pose_ori;
421 rv.
x = pointRelative.
r * cos(globalPhi) + pose_x;
422 rv.
y = pointRelative.
r * sin(globalPhi) + pose_y;
449 if (image_x > width) {
450 throw Exception(
"MirrorModel::Bulb::setWorldPoint(): image_x out of bounds");
452 if (image_y > height) {
453 throw Exception(
"MirrorModel::Bulb::setWorldPoint(): image_x out of bounds");
455 if (world_r == 0.f) {
456 throw Exception(
"MirrorModel::Bulb::setWorldPoint(): radius cannot be zero");
460 lut[image_y * width + image_x].
r = world_r;
461 lut[image_y * width + image_x].
phi = world_phi;
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;
468 if (dist_new < distance_min) {
469 distance_min = dist_new;
476 memset(lut, 0, lut_bytes);
484 center.
x = image_center_x;
485 center.
y = image_center_y;
493 if (image_x > width) {
494 throw Exception(
"MirrorModel::Bulb::setCenter(): image_x out of bounds");
496 if (image_y > height) {
497 throw Exception(
"MirrorModel::Bulb::setCenter(): image_y out of bounds");
500 image_center_x = image_x;
501 image_center_y = image_y;
509 if (angle >= -M_PI && angle <= M_PI) {
514 throw Exception(
"MirrorModel::Bulb::setOrientation(): angle is invalid");
542 if (image_y > height)
545 return (lut[image_y * width + image_x].r != 0.0);
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) {
576 return atan2f((
float(image_y) -
float(image_center_y)), (
float(image_x) -
float(image_center_x)));
588 unsigned int image_p1_y,
589 unsigned int image_p2_x,
590 unsigned int image_p2_y)
592 float diffX = float(image_p1_x) - float(image_p2_x);
593 float diffY = float(image_p1_y) - float(image_p2_y);
595 return sqrt(diffX * diffX + diffY * diffY);
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));
613 angle_in_image = M_PI - ((-(angle_in_image - orientation)) - M_PI);
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);
623 angle_in_image = M_PI - ((-(angle_in_image + M_PI)) - M_PI);
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) {
634 angle_in_image = M_PI;
635 }
else if (angle_in_image < -M_PI) {
637 angle_in_image = -M_PI;
639 cout <<
"Bulb::convertAngleI2W: ERROR! An invalid angle occurred (angle=" << angle_in_image
644 return angle_in_image;
657 struct utsname uname_info;
660 size_t loc = rv.find(
"%h");
661 if (loc != string::npos) {
662 rv.replace(loc, 2, uname_info.nodename);
Base class for exceptions in Fawkes.
size_t data_size() const
Get the size of the data-segment.
void set_destroy_on_delete(bool destroy)
Set deletion behaviour.
Bulb mirror lookup table.
Bulb & operator=(const Bulb &bulb)
Assignment operator.
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.
virtual bool isValidPoint(unsigned int image_x, unsigned int image_y) const
Check if the given point is valid.
virtual float getOrientation() const
Get orientation of the omni-camera.
static std::string composeFilename(const char *format)
Compose a filename matching the given format.
unsigned int numNonZero() const
Get number of non-zero entries.
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.
void load(const char *filename)
Load LUT from file.
virtual fawkes::polar_coord_2d_t getWorldPointRelative(unsigned int image_x, unsigned int image_y) const
Get relative coordinate based on image coordinates.
virtual void setWorldPoint(unsigned int image_x, unsigned int image_y, float world_r, float world_phi)
Set a world point mapping.
Bulb(const char *filename)
Constructor.
float getAngle(unsigned int image_x, unsigned int image_y) const
Angle between direction to point and "to the right".
virtual fawkes::upoint_t getCenter() const
Get the image pixel that is the center of the omni-camera.
virtual ~Bulb()
Destructor.
void save(const char *filename)
Save LUT from file.
virtual void setOrientation(float angle)
Set orientation of the omni-camera device.
float convertAngleI2W(float angle_in_image) const
convertAngleI2W
virtual void setCenter(unsigned int image_x, unsigned int image_y)
Set center of omni-camera to given image pixel.
const fawkes::polar_coord_2d_t * get_lut() const
Get the raw lookup table.
virtual void unwarp2warp(unsigned int unwarp_x, unsigned int unwarp_y, unsigned int *warp_x, unsigned int *warp_y)
Transform unwarped to warped point.
virtual void reset()
Reset model.
virtual void warp2unwarp(unsigned int warp_x, unsigned int warp_y, unsigned int *unwarp_x, unsigned int *unwarp_y)
Transform warped to unwarped point.
virtual const char * getName()
Get name of model.
bool isNonZero(unsigned int image_x, unsigned int image_y) const
Check if pixel maps to valid world point.
virtual bool isValid()
Check if a valid LUT has been loaded.
Shared memory lookup table.
unsigned char * buffer() const
Get LUT buffer.
Fawkes library namespace.
Cartesian coordinates (2D).
Point with cartesian coordinates as unsigned integers.
unsigned int x
x coordinate
unsigned int y
y coordinate