Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * bumblebee2.cpp - Point Grey Bumblebee 2 camera 00004 * 00005 * Generated: Sat Apr 14 20:51:19 2007 (watching Ghostbusters) 00006 * Copyright 2005-2007 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. A runtime exception applies to 00014 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 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 // include <fvutils/writers/pnm.h> 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 /* just to make Emacs auto-indent happy */ 00053 } 00054 #endif 00055 00056 /** @class Bumblebee2Camera <cams/bumblebee2.h> 00057 * Bumblebee2 camera. 00058 * Camera implementation that allows fo access to the PointGrey Research Bumblebee2 00059 * camera. It uses libdc1394 to access the camera for fast image transfers (as recommended 00060 * by PTGrey) and can be used in conjunction with the TriclopsStereoProcessor in the 00061 * stereo utilities library. 00062 * 00063 * and the Triclops SDK by PTGrey for calculation of the stereo image. 00064 * This implementation is based on the Firewire implementation and extends it. The 00065 * capture() method implicitly does all the stereo processing needed. This cannot 00066 * be turned off. The video modes is appropriately configured for the camera. You can 00067 * get access to the left and right images where necessary using the set_image_number() 00068 * method and the constants LEFT_ORIGINAL and RIGHT_ORIGINAL. The disparity image buffer 00069 * can be retrieved via buffer_disparity(). 00070 * 00071 * Currently only the low resolution version (640x480) of the Bumblebee2 is supported, 00072 * an extension for the hires version may follow if we get one of these cameras. 00073 * 00074 * This class also encapsulates a coordinate system transformation that you can use to 00075 * transform the coordinates from the camera system to another right-handed system like 00076 * the robot system. 00077 * 00078 * The camera coordinate system has the X-axis pointing to the right, 00079 * Y-axis to the floor and Z-axis forward, if the camera is placed parallel to the ground 00080 * and you look in the direction of the camera. The origin of the system is in the right 00081 * lens system of the Bumblebee. 00082 * 00083 * @author Tim Niemueller 00084 */ 00085 00086 00087 /** Original image in RAW16 */ 00088 const unsigned int Bumblebee2Camera::ORIGINAL = 0; 00089 00090 /** Deinterlaced image */ 00091 const unsigned int Bumblebee2Camera::DEINTERLACED = 1; 00092 00093 /** From bayer tile decoded RGB image */ 00094 const unsigned int Bumblebee2Camera::RGB_IMAGE = 2; 00095 00096 00097 /// PGR specific registers 00098 /** PTGrey proprietary register: Bayer tile mapping information */ 00099 #define PGR_BAYER_TILE_MAPPING_REGISTER (0x1040) 00100 00101 /** PTGrey proprietary: config data length */ 00102 #define PGR_REG_CONFIG_LENGTH (0x1FFC) 00103 00104 /** PTGrey proprietary register: config register */ 00105 #define PGR_REG_CONFIG_DATA (0x2000) 00106 00107 /** PTGrey proprietary register: unit directory offset */ 00108 #define PGR_REG_UNIT_DIRECTORY_OFFSET (0x0424) 00109 00110 /** PTGrey proprietary register: image data format */ 00111 #define PGR_REG_IMAGE_DATA_FORMAT (0x1048) 00112 /** PTGrey image data format: PGR-specific (little endian) mode */ 00113 #define PTG_Y16_Data_Format_PGR_specific (0xFFFFFFFE) 00114 00115 /** PTGrey proprietary register: serial number */ 00116 #define PGR_REG_SERIAL_NUMBER (0x1F20) 00117 00118 /** PTGrey image data format: PGR-specific (little endian) mode */ 00119 /** Constructor. 00120 * Initialize and take parameters from camera argument parser. The following 00121 * arguments are supported: 00122 * - nbufs=NBUFS, number of DMA buffers, integer, 0 < n <= 32 00123 * - width=WIDTH, width in pixels of Format7 ROI 00124 * - height=HEIGHT, height in pixels of Format7 ROI 00125 * - startx=STARTX, X start of Format7 ROI 00126 * - starty=STARTY, Y start of Format7 ROI 00127 * @param cap camera argument parser 00128 */ 00129 Bumblebee2Camera::Bumblebee2Camera(const CameraArgumentParser *cap) 00130 : FirewireCamera(DC1394_FRAMERATE_30, 00131 DC1394_VIDEO_MODE_FORMAT7_3, 00132 DC1394_ISO_SPEED_400, 00133 /* num buffers */ 8) 00134 { 00135 // Defaults 00136 00137 _model = strdup(cap->cam_id().c_str()); 00138 // num_buffers set in constructor call 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 /** Destructor. */ 00175 Bumblebee2Camera::~Bumblebee2Camera() 00176 { 00177 if (__buffer_deinterlaced != NULL) free(__buffer_deinterlaced); 00178 if (__buffer_rgb != NULL) free(__buffer_rgb); 00179 } 00180 00181 00182 /** Get BB2 serial no. 00183 * @return BB2 serial number. 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 /** Verify GUID validity. 00200 * Compares the given GUID with the GUID of the camera. The GUID may be of two 00201 * forms. If the first four bytes are all 0xFF then it is assumed that the 00202 * GUID was created from the BB2-specific serial number. For example if a 00203 * rectification LUT was generated with the context file only but without 00204 * access to the real camera. Otherwise the GUID is matched against the 00205 * Firewire GUID. 00206 * @param ver_guid GUID to verify 00207 * @return true if the given GUID matches the current camera, false otherwise 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 // serial number! 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 /** Check if connected camera is a Bumblebee2. 00339 * @return true, if the connected camera is a Bumblebee2, false otherwise 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 /** De-interlace the 16 bit data into 2 bayer tile pattern images. */ 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 /** Extract RGB color image from the bayer tile image. 00360 * This will transform the bayer tile image to an RGB image using the 00361 * nearest neighbour method. 00362 * Note: this will alias colors on the top and bottom rows 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 /** De-interlace the 16 bit data into 2 bayer tile pattern images. 00377 * Can be used for offline de-interlacing. 00378 * @param raw16 In-buffer RAW16-encoded 00379 * @param deinterlaced upon return contains the deinterlaced image 00380 * @param width width of image in pixels 00381 * @param height height of image in pixels 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 /** Extract RGB color image from the bayer tile image. 00392 * This will transform the bayer tile image to an RGB image using the 00393 * nearest neighbour method. 00394 * Note: this will alias colors on the top and bottom rows 00395 * @param deinterlaced in-buffer with deinterlaced image 00396 * @param rgb upon return contains RGB image 00397 * @param width width of image in pixels 00398 * @param height height of image in pixels 00399 * @param bayer_pattern bayer pattern, one of 00400 * - 0x59595959 (YYYY, no pattern) 00401 * - 0x52474742 (RGGB) 00402 * - 0x47524247 (GRBG) 00403 * - 0x42474752 (BGGR) 00404 * This depends on the used camera. 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 /** Retrieve bayer tile. 00438 * This is an internal method that access a special PTGrey register in the camera to 00439 * determine the bayer tile mode. 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 // Magic numbers are specific to PTGrey cameras 00450 switch (value) { 00451 default: 00452 case 0x59595959: // YYYY 00453 // no bayer 00454 __bayer_pattern = (dc1394color_filter_t) 0; 00455 break; 00456 case 0x52474742: // RGGB 00457 __bayer_pattern = DC1394_COLOR_FILTER_RGGB; 00458 break; 00459 case 0x47425247: // GBRG 00460 __bayer_pattern = DC1394_COLOR_FILTER_GBRG; 00461 break; 00462 case 0x47524247: // GRBG 00463 __bayer_pattern = DC1394_COLOR_FILTER_GRBG; 00464 break; 00465 case 0x42474752: // BGGR 00466 __bayer_pattern = DC1394_COLOR_FILTER_BGGR; 00467 break; 00468 } 00469 } 00470 00471 00472 /** Retrieve config from camera. 00473 * This method retrieves the config from the camera and writes it to a file such that 00474 * the Triclops SDK can use it for context initialization. 00475 * @param filename filename to write the config to 00476 * @exception Exception thrown if there is an error when trying to retrieve the config 00477 * or writing it to a file. 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 // the length of the config file 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 // Read the config file, and save it to the output file, 00502 // while fixing endianness. 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 } // end namespace firevision