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 <fvcams/bumblebee2.h> 00025 00026 #include <fvcams/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 #elif defined(__MACH__) && defined(__APPLE__) 00038 # include <sys/_endian.h> 00039 #else 00040 # include <endian.h> 00041 #endif 00042 00043 #include <utils/math/angle.h> 00044 00045 #include <cstdio> 00046 00047 #include <dc1394/utils.h> 00048 #include <dc1394/register.h> 00049 #include <dc1394/conversions.h> 00050 00051 using namespace fawkes; 00052 00053 namespace firevision { 00054 #if 0 /* just to make Emacs auto-indent happy */ 00055 } 00056 #endif 00057 00058 /** @class Bumblebee2Camera <fvcams/bumblebee2.h> 00059 * Bumblebee2 camera. 00060 * Camera implementation that allows fo access to the PointGrey Research Bumblebee2 00061 * camera. It uses libdc1394 to access the camera for fast image transfers (as recommended 00062 * by PTGrey) and can be used in conjunction with the TriclopsStereoProcessor in the 00063 * stereo utilities library. 00064 * 00065 * and the Triclops SDK by PTGrey for calculation of the stereo image. 00066 * This implementation is based on the Firewire implementation and extends it. The 00067 * capture() method implicitly does all the stereo processing needed. This cannot 00068 * be turned off. The video modes is appropriately configured for the camera. You can 00069 * get access to the left and right images where necessary using the set_image_number() 00070 * method and the constants LEFT_ORIGINAL and RIGHT_ORIGINAL. The disparity image buffer 00071 * can be retrieved via buffer_disparity(). 00072 * 00073 * Currently only the low resolution version (640x480) of the Bumblebee2 is supported, 00074 * an extension for the hires version may follow if we get one of these cameras. 00075 * 00076 * This class also encapsulates a coordinate system transformation that you can use to 00077 * transform the coordinates from the camera system to another right-handed system like 00078 * the robot system. 00079 * 00080 * The camera coordinate system has the X-axis pointing to the right, 00081 * Y-axis to the floor and Z-axis forward, if the camera is placed parallel to the ground 00082 * and you look in the direction of the camera. The origin of the system is in the right 00083 * lens system of the Bumblebee. 00084 * 00085 * @author Tim Niemueller 00086 */ 00087 00088 00089 /** Original image in RAW16 */ 00090 const unsigned int Bumblebee2Camera::ORIGINAL = 0; 00091 00092 /** Deinterlaced image */ 00093 const unsigned int Bumblebee2Camera::DEINTERLACED = 1; 00094 00095 /** From bayer tile decoded RGB image */ 00096 const unsigned int Bumblebee2Camera::RGB_IMAGE = 2; 00097 00098 00099 /// PGR specific registers 00100 /** PTGrey proprietary register: Bayer tile mapping information */ 00101 #define PGR_BAYER_TILE_MAPPING_REGISTER (0x1040) 00102 00103 /** PTGrey proprietary: config data length */ 00104 #define PGR_REG_CONFIG_LENGTH (0x1FFC) 00105 00106 /** PTGrey proprietary register: config register */ 00107 #define PGR_REG_CONFIG_DATA (0x2000) 00108 00109 /** PTGrey proprietary register: unit directory offset */ 00110 #define PGR_REG_UNIT_DIRECTORY_OFFSET (0x0424) 00111 00112 /** PTGrey proprietary register: image data format */ 00113 #define PGR_REG_IMAGE_DATA_FORMAT (0x1048) 00114 /** PTGrey image data format: PGR-specific (little endian) mode */ 00115 #define PTG_Y16_Data_Format_PGR_specific (0xFFFFFFFE) 00116 00117 /** PTGrey proprietary register: serial number */ 00118 #define PGR_REG_SERIAL_NUMBER (0x1F20) 00119 00120 /** PTGrey image data format: PGR-specific (little endian) mode */ 00121 /** Constructor. 00122 * Initialize and take parameters from camera argument parser. The following 00123 * arguments are supported: 00124 * - nbufs=NBUFS, number of DMA buffers, integer, 0 < n <= 32 00125 * - width=WIDTH, width in pixels of Format7 ROI 00126 * - height=HEIGHT, height in pixels of Format7 ROI 00127 * - startx=STARTX, X start of Format7 ROI 00128 * - starty=STARTY, Y start of Format7 ROI 00129 * @param cap camera argument parser 00130 */ 00131 Bumblebee2Camera::Bumblebee2Camera(const CameraArgumentParser *cap) 00132 : FirewireCamera(DC1394_FRAMERATE_30, 00133 DC1394_VIDEO_MODE_FORMAT7_3, 00134 DC1394_ISO_SPEED_400, 00135 /* num buffers */ 8) 00136 { 00137 // Defaults 00138 00139 _model = strdup(cap->cam_id().c_str()); 00140 // num_buffers set in constructor call 00141 _format7_coding = DC1394_COLOR_CODING_RAW16; 00142 _format7_width = 640; 00143 _format7_height = 480; 00144 _format7_startx = _format7_starty = 0; 00145 00146 if ( cap->has("nbufs") ) { 00147 _num_buffers = atoi(cap->get("nbufs").c_str()); 00148 } 00149 if ( cap->has("width") ) { 00150 _format7_width = atoi(cap->get("width").c_str()); 00151 } 00152 if ( cap->has("height") ) { 00153 _format7_height = atoi(cap->get("height").c_str()); 00154 } 00155 if ( cap->has("startx") ) { 00156 _format7_startx = atoi(cap->get("startx").c_str()); 00157 } 00158 if ( cap->has("starty") ) { 00159 _format7_starty = atoi(cap->get("starty").c_str()); 00160 } 00161 if ( cap->has("focus") ) { 00162 parse_set_focus(cap->get("focus").c_str()); 00163 } 00164 if ( cap->has("white_balance") ) { 00165 parse_set_white_balance(cap->get("white_balance").c_str()); 00166 } 00167 if ( cap->has("shutter") ) { 00168 parse_set_shutter(cap->get("shutter").c_str()); 00169 } 00170 00171 __buffer_deinterlaced = NULL; 00172 __buffer_rgb = NULL; 00173 } 00174 00175 00176 /** Destructor. */ 00177 Bumblebee2Camera::~Bumblebee2Camera() 00178 { 00179 if (__buffer_deinterlaced != NULL) free(__buffer_deinterlaced); 00180 if (__buffer_rgb != NULL) free(__buffer_rgb); 00181 } 00182 00183 00184 /** Get BB2 serial no. 00185 * @return BB2 serial number. 00186 */ 00187 uint32_t 00188 Bumblebee2Camera::serial_no() const 00189 { 00190 if ( ! _opened ) throw Exception("Camera not opened"); 00191 00192 uint32_t value = 0; 00193 dc1394error_t err = dc1394_get_control_register( _camera, PGR_REG_SERIAL_NUMBER, &value ); 00194 if ( err != DC1394_SUCCESS ) { 00195 throw Exception("Bumblebee2::serial_no: dc1394_get_control_register(PGR_REG_SERIAL_NUMBER) failed\n"); 00196 } 00197 return value; 00198 } 00199 00200 00201 /** Verify GUID validity. 00202 * Compares the given GUID with the GUID of the camera. The GUID may be of two 00203 * forms. If the first four bytes are all 0xFF then it is assumed that the 00204 * GUID was created from the BB2-specific serial number. For example if a 00205 * rectification LUT was generated with the context file only but without 00206 * access to the real camera. Otherwise the GUID is matched against the 00207 * Firewire GUID. 00208 * @param ver_guid GUID to verify 00209 * @return true if the given GUID matches the current camera, false otherwise 00210 */ 00211 bool 00212 Bumblebee2Camera::verify_guid(uint64_t ver_guid) const 00213 { 00214 if ( ! _opened ) throw Exception("Camera not opened"); 00215 00216 uint64_t tguid = ver_guid; 00217 tguid >>= 32; 00218 tguid &= 0xFFFFFFFF; 00219 if ( tguid == 0xFFFFFFFF ) { 00220 // serial number! 00221 ver_guid &= 0xFFFFFFFF; 00222 return (serial_no() == ver_guid); 00223 } else { 00224 return (guid() == ver_guid); 00225 } 00226 } 00227 00228 00229 void 00230 Bumblebee2Camera::print_info() 00231 { 00232 FirewireCamera::print_info(); 00233 00234 printf("Serial: %u\n", serial_no()); 00235 #if (defined(__WORDSIZE) && __WORDSIZE == 64) || (defined(LONG_BIT) && LONG_BIT == 64) 00236 printf("GUID: 0x%016lx\n", (long unsigned int)guid()); 00237 #else 00238 printf("GUID: 0x%016llx\n", guid()); 00239 #endif 00240 } 00241 00242 void 00243 Bumblebee2Camera::open() 00244 { 00245 try { 00246 FirewireCamera::open(); 00247 } catch (Exception &e) { 00248 throw; 00249 } 00250 00251 if ( ! _opened ) { 00252 throw Exception("Bumblebee2Camera::open: FirewireCamera::open dit not suceed"); 00253 } 00254 00255 __buffer_deinterlaced = (unsigned char *)malloc(pixel_width() * pixel_height() * 2); 00256 __buffer_rgb = malloc_buffer(RGB, pixel_width(), pixel_height() * 2); 00257 __buffer = NULL; 00258 00259 #if __BYTE_ORDER == __LITTLE_ENDIAN 00260 dc1394error_t err; 00261 typedef union { 00262 uint32_t value; 00263 struct { 00264 uint32_t presence : 1; 00265 uint32_t reserved1 : 21; 00266 uint32_t mirror : 1; 00267 uint32_t bayer_mono : 1; 00268 uint32_t reserved2 : 7; 00269 uint32_t data_format: 1; 00270 } idf; 00271 } idf_u; 00272 idf_u value; 00273 err = dc1394_get_control_register( _camera, PGR_REG_IMAGE_DATA_FORMAT, &(value.value) ); 00274 if ( err != DC1394_SUCCESS ) { 00275 throw Exception("Bumblebee2::open: dc1394_get_control_register(PGR_REG_DATA_FORMAT) failed\n"); 00276 } 00277 value.value &= PTG_Y16_Data_Format_PGR_specific; 00278 value.idf.data_format = 0; 00279 err = dc1394_set_control_register( _camera, PGR_REG_IMAGE_DATA_FORMAT, value.value ); 00280 if ( err != DC1394_SUCCESS ) { 00281 throw Exception("Bumblebee2::open: Setting PGR-specific mode on little-endian system failed\n"); 00282 } 00283 #endif 00284 00285 get_bayer_tile(); 00286 } 00287 00288 00289 void 00290 Bumblebee2Camera::close() 00291 { 00292 if ( _opened ) { 00293 FirewireCamera::close(); 00294 if (__buffer_deinterlaced != NULL) { 00295 free(__buffer_deinterlaced); 00296 __buffer_deinterlaced = NULL; 00297 } 00298 if (__buffer_rgb != NULL) { 00299 free(__buffer_rgb); 00300 __buffer_rgb = NULL; 00301 } 00302 } 00303 } 00304 00305 void 00306 Bumblebee2Camera::capture() 00307 { 00308 try { 00309 FirewireCamera::capture(); 00310 } catch (CaptureException &e) { 00311 e.append("Bumblebee2Camera::capture: failed to retrieve image"); 00312 if ( ORIGINAL == __image_num ) __buffer = NULL; 00313 throw; 00314 } 00315 if ( ORIGINAL == __image_num ) { 00316 __buffer = _frame->image; 00317 } 00318 } 00319 00320 00321 unsigned char * 00322 Bumblebee2Camera::buffer() 00323 { 00324 return __buffer; 00325 } 00326 00327 00328 void 00329 Bumblebee2Camera::set_image_number(unsigned int image_num) 00330 { 00331 __image_num = image_num; 00332 switch ( image_num ) { 00333 case DEINTERLACED: __buffer = __buffer_deinterlaced; break; 00334 case RGB_IMAGE: __buffer = __buffer_rgb; break; 00335 default: __buffer = NULL; break; 00336 } 00337 } 00338 00339 00340 /** Check if connected camera is a Bumblebee2. 00341 * @return true, if the connected camera is a Bumblebee2, false otherwise 00342 */ 00343 bool 00344 Bumblebee2Camera::is_bumblebee2() 00345 { 00346 if ( ! _opened ) throw CameraNotOpenedException(); 00347 00348 return( strncmp( _camera->model, "Bumblebee2", strlen("Bumblebee2") ) == 0); 00349 } 00350 00351 00352 /** De-interlace the 16 bit data into 2 bayer tile pattern images. */ 00353 void 00354 Bumblebee2Camera::deinterlace_stereo() 00355 { 00356 dc1394_deinterlace_stereo( _frame->image, __buffer_deinterlaced, 00357 pixel_width(), 2 * pixel_height() ); 00358 } 00359 00360 00361 /** Extract RGB color image from the bayer tile image. 00362 * This will transform the bayer tile image to an RGB image using the 00363 * nearest neighbour method. 00364 * Note: this will alias colors on the top and bottom rows 00365 */ 00366 void 00367 Bumblebee2Camera::decode_bayer() 00368 { 00369 dc1394_bayer_decoding_8bit( __buffer_deinterlaced, __buffer_rgb, 00370 pixel_width(), 2 * pixel_height(), 00371 __bayer_pattern, DC1394_BAYER_METHOD_NEAREST ); 00372 } 00373 00374 00375 00376 00377 00378 /** De-interlace the 16 bit data into 2 bayer tile pattern images. 00379 * Can be used for offline de-interlacing. 00380 * @param raw16 In-buffer RAW16-encoded 00381 * @param deinterlaced upon return contains the deinterlaced image 00382 * @param width width of image in pixels 00383 * @param height height of image in pixels 00384 */ 00385 void 00386 Bumblebee2Camera::deinterlace_stereo(unsigned char *raw16, unsigned char *deinterlaced, 00387 unsigned int width, unsigned int height) 00388 { 00389 dc1394_deinterlace_stereo( raw16, deinterlaced, width, 2 * height ); 00390 } 00391 00392 00393 /** Extract RGB color image from the bayer tile image. 00394 * This will transform the bayer tile image to an RGB image using the 00395 * nearest neighbour method. 00396 * Note: this will alias colors on the top and bottom rows 00397 * @param deinterlaced in-buffer with deinterlaced image 00398 * @param rgb upon return contains RGB image 00399 * @param width width of image in pixels 00400 * @param height height of image in pixels 00401 * @param bayer_pattern bayer pattern, one of 00402 * - 0x59595959 (YYYY, no pattern) 00403 * - 0x52474742 (RGGB) 00404 * - 0x47524247 (GRBG) 00405 * - 0x42474752 (BGGR) 00406 * This depends on the used camera. 00407 */ 00408 void 00409 Bumblebee2Camera::decode_bayer(unsigned char *deinterlaced, unsigned char *rgb, 00410 unsigned int width, unsigned int height, 00411 bayer_pattern_t bayer_pattern) 00412 { 00413 dc1394color_filter_t dc_bayer_pattern; 00414 00415 switch (bayer_pattern) { 00416 default: 00417 case BAYER_PATTERN_YYYY: 00418 dc_bayer_pattern = (dc1394color_filter_t) 0; 00419 break; 00420 case BAYER_PATTERN_RGGB: 00421 dc_bayer_pattern = DC1394_COLOR_FILTER_RGGB; 00422 break; 00423 case BAYER_PATTERN_GBRG: 00424 dc_bayer_pattern = DC1394_COLOR_FILTER_GBRG; 00425 break; 00426 case BAYER_PATTERN_GRBG: 00427 dc_bayer_pattern = DC1394_COLOR_FILTER_GRBG; 00428 break; 00429 case BAYER_PATTERN_BGGR: 00430 dc_bayer_pattern = DC1394_COLOR_FILTER_BGGR; 00431 break; 00432 } 00433 00434 dc1394_bayer_decoding_8bit( deinterlaced, rgb, width, 2 * height, 00435 dc_bayer_pattern, DC1394_BAYER_METHOD_NEAREST ); 00436 } 00437 00438 00439 /** Retrieve bayer tile. 00440 * This is an internal method that access a special PTGrey register in the camera to 00441 * determine the bayer tile mode. 00442 */ 00443 void 00444 Bumblebee2Camera::get_bayer_tile() 00445 { 00446 uint32_t value; 00447 if (dc1394_get_control_register( _camera, PGR_BAYER_TILE_MAPPING_REGISTER, &value) != DC1394_SUCCESS ) { 00448 throw Exception("Could not query bayer tile register"); 00449 } 00450 00451 // Magic numbers are specific to PTGrey cameras 00452 switch (value) { 00453 default: 00454 case 0x59595959: // YYYY 00455 // no bayer 00456 __bayer_pattern = (dc1394color_filter_t) 0; 00457 break; 00458 case 0x52474742: // RGGB 00459 __bayer_pattern = DC1394_COLOR_FILTER_RGGB; 00460 break; 00461 case 0x47425247: // GBRG 00462 __bayer_pattern = DC1394_COLOR_FILTER_GBRG; 00463 break; 00464 case 0x47524247: // GRBG 00465 __bayer_pattern = DC1394_COLOR_FILTER_GRBG; 00466 break; 00467 case 0x42474752: // BGGR 00468 __bayer_pattern = DC1394_COLOR_FILTER_BGGR; 00469 break; 00470 } 00471 } 00472 00473 00474 /** Retrieve config from camera. 00475 * This method retrieves the config from the camera and writes it to a file such that 00476 * the Triclops SDK can use it for context initialization. 00477 * @param filename filename to write the config to 00478 * @exception Exception thrown if there is an error when trying to retrieve the config 00479 * or writing it to a file. 00480 */ 00481 void 00482 Bumblebee2Camera::write_triclops_config_from_camera_to_file(const char *filename) 00483 { 00484 dc1394error_t err; 00485 uint32_t value; 00486 00487 err = dc1394_get_control_register( _camera, PGR_REG_CONFIG_LENGTH, &value ); 00488 if ( err != DC1394_SUCCESS ) { 00489 throw Exception("dc1394_get_control_register(PGR_REG_CONFIG_LENGTH) failed\n"); 00490 } 00491 00492 // the length of the config file 00493 unsigned long file_size_bytes = value; 00494 if( file_size_bytes == 0 ) { 00495 throw Exception("File size == 0!\n" ); 00496 } 00497 00498 FILE* file = fopen( filename, "w" ); 00499 if ( !file ) { 00500 throw Exception("Can't open temporary file\n" ); 00501 } 00502 00503 // Read the config file, and save it to the output file, 00504 // while fixing endianness. 00505 for( unsigned long offset = 0 ; offset < file_size_bytes; offset += 4 ) { 00506 err = dc1394_get_control_register( _camera, 00507 PGR_REG_CONFIG_DATA + offset, 00508 &value ); 00509 00510 if( err != DC1394_SUCCESS ) { 00511 Exception e("Failed to get control register"); 00512 e.append("Can't get control register 0x%x\n", 00513 (int) (PGR_REG_CONFIG_DATA+offset) ); 00514 fclose( file ); 00515 throw e; 00516 } 00517 00518 for( int i = 24; i >= 0; i -= 8 ) { 00519 fputc( ( (value>>i) & 0xFF ), file ); 00520 } 00521 } 00522 fclose( file ); 00523 } 00524 00525 } // end namespace firevision