00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include <cams/bumblebee2.h>
00025
00026 #include <cams/cam_exceptions.h>
00027 #include <core/exception.h>
00028 #include <fvutils/system/camargp.h>
00029 #include <fvutils/color/conversions.h>
00030
00031
00032 #include <stdlib.h>
00033 #include <unistd.h>
00034 #include <string>
00035 #include <endian.h>
00036
00037 #include <utils/math/angle.h>
00038
00039 #include <cstdio>
00040
00041 #include <dc1394/utils.h>
00042 #include <dc1394/register.h>
00043 #include <dc1394/conversions.h>
00044
00045 using namespace fawkes;
00046
00047 namespace firevision {
00048 #if 0
00049 }
00050 #endif
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084 const unsigned int Bumblebee2Camera::ORIGINAL = 0;
00085
00086
00087 const unsigned int Bumblebee2Camera::DEINTERLACED = 1;
00088
00089
00090 const unsigned int Bumblebee2Camera::RGB_IMAGE = 2;
00091
00092
00093
00094
00095 #define PGR_BAYER_TILE_MAPPING_REGISTER (0x1040)
00096
00097
00098 #define PGR_REG_CONFIG_LENGTH (0x1FFC)
00099
00100
00101 #define PGR_REG_CONFIG_DATA (0x2000)
00102
00103
00104 #define PGR_REG_UNIT_DIRECTORY_OFFSET (0x0424)
00105
00106
00107 #define PGR_REG_IMAGE_DATA_FORMAT (0x1048)
00108
00109 #define PTG_Y16_Data_Format_PGR_specific (0xFFFFFFFE)
00110
00111
00112 #define PGR_REG_SERIAL_NUMBER (0x1F20)
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125 Bumblebee2Camera::Bumblebee2Camera(const CameraArgumentParser *cap)
00126 : FirewireCamera(DC1394_FRAMERATE_30,
00127 DC1394_VIDEO_MODE_FORMAT7_3,
00128 DC1394_ISO_SPEED_400,
00129 8)
00130 {
00131
00132
00133 _model = strdup(cap->cam_id().c_str());
00134
00135 _format7_coding = DC1394_COLOR_CODING_RAW16;
00136 _format7_width = 640;
00137 _format7_height = 480;
00138 _format7_startx = _format7_starty = 0;
00139
00140 if ( cap->has("nbufs") ) {
00141 _num_buffers = atoi(cap->get("nbufs").c_str());
00142 }
00143 if ( cap->has("width") ) {
00144 _format7_width = atoi(cap->get("width").c_str());
00145 }
00146 if ( cap->has("height") ) {
00147 _format7_height = atoi(cap->get("height").c_str());
00148 }
00149 if ( cap->has("startx") ) {
00150 _format7_startx = atoi(cap->get("startx").c_str());
00151 }
00152 if ( cap->has("starty") ) {
00153 _format7_starty = atoi(cap->get("starty").c_str());
00154 }
00155 if ( cap->has("focus") ) {
00156 parse_set_focus(cap->get("focus").c_str());
00157 }
00158 if ( cap->has("white_balance") ) {
00159 parse_set_white_balance(cap->get("white_balance").c_str());
00160 }
00161 if ( cap->has("shutter") ) {
00162 parse_set_shutter(cap->get("shutter").c_str());
00163 }
00164
00165 __buffer_deinterlaced = NULL;
00166 __buffer_rgb = NULL;
00167 }
00168
00169
00170
00171 Bumblebee2Camera::~Bumblebee2Camera()
00172 {
00173 if (__buffer_deinterlaced != NULL) free(__buffer_deinterlaced);
00174 if (__buffer_rgb != NULL) free(__buffer_rgb);
00175 }
00176
00177
00178
00179
00180
00181 uint32_t
00182 Bumblebee2Camera::serial_no() const
00183 {
00184 if ( ! _opened ) throw Exception("Camera not opened");
00185
00186 uint32_t value = 0;
00187 dc1394error_t err = dc1394_get_control_register( _camera, PGR_REG_SERIAL_NUMBER, &value );
00188 if ( err != DC1394_SUCCESS ) {
00189 throw Exception("Bumblebee2::serial_no: dc1394_get_control_register(PGR_REG_SERIAL_NUMBER) failed\n");
00190 }
00191 return value;
00192 }
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205 bool
00206 Bumblebee2Camera::verify_guid(uint64_t ver_guid) const
00207 {
00208 if ( ! _opened ) throw Exception("Camera not opened");
00209
00210 uint64_t tguid = ver_guid;
00211 tguid >>= 32;
00212 tguid &= 0xFFFFFFFF;
00213 if ( tguid == 0xFFFFFFFF ) {
00214
00215 ver_guid &= 0xFFFFFFFF;
00216 return (serial_no() == ver_guid);
00217 } else {
00218 return (guid() == ver_guid);
00219 }
00220 }
00221
00222
00223 void
00224 Bumblebee2Camera::print_info()
00225 {
00226 FirewireCamera::print_info();
00227
00228 printf("Serial: %u\n", serial_no());
00229 #if __WORDSIZE == 64
00230 printf("GUID: 0x%016lx\n", guid());
00231 #else
00232 printf("GUID: 0x%016llx\n", guid());
00233 #endif
00234 }
00235
00236 void
00237 Bumblebee2Camera::open()
00238 {
00239 try {
00240 FirewireCamera::open();
00241 } catch (Exception &e) {
00242 throw;
00243 }
00244
00245 if ( ! _opened ) {
00246 throw Exception("Bumblebee2Camera::open: FirewireCamera::open dit not suceed");
00247 }
00248
00249 __buffer_deinterlaced = (unsigned char *)malloc(pixel_width() * pixel_height() * 2);
00250 __buffer_rgb = malloc_buffer(RGB, pixel_width(), pixel_height() * 2);
00251 __buffer = NULL;
00252
00253 #if __BYTE_ORDER == __LITTLE_ENDIAN
00254 dc1394error_t err;
00255 typedef union {
00256 uint32_t value;
00257 struct {
00258 uint32_t presence : 1;
00259 uint32_t reserved1 : 21;
00260 uint32_t mirror : 1;
00261 uint32_t bayer_mono : 1;
00262 uint32_t reserved2 : 7;
00263 uint32_t data_format: 1;
00264 } idf;
00265 } idf_u;
00266 idf_u value;
00267 err = dc1394_get_control_register( _camera, PGR_REG_IMAGE_DATA_FORMAT, &(value.value) );
00268 if ( err != DC1394_SUCCESS ) {
00269 throw Exception("Bumblebee2::open: dc1394_get_control_register(PGR_REG_DATA_FORMAT) failed\n");
00270 }
00271 value.value &= PTG_Y16_Data_Format_PGR_specific;
00272 value.idf.data_format = 0;
00273 err = dc1394_set_control_register( _camera, PGR_REG_IMAGE_DATA_FORMAT, value.value );
00274 if ( err != DC1394_SUCCESS ) {
00275 throw Exception("Bumblebee2::open: Setting PGR-specific mode on little-endian system failed\n");
00276 }
00277 #endif
00278
00279 get_bayer_tile();
00280 }
00281
00282
00283 void
00284 Bumblebee2Camera::close()
00285 {
00286 if ( _opened ) {
00287 FirewireCamera::close();
00288 if (__buffer_deinterlaced != NULL) {
00289 free(__buffer_deinterlaced);
00290 __buffer_deinterlaced = NULL;
00291 }
00292 if (__buffer_rgb != NULL) {
00293 free(__buffer_rgb);
00294 __buffer_rgb = NULL;
00295 }
00296 }
00297 }
00298
00299 void
00300 Bumblebee2Camera::capture()
00301 {
00302 try {
00303 FirewireCamera::capture();
00304 } catch (CaptureException &e) {
00305 e.append("Bumblebee2Camera::capture: failed to retrieve image");
00306 if ( ORIGINAL == __image_num ) __buffer = NULL;
00307 throw;
00308 }
00309 if ( ORIGINAL == __image_num ) {
00310 __buffer = _frame->image;
00311 }
00312 }
00313
00314
00315 unsigned char *
00316 Bumblebee2Camera::buffer()
00317 {
00318 return __buffer;
00319 }
00320
00321
00322 void
00323 Bumblebee2Camera::set_image_number(unsigned int image_num)
00324 {
00325 __image_num = image_num;
00326 switch ( image_num ) {
00327 case DEINTERLACED: __buffer = __buffer_deinterlaced; break;
00328 case RGB_IMAGE: __buffer = __buffer_rgb; break;
00329 default: __buffer = NULL; break;
00330 }
00331 }
00332
00333
00334
00335
00336
00337 bool
00338 Bumblebee2Camera::is_bumblebee2()
00339 {
00340 if ( ! _opened ) throw CameraNotOpenedException();
00341
00342 return( strncmp( _camera->model, "Bumblebee2", strlen("Bumblebee2") ) == 0);
00343 }
00344
00345
00346
00347 void
00348 Bumblebee2Camera::deinterlace_stereo()
00349 {
00350 dc1394_deinterlace_stereo( _frame->image, __buffer_deinterlaced,
00351 pixel_width(), 2 * pixel_height() );
00352 }
00353
00354
00355
00356
00357
00358
00359
00360 void
00361 Bumblebee2Camera::decode_bayer()
00362 {
00363 dc1394_bayer_decoding_8bit( __buffer_deinterlaced, __buffer_rgb,
00364 pixel_width(), 2 * pixel_height(),
00365 __bayer_pattern, DC1394_BAYER_METHOD_NEAREST );
00366 }
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379 void
00380 Bumblebee2Camera::deinterlace_stereo(unsigned char *raw16, unsigned char *deinterlaced,
00381 unsigned int width, unsigned int height)
00382 {
00383 dc1394_deinterlace_stereo( raw16, deinterlaced, width, 2 * height );
00384 }
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402 void
00403 Bumblebee2Camera::decode_bayer(unsigned char *deinterlaced, unsigned char *rgb,
00404 unsigned int width, unsigned int height,
00405 bayer_pattern_t bayer_pattern)
00406 {
00407 dc1394color_filter_t dc_bayer_pattern;
00408
00409 switch (bayer_pattern) {
00410 default:
00411 case BAYER_PATTERN_YYYY:
00412 dc_bayer_pattern = (dc1394color_filter_t) 0;
00413 break;
00414 case BAYER_PATTERN_RGGB:
00415 dc_bayer_pattern = DC1394_COLOR_FILTER_RGGB;
00416 break;
00417 case BAYER_PATTERN_GBRG:
00418 dc_bayer_pattern = DC1394_COLOR_FILTER_GBRG;
00419 break;
00420 case BAYER_PATTERN_GRBG:
00421 dc_bayer_pattern = DC1394_COLOR_FILTER_GRBG;
00422 break;
00423 case BAYER_PATTERN_BGGR:
00424 dc_bayer_pattern = DC1394_COLOR_FILTER_BGGR;
00425 break;
00426 }
00427
00428 dc1394_bayer_decoding_8bit( deinterlaced, rgb, width, 2 * height,
00429 dc_bayer_pattern, DC1394_BAYER_METHOD_NEAREST );
00430 }
00431
00432
00433
00434
00435
00436
00437 void
00438 Bumblebee2Camera::get_bayer_tile()
00439 {
00440 uint32_t value;
00441 if (dc1394_get_control_register( _camera, PGR_BAYER_TILE_MAPPING_REGISTER, &value) != DC1394_SUCCESS ) {
00442 throw Exception("Could not query bayer tile register");
00443 }
00444
00445
00446 switch (value) {
00447 default:
00448 case 0x59595959:
00449
00450 __bayer_pattern = (dc1394color_filter_t) 0;
00451 break;
00452 case 0x52474742:
00453 __bayer_pattern = DC1394_COLOR_FILTER_RGGB;
00454 break;
00455 case 0x47425247:
00456 __bayer_pattern = DC1394_COLOR_FILTER_GBRG;
00457 break;
00458 case 0x47524247:
00459 __bayer_pattern = DC1394_COLOR_FILTER_GRBG;
00460 break;
00461 case 0x42474752:
00462 __bayer_pattern = DC1394_COLOR_FILTER_BGGR;
00463 break;
00464 }
00465 }
00466
00467
00468
00469
00470
00471
00472
00473
00474
00475 void
00476 Bumblebee2Camera::write_triclops_config_from_camera_to_file(const char *filename)
00477 {
00478 dc1394error_t err;
00479 uint32_t value;
00480
00481 err = dc1394_get_control_register( _camera, PGR_REG_CONFIG_LENGTH, &value );
00482 if ( err != DC1394_SUCCESS ) {
00483 throw Exception("dc1394_get_control_register(PGR_REG_CONFIG_LENGTH) failed\n");
00484 }
00485
00486
00487 unsigned long file_size_bytes = value;
00488 if( file_size_bytes == 0 ) {
00489 throw Exception("File size == 0!\n" );
00490 }
00491
00492 FILE* file = fopen( filename, "w" );
00493 if ( !file ) {
00494 throw Exception("Can't open temporary file\n" );
00495 }
00496
00497
00498
00499 for( unsigned long offset = 0 ; offset < file_size_bytes; offset += 4 ) {
00500 err = dc1394_get_control_register( _camera,
00501 PGR_REG_CONFIG_DATA + offset,
00502 &value );
00503
00504 if( err != DC1394_SUCCESS ) {
00505 Exception e("Failed to get control register");
00506 e.append("Can't get control register 0x%x\n",
00507 (int) (PGR_REG_CONFIG_DATA+offset) );
00508 fclose( file );
00509 throw e;
00510 }
00511
00512 for( int i = 24; i >= 0; i -= 8 ) {
00513 fputc( ( (value>>i) & 0xFF ), file );
00514 }
00515 }
00516 fclose( file );
00517 }
00518
00519 }