00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include <core/exception.h>
00026
00027 #include <models/mirror/bulb.h>
00028 #include <utils/system/console_colors.h>
00029 #include <fvutils/ipc/shm_lut.h>
00030
00031
00032 #include <cmath>
00033 #include <string>
00034 #include <cstring>
00035 #include <cstdio>
00036 #include <cerrno>
00037 #include <cstdlib>
00038 #include <iostream>
00039 #include <sys/utsname.h>
00040
00041 using namespace std;
00042 using namespace fawkes;
00043
00044 namespace firevision {
00045 #if 0
00046 }
00047 #endif
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062 Bulb::Bulb(const char *filename)
00063 {
00064 init();
00065 load(filename);
00066 }
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076 Bulb::Bulb(const char *filename,
00077 const char *lut_id, bool destroy_on_delete)
00078 {
00079 init();
00080
00081 this->lut_id = strdup(lut_id);
00082 this->destroy_on_delete = destroy_on_delete;
00083
00084 create();
00085 load(filename);
00086 }
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097 Bulb::Bulb(unsigned int width, unsigned int height,
00098 const char *lut_id, bool destroy_on_delete)
00099 {
00100 init();
00101
00102 this->width = width;
00103 this->height = height;
00104 this->lut_id = strdup(lut_id);
00105 this->destroy_on_delete = destroy_on_delete;
00106
00107 valid = ((width > 0) && (height > 0));
00108
00109 image_center_x = width / 2;
00110 image_center_y = height / 2;
00111
00112 create();
00113 }
00114
00115
00116
00117
00118
00119
00120
00121 Bulb::Bulb(unsigned int width, unsigned int height)
00122 {
00123 init();
00124
00125 this->width = width;
00126 this->height = height;
00127 this->lut_id = NULL;
00128
00129 valid = ((width > 0) && (height > 0));
00130
00131 image_center_x = width / 2;
00132 image_center_y = height / 2;
00133
00134 create();
00135 }
00136
00137
00138
00139
00140
00141 Bulb::Bulb(const Bulb &bulb)
00142 {
00143 init();
00144
00145 this->valid = bulb.valid;
00146
00147 this->width = bulb.width;
00148 this->height = bulb.height;
00149
00150 this->image_center_x = bulb.image_center_x;
00151 this->image_center_y = bulb.image_center_y;
00152
00153 this->orientation = bulb.orientation;
00154
00155 this->distance_min = distance_min;
00156 this->distance_max = distance_max;
00157
00158 create();
00159
00160 memcpy(lut, bulb.lut, lut_bytes);
00161 }
00162
00163
00164
00165 void
00166 Bulb::init()
00167 {
00168 valid = false;
00169 width = 0;
00170 height = 0;
00171 lut_id = NULL;
00172 image_center_x = 0;
00173 image_center_y = 0;
00174
00175
00176 orientation = 0.0;
00177
00178
00179 distance_min = 999999.0;
00180 distance_max = 0.0;
00181
00182 image_center_x = width / 2;
00183 image_center_y = height / 2;
00184
00185 shm_lut = 0;
00186 lut = NULL;
00187 lut_bytes = 0;
00188
00189 }
00190
00191
00192
00193
00194 Bulb::~Bulb()
00195 {
00196 erase();
00197 if ( lut_id != NULL ) {
00198 free(lut_id);
00199 }
00200 }
00201
00202
00203
00204
00205
00206
00207
00208 void
00209 Bulb::create()
00210 {
00211 bytes_per_sample = sizeof(polar_coord_2d_t);
00212
00213 if ( lut_id != NULL ) {
00214 shm_lut = new SharedMemoryLookupTable( lut_id,
00215 width, height, 1,
00216 bytes_per_sample);
00217 shm_lut->set_destroy_on_delete( destroy_on_delete );
00218 lut = (polar_coord_2d_t *)shm_lut->buffer();
00219 lut_bytes = shm_lut->data_size();
00220 } else {
00221 lut_bytes = width * height * bytes_per_sample;
00222 lut = (polar_coord_2d_t *)malloc( lut_bytes );
00223 }
00224 memset(lut, 0, lut_bytes);
00225 }
00226
00227
00228
00229 void
00230 Bulb::erase()
00231 {
00232 if ( lut_id != NULL ) {
00233 delete shm_lut;
00234 shm_lut = NULL;
00235 lut = NULL;
00236 lut_bytes = 0;
00237 } else {
00238 if (lut != NULL) {
00239 free(lut);
00240 }
00241 lut = NULL;
00242 lut_bytes = 0;
00243 }
00244 }
00245
00246
00247
00248
00249
00250 void
00251 Bulb::load(const char *filename)
00252 {
00253 FILE *f = fopen(filename, "r");
00254 if (f == NULL) {
00255 throw Exception("Cannot open bulb file");
00256 }
00257
00258 bulb_file_header_t h;
00259 if ( (fread(&h, sizeof(h), 1, f) == 0) && (! feof(f)) && (ferror(f) != 0)) {
00260 throw Exception("Bulb file header invalid");
00261 }
00262
00263 width = h.width;
00264 height = h.height;
00265 image_center_x = h.center_x;
00266 image_center_y = h.center_y;
00267 orientation = h.orientation;
00268 distance_min = h.dist_min;
00269 distance_max = h.dist_max;
00270
00271 erase();
00272 create();
00273
00274 if ( (fread(lut, lut_bytes, 1, f) == 0) && (! feof(f)) && (ferror(f) != 0)) {
00275 erase();
00276 throw Exception("Could not read bulb data from file");
00277 }
00278
00279 fclose(f);
00280 }
00281
00282
00283
00284
00285
00286 void
00287 Bulb::save(const char *filename)
00288 {
00289 if (! valid) {
00290 throw Exception("Bulb is not valid");
00291 }
00292
00293 FILE *f = fopen(filename, "w");
00294
00295 if (f == NULL) {
00296 throw Exception("Could not open bulb file for writing");
00297 }
00298
00299 bulb_file_header_t h;
00300
00301 h.width = width;
00302 h.height = height;
00303 h.center_x = image_center_x;
00304 h.center_y = image_center_y;
00305 h.orientation = orientation;
00306 h.dist_min = distance_min;
00307 h.dist_max = distance_max;
00308
00309 if ( fwrite(&h, sizeof(h), 1, f) == 0 ) {
00310 throw Exception("Cannot write bulb file header");
00311 }
00312
00313
00314 if ( fwrite(lut, lut_bytes, 1, f) == 0 ) {
00315 throw Exception("Cannot write bulb file data");
00316 }
00317
00318 fclose(f);
00319 }
00320
00321
00322 void
00323 Bulb::warp2unwarp(unsigned int warp_x, unsigned int warp_y,
00324 unsigned int *unwarp_x, unsigned int *unwarp_y) {
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336 }
00337
00338
00339 void
00340 Bulb::unwarp2warp(unsigned int unwarp_x, unsigned int unwarp_y,
00341 unsigned int *warp_x, unsigned int *warp_y )
00342 {
00343
00344 }
00345
00346
00347 const char *
00348 Bulb::getName() {
00349 return "Mirrormodel::Bulb";
00350 }
00351
00352
00353
00354
00355
00356 bool
00357 Bulb::isValid()
00358 {
00359 return valid;
00360 }
00361
00362
00363 polar_coord_2d_t
00364 Bulb::getWorldPointRelative(unsigned int image_x,
00365 unsigned int image_y) const
00366 {
00367 if ( (image_x > width) || (image_y > height) ) {
00368 polar_coord_2d_t rv;
00369 rv.r = rv.phi = 0;
00370 return rv;
00371 } else {
00372
00373 polar_coord_2d_t rv;
00374 rv.r = lut[image_y * width + image_x].r;
00375 rv.phi = lut[image_y * width + image_x].phi;
00376 return rv;
00377
00378 }
00379 }
00380
00381
00382 cart_coord_2d_t
00383 Bulb::getWorldPointGlobal(unsigned int image_x,
00384 unsigned int image_y,
00385 float pose_x,
00386 float pose_y,
00387 float pose_ori ) const
00388 {
00389
00390 cart_coord_2d_t rv;
00391 rv.x = 0;
00392 rv.y = 0;
00393
00394 if (image_x > width) return rv;
00395 if (image_y > height) return rv;
00396
00397
00398
00399 polar_coord_2d_t pointRelative;
00400 pointRelative = getWorldPointRelative( image_x, image_y );
00401
00402
00403
00404 float globalPhi;
00405 if ( pose_ori >= 0.0 &&
00406 pointRelative.phi >= 0.0 &&
00407 pointRelative.phi + pose_ori > M_PI ) {
00408 globalPhi = -( 2*M_PI - (pointRelative.phi + pose_ori) );
00409 } else if ( pose_ori < 0.0 &&
00410 pointRelative.phi < 0.0 &&
00411 pointRelative.phi + pose_ori < -M_PI ) {
00412 globalPhi = 2*M_PI - fabs( pointRelative.phi + pose_ori );
00413 } else {
00414 globalPhi = pointRelative.phi + pose_ori;
00415 }
00416
00417
00418
00419 rv.x = pointRelative.r * cos( globalPhi ) + pose_x;
00420 rv.y = pointRelative.r * sin( globalPhi ) + pose_y;
00421
00422 return rv;
00423 }
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434 void
00435 Bulb::setWorldPoint(unsigned int image_x,
00436 unsigned int image_y,
00437 float world_r,
00438 float world_phi)
00439 {
00440 if (image_x > width) {
00441 throw Exception("MirrorModel::Bulb::setWorldPoint(): image_x out of bounds");
00442 }
00443 if (image_y > height) {
00444 throw Exception("MirrorModel::Bulb::setWorldPoint(): image_x out of bounds");
00445 }
00446 if (world_r == 0.f) {
00447 throw Exception("MirrorModel::Bulb::setWorldPoint(): radius cannot be zero");
00448 }
00449
00450
00451 lut[image_y * width + image_x].r = world_r;
00452 lut[image_y * width + image_x].phi = world_phi;
00453
00454
00455 float dist_new = getDistanceInImage( image_x, image_y,
00456 image_center_x, image_center_y );
00457 if (dist_new > distance_max) {
00458 distance_max = dist_new;
00459 }
00460 if (dist_new < distance_min) {
00461 distance_min = dist_new;
00462 }
00463 }
00464
00465
00466 void
00467 Bulb::reset()
00468 {
00469 memset(lut, 0, lut_bytes);
00470 }
00471
00472
00473 point_t
00474 Bulb::getCenter() const
00475 {
00476 point_t center;
00477
00478 center.x = image_center_x;
00479 center.y = image_center_y;
00480
00481 return center;
00482 }
00483
00484
00485 void
00486 Bulb::setCenter(unsigned int image_x,
00487 unsigned int image_y )
00488 {
00489 if (image_x > width) {
00490 throw Exception("MirrorModel::Bulb::setCenter(): image_x out of bounds");
00491 }
00492 if (image_y > height) {
00493 throw Exception("MirrorModel::Bulb::setCenter(): image_y out of bounds");
00494 }
00495
00496 image_center_x = image_x;
00497 image_center_y = image_y;
00498
00499
00500 }
00501
00502
00503 void
00504 Bulb::setOrientation(float angle)
00505 {
00506 if (angle >= -M_PI &&
00507 angle <= M_PI ) {
00508
00509 orientation = angle;
00510 } else {
00511
00512 throw Exception("MirrorModel::Bulb::setOrientation(): angle is invalid");
00513 }
00514 }
00515
00516
00517 float
00518 Bulb::getOrientation() const
00519 {
00520 return orientation;
00521 }
00522
00523
00524 bool
00525 Bulb::isValidPoint(unsigned int image_x, unsigned int image_y) const
00526 {
00527 return isNonZero(image_x, image_y);
00528 }
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538 bool
00539 Bulb::isNonZero(unsigned int image_x,
00540 unsigned int image_y ) const
00541 {
00542 if (image_x > width) return false;
00543 if (image_y > height) return false;
00544
00545 return (lut[image_y * width + image_x].r != 0.0);
00546 }
00547
00548
00549
00550
00551
00552 unsigned int
00553 Bulb::numNonZero() const
00554 {
00555 unsigned int num_nonzero = 0;
00556 for (unsigned int h = 0; h < height; ++h) {
00557 for (unsigned int w = 0; w < width; ++w) {
00558 if ( lut[h * width + w].r != 0.0 ) {
00559 ++num_nonzero;
00560 }
00561 }
00562 }
00563
00564 return num_nonzero;
00565 }
00566
00567
00568
00569
00570
00571
00572
00573
00574 float
00575 Bulb::getAngle(unsigned int image_x,
00576 unsigned int image_y ) const
00577 {
00578 return atan2f((float(image_y) - float(image_center_y)),
00579 (float(image_x) - float(image_center_x)));
00580 }
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590 float
00591 Bulb::getDistanceInImage(unsigned int image_p1_x, unsigned int image_p1_y,
00592 unsigned int image_p2_x, unsigned int image_p2_y )
00593 {
00594 float diffX = float(image_p1_x) - float(image_p2_x);
00595 float diffY = float(image_p1_y) - float(image_p2_y);
00596
00597 return sqrt( diffX * diffX +
00598 diffY * diffY );
00599 }
00600
00601
00602
00603
00604
00605
00606
00607
00608 float
00609 Bulb::convertAngleI2W (float angle_in_image) const
00610 {
00611
00612 if (angle_in_image - orientation >= -M_PI &&
00613 angle_in_image - orientation <= M_PI ) {
00614 angle_in_image = angle_in_image - orientation;
00615 }
00616 else if (angle_in_image - orientation > M_PI) {
00617 angle_in_image = -( M_PI - ((angle_in_image - orientation) - M_PI) );
00618 }
00619 else {
00620 angle_in_image = M_PI - ( (-(angle_in_image - orientation)) - M_PI );
00621 }
00622
00623
00624
00625 if (angle_in_image + M_PI >= -M_PI &&
00626 angle_in_image + M_PI <= M_PI ) {
00627 angle_in_image = angle_in_image + M_PI;
00628 }
00629 else if (angle_in_image + M_PI > M_PI) {
00630 angle_in_image = -( M_PI - angle_in_image );
00631 }
00632 else {
00633 angle_in_image = M_PI - ( (-(angle_in_image + M_PI)) - M_PI );
00634 }
00635
00636
00637
00638 if (angle_in_image >= 0.0 &&
00639 angle_in_image <= M_PI ) {
00640 angle_in_image = (-angle_in_image + M_PI);
00641 } else if (angle_in_image >= -M_PI &&
00642 angle_in_image <= 0.0 ) {
00643 angle_in_image = (-angle_in_image - M_PI);
00644 } else if (angle_in_image > M_PI) {
00645
00646 angle_in_image = M_PI;
00647 } else if (angle_in_image < -M_PI) {
00648
00649 angle_in_image = -M_PI;
00650 } else {
00651 cout << "Bulb::convertAngleI2W: ERROR! An invalid angle occurred (angle="
00652 << angle_in_image << ")." << endl;
00653 return 0.0;
00654 }
00655
00656 return angle_in_image;
00657 }
00658
00659
00660
00661
00662
00663
00664
00665 string
00666 Bulb::composeFilename(const char *format)
00667 {
00668 string rv = format;
00669
00670 struct utsname uname_info;
00671 uname( &uname_info );
00672
00673 size_t loc = rv.find( "%h" );
00674 if (loc != string::npos) {
00675 rv.replace( loc, 2, uname_info.nodename );
00676 }
00677
00678 return rv;
00679 }
00680
00681 }