25 #include <core/exception.h> 27 #include <fvmodels/mirror/bulb.h> 28 #include <utils/system/console_colors.h> 29 #include <fvutils/ipc/shm_lut.h> 39 #include <sys/utsname.h> 62 Bulb::Bulb(
const char *filename)
76 Bulb::Bulb(
const char *filename,
77 const char *lut_id,
bool destroy_on_delete)
81 this->lut_id = strdup(lut_id);
82 this->destroy_on_delete = destroy_on_delete;
97 Bulb::Bulb(
unsigned int width,
unsigned int height,
98 const char *lut_id,
bool destroy_on_delete)
103 this->height = height;
104 this->lut_id = strdup(lut_id);
105 this->destroy_on_delete = destroy_on_delete;
107 valid = ((width > 0) && (height > 0));
109 image_center_x = width / 2;
110 image_center_y = height / 2;
121 Bulb::Bulb(
unsigned int width,
unsigned int height)
126 this->height = height;
129 valid = ((width > 0) && (height > 0));
131 image_center_x = width / 2;
132 image_center_y = height / 2;
145 this->valid = bulb.valid;
147 this->width = bulb.width;
148 this->height = bulb.height;
150 this->image_center_x = bulb.image_center_x;
151 this->image_center_y = bulb.image_center_y;
153 this->orientation = bulb.orientation;
155 this->distance_min = bulb.distance_min;
156 this->distance_max = bulb.distance_max;
160 memcpy(lut, bulb.lut, lut_bytes);
179 distance_min = 999999.0;
182 image_center_x = width / 2;
183 image_center_y = height / 2;
197 if ( lut_id != NULL ) {
213 if ( lut_id != NULL ) {
217 shm_lut->set_destroy_on_delete( destroy_on_delete );
219 lut_bytes = shm_lut->data_size();
221 lut_bytes = width * height * bytes_per_sample;
224 memset(lut, 0, lut_bytes);
232 if ( lut_id != NULL ) {
251 Bulb::load(
const char *filename)
253 FILE *f = fopen(filename,
"r");
255 throw Exception(
"Cannot open bulb file");
259 if ( (fread(&h,
sizeof(h), 1, f) == 0) && (! feof(f)) && (ferror(f) != 0)) {
260 throw Exception(
"Bulb file header invalid");
274 if ( (fread(lut, lut_bytes, 1, f) == 0) && (! feof(f)) && (ferror(f) != 0)) {
276 throw Exception(
"Could not read bulb data from file");
287 Bulb::save(
const char *filename)
293 FILE *f = fopen(filename,
"w");
296 throw Exception(
"Could not open bulb file for writing");
309 if ( fwrite(&h,
sizeof(h), 1, f) == 0 ) {
310 throw Exception(
"Cannot write bulb file header");
314 if ( fwrite(lut, lut_bytes, 1, f) == 0 ) {
315 throw Exception(
"Cannot write bulb file data");
323 Bulb::warp2unwarp(
unsigned int warp_x,
unsigned int warp_y,
324 unsigned int *unwarp_x,
unsigned int *unwarp_y) {
340 Bulb::unwarp2warp(
unsigned int unwarp_x,
unsigned int unwarp_y,
341 unsigned int *warp_x,
unsigned int *warp_y )
349 return "Mirrormodel::Bulb";
364 Bulb::getWorldPointRelative(
unsigned int image_x,
365 unsigned int image_y)
const 367 if ( (image_x > width) || (image_y > height) ) {
374 rv.
r = lut[image_y * width + image_x].r;
375 rv.
phi = lut[image_y * width + image_x].phi;
383 Bulb::getWorldPointGlobal(
unsigned int image_x,
384 unsigned int image_y,
387 float pose_ori )
const 394 if (image_x > width)
return rv;
395 if (image_y > height)
return rv;
400 pointRelative = getWorldPointRelative( image_x, image_y );
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 );
414 globalPhi = pointRelative.
phi + pose_ori;
419 rv.
x = pointRelative.
r * cos( globalPhi ) + pose_x;
420 rv.
y = pointRelative.
r * sin( globalPhi ) + pose_y;
432 Bulb::get_lut()
const 447 Bulb::setWorldPoint(
unsigned int image_x,
448 unsigned int image_y,
452 if (image_x > width) {
453 throw Exception(
"MirrorModel::Bulb::setWorldPoint(): image_x out of bounds");
455 if (image_y > height) {
456 throw Exception(
"MirrorModel::Bulb::setWorldPoint(): image_x out of bounds");
458 if (world_r == 0.f) {
459 throw Exception(
"MirrorModel::Bulb::setWorldPoint(): radius cannot be zero");
463 lut[image_y * width + image_x].r = world_r;
464 lut[image_y * width + image_x].phi = world_phi;
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;
472 if (dist_new < distance_min) {
473 distance_min = dist_new;
481 memset(lut, 0, lut_bytes);
486 Bulb::getCenter()
const 490 center.
x = image_center_x;
491 center.
y = image_center_y;
498 Bulb::setCenter(
unsigned int image_x,
499 unsigned int image_y )
501 if (image_x > width) {
502 throw Exception(
"MirrorModel::Bulb::setCenter(): image_x out of bounds");
504 if (image_y > height) {
505 throw Exception(
"MirrorModel::Bulb::setCenter(): image_y out of bounds");
508 image_center_x = image_x;
509 image_center_y = image_y;
516 Bulb::setOrientation(
float angle)
518 if (angle >= -M_PI &&
524 throw Exception(
"MirrorModel::Bulb::setOrientation(): angle is invalid");
530 Bulb::getOrientation()
const 537 Bulb::isValidPoint(
unsigned int image_x,
unsigned int image_y)
const 539 return isNonZero(image_x, image_y);
551 Bulb::isNonZero(
unsigned int image_x,
552 unsigned int image_y )
const 554 if (image_x > width)
return false;
555 if (image_y > height)
return false;
557 return (lut[image_y * width + image_x].r != 0.0);
565 Bulb::numNonZero()
const 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 ) {
587 Bulb::getAngle(
unsigned int image_x,
588 unsigned int image_y )
const 590 return atan2f((
float(image_y) -
float(image_center_y)),
591 (
float(image_x) -
float(image_center_x)));
603 Bulb::getDistanceInImage(
unsigned int image_p1_x,
unsigned int image_p1_y,
604 unsigned int image_p2_x,
unsigned int image_p2_y )
606 float diffX = float(image_p1_x) - float(image_p2_x);
607 float diffY = float(image_p1_y) - float(image_p2_y);
609 return sqrt( diffX * diffX +
621 Bulb::convertAngleI2W (
float angle_in_image)
const 624 if (angle_in_image - orientation >= -M_PI &&
625 angle_in_image - orientation <= M_PI ) {
626 angle_in_image = angle_in_image - orientation;
628 else if (angle_in_image - orientation > M_PI) {
629 angle_in_image = -( M_PI - ((angle_in_image - orientation) - M_PI) );
632 angle_in_image = M_PI - ( (-(angle_in_image - orientation)) - M_PI );
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;
641 else if (angle_in_image + M_PI > M_PI) {
642 angle_in_image = -( M_PI - angle_in_image );
645 angle_in_image = M_PI - ( (-(angle_in_image + M_PI)) - M_PI );
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) {
658 angle_in_image = M_PI;
659 }
else if (angle_in_image < -M_PI) {
661 angle_in_image = -M_PI;
663 cout <<
"Bulb::convertAngleI2W: ERROR! An invalid angle occurred (angle=" 664 << angle_in_image <<
")." << endl;
668 return angle_in_image;
678 Bulb::composeFilename(
const char *format)
682 struct utsname uname_info;
683 uname( &uname_info );
685 size_t loc = rv.find(
"%h" );
686 if (loc != string::npos) {
687 rv.replace( loc, 2, uname_info.nodename );
Cartesian coordinates (2D).
Fawkes library namespace.
unsigned int y
y coordinate
unsigned int x
x coordinate
Base class for exceptions in Fawkes.
Bulb mirror lookup table.
Point with cartesian coordinates as unsigned integers.
Shared memory lookup table.