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>
42 namespace firevision {
56 Bulb::Bulb(
const char *filename)
69 Bulb::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;
88 Bulb::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;
110 Bulb::Bulb(
unsigned int width,
unsigned int height)
115 this->height = height;
118 valid = ((width > 0) && (height > 0));
120 image_center_x = width / 2;
121 image_center_y = height / 2;
129 Bulb::Bulb(
const Bulb &bulb)
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);
156 Bulb::operator=(
const Bulb &bulb)
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) {
230 shm_lut->set_destroy_on_delete(destroy_on_delete);
232 lut_bytes = shm_lut->data_size();
234 lut_bytes = width * height * bytes_per_sample;
237 memset(lut, 0, lut_bytes);
244 if (lut_id != NULL) {
262 Bulb::load(
const char *filename)
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");
297 Bulb::save(
const char *filename)
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");
331 Bulb::warp2unwarp(
unsigned int warp_x,
333 unsigned int *unwarp_x,
334 unsigned int *unwarp_y)
350 Bulb::unwarp2warp(
unsigned int unwarp_x,
351 unsigned int unwarp_y,
352 unsigned int *warp_x,
353 unsigned int *warp_y)
360 return "Mirrormodel::Bulb";
373 Bulb::getWorldPointRelative(
unsigned int image_x,
unsigned int image_y)
const
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;
389 Bulb::getWorldPointGlobal(
unsigned int image_x,
390 unsigned int image_y,
393 float pose_ori)
const
401 if (image_y > height)
406 pointRelative = getWorldPointRelative(image_x, image_y);
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;
433 Bulb::get_lut()
const
447 Bulb::setWorldPoint(
unsigned int image_x,
unsigned int image_y,
float world_r,
float world_phi)
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);
480 Bulb::getCenter()
const
484 center.
x = image_center_x;
485 center.
y = image_center_y;
491 Bulb::setCenter(
unsigned int image_x,
unsigned int image_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;
507 Bulb::setOrientation(
float angle)
509 if (angle >= -M_PI && angle <= M_PI) {
514 throw Exception(
"MirrorModel::Bulb::setOrientation(): angle is invalid");
519 Bulb::getOrientation()
const
525 Bulb::isValidPoint(
unsigned int image_x,
unsigned int image_y)
const
527 return isNonZero(image_x, image_y);
538 Bulb::isNonZero(
unsigned int image_x,
unsigned int image_y)
const
542 if (image_y > height)
545 return (lut[image_y * width + image_x].r != 0.0);
552 Bulb::numNonZero()
const
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) {
574 Bulb::getAngle(
unsigned int image_x,
unsigned int image_y)
const
576 return atan2f((
float(image_y) -
float(image_center_y)), (
float(image_x) -
float(image_center_x)));
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)
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);
605 Bulb::convertAngleI2W(
float angle_in_image)
const
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;
653 Bulb::composeFilename(
const char *format)
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);