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