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