Fawkes API  Fawkes Development Version
bumblebee2.cpp
1 
2 /***************************************************************************
3  * bumblebee2.cpp - Point Grey Bumblebee 2 camera
4  *
5  * Generated: Sat Apr 14 20:51:19 2007 (watching Ghostbusters)
6  * Copyright 2005-2007 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22  */
23 
24 #include <fvcams/bumblebee2.h>
25 
26 #include <fvcams/cam_exceptions.h>
27 #include <core/exception.h>
28 #include <fvutils/system/camargp.h>
29 #include <fvutils/color/conversions.h>
30 // include <fvutils/writers/pnm.h>
31 
32 #include <stdlib.h>
33 #include <unistd.h>
34 #include <string>
35 #ifdef __FreeBSD__
36 # include <sys/endian.h>
37 #elif defined(__MACH__) && defined(__APPLE__)
38 # include <sys/_endian.h>
39 #else
40 # include <endian.h>
41 #endif
42 
43 #include <utils/math/angle.h>
44 
45 #include <cstdio>
46 
47 #include <dc1394/utils.h>
48 #include <dc1394/register.h>
49 #include <dc1394/conversions.h>
50 
51 using namespace fawkes;
52 
53 namespace firevision {
54 #if 0 /* just to make Emacs auto-indent happy */
55 }
56 #endif
57 
58 /** @class Bumblebee2Camera <fvcams/bumblebee2.h>
59  * Bumblebee2 camera.
60  * Camera implementation that allows fo access to the PointGrey Research Bumblebee2
61  * camera. It uses libdc1394 to access the camera for fast image transfers (as recommended
62  * by PTGrey) and can be used in conjunction with the TriclopsStereoProcessor in the
63  * stereo utilities library.
64  *
65  * and the Triclops SDK by PTGrey for calculation of the stereo image.
66  * This implementation is based on the Firewire implementation and extends it. The
67  * capture() method implicitly does all the stereo processing needed. This cannot
68  * be turned off. The video modes is appropriately configured for the camera. You can
69  * get access to the left and right images where necessary using the set_image_number()
70  * method and the constants LEFT_ORIGINAL and RIGHT_ORIGINAL. The disparity image buffer
71  * can be retrieved via buffer_disparity().
72  *
73  * Currently only the low resolution version (640x480) of the Bumblebee2 is supported,
74  * an extension for the hires version may follow if we get one of these cameras.
75  *
76  * This class also encapsulates a coordinate system transformation that you can use to
77  * transform the coordinates from the camera system to another right-handed system like
78  * the robot system.
79  *
80  * The camera coordinate system has the X-axis pointing to the right,
81  * Y-axis to the floor and Z-axis forward, if the camera is placed parallel to the ground
82  * and you look in the direction of the camera. The origin of the system is in the right
83  * lens system of the Bumblebee.
84  *
85  * @author Tim Niemueller
86  */
87 
88 
89 /** Original image in RAW16 */
90 const unsigned int Bumblebee2Camera::ORIGINAL = 0;
91 
92 /** Deinterlaced image */
93 const unsigned int Bumblebee2Camera::DEINTERLACED = 1;
94 
95 /** From bayer tile decoded RGB image */
96 const unsigned int Bumblebee2Camera::RGB_IMAGE = 2;
97 
98 
99 /// PGR specific registers
100 /** PTGrey proprietary register: Bayer tile mapping information */
101 #define PGR_BAYER_TILE_MAPPING_REGISTER (0x1040)
102 
103 /** PTGrey proprietary: config data length */
104 #define PGR_REG_CONFIG_LENGTH (0x1FFC)
105 
106 /** PTGrey proprietary register: config register */
107 #define PGR_REG_CONFIG_DATA (0x2000)
108 
109 /** PTGrey proprietary register: unit directory offset */
110 #define PGR_REG_UNIT_DIRECTORY_OFFSET (0x0424)
111 
112 /** PTGrey proprietary register: image data format */
113 #define PGR_REG_IMAGE_DATA_FORMAT (0x1048)
114 /** PTGrey image data format: PGR-specific (little endian) mode */
115 #define PTG_Y16_Data_Format_PGR_specific (0xFFFFFFFE)
116 
117 /** PTGrey proprietary register: serial number */
118 #define PGR_REG_SERIAL_NUMBER (0x1F20)
119 
120 /** PTGrey image data format: PGR-specific (little endian) mode */
121 /** Constructor.
122  * Initialize and take parameters from camera argument parser. The following
123  * arguments are supported:
124  * - nbufs=NBUFS, number of DMA buffers, integer, 0 < n <= 32
125  * - width=WIDTH, width in pixels of Format7 ROI
126  * - height=HEIGHT, height in pixels of Format7 ROI
127  * - startx=STARTX, X start of Format7 ROI
128  * - starty=STARTY, Y start of Format7 ROI
129  * @param cap camera argument parser
130  */
131 Bumblebee2Camera::Bumblebee2Camera(const CameraArgumentParser *cap)
132  : FirewireCamera(DC1394_FRAMERATE_30,
133  DC1394_VIDEO_MODE_FORMAT7_3,
134  DC1394_ISO_SPEED_400,
135  /* num buffers */ 8)
136 {
137  // Defaults
138 
139  _model = strdup(cap->cam_id().c_str());
140  // num_buffers set in constructor call
141  _format7_coding = DC1394_COLOR_CODING_RAW16;
142  _format7_width = 640;
143  _format7_height = 480;
145 
146  if ( cap->has("nbufs") ) {
147  _num_buffers = atoi(cap->get("nbufs").c_str());
148  }
149  if ( cap->has("width") ) {
150  _format7_width = atoi(cap->get("width").c_str());
151  }
152  if ( cap->has("height") ) {
153  _format7_height = atoi(cap->get("height").c_str());
154  }
155  if ( cap->has("startx") ) {
156  _format7_startx = atoi(cap->get("startx").c_str());
157  }
158  if ( cap->has("starty") ) {
159  _format7_starty = atoi(cap->get("starty").c_str());
160  }
161  if ( cap->has("focus") ) {
162  parse_set_focus(cap->get("focus").c_str());
163  }
164  if ( cap->has("white_balance") ) {
165  parse_set_white_balance(cap->get("white_balance").c_str());
166  }
167  if ( cap->has("shutter") ) {
168  parse_set_shutter(cap->get("shutter").c_str());
169  }
170 
171  __buffer_deinterlaced = NULL;
172  __buffer_rgb = NULL;
173 }
174 
175 
176 /** Destructor. */
178 {
179  if (__buffer_deinterlaced != NULL) free(__buffer_deinterlaced);
180  if (__buffer_rgb != NULL) free(__buffer_rgb);
181 }
182 
183 
184 /** Get BB2 serial no.
185  * @return BB2 serial number.
186  */
187 uint32_t
189 {
190  if ( ! _opened ) throw Exception("Camera not opened");
191 
192  uint32_t value = 0;
193  dc1394error_t err = dc1394_get_control_register( _camera, PGR_REG_SERIAL_NUMBER, &value );
194  if ( err != DC1394_SUCCESS ) {
195  throw Exception("Bumblebee2::serial_no: dc1394_get_control_register(PGR_REG_SERIAL_NUMBER) failed\n");
196  }
197  return value;
198 }
199 
200 
201 /** Verify GUID validity.
202  * Compares the given GUID with the GUID of the camera. The GUID may be of two
203  * forms. If the first four bytes are all 0xFF then it is assumed that the
204  * GUID was created from the BB2-specific serial number. For example if a
205  * rectification LUT was generated with the context file only but without
206  * access to the real camera. Otherwise the GUID is matched against the
207  * Firewire GUID.
208  * @param ver_guid GUID to verify
209  * @return true if the given GUID matches the current camera, false otherwise
210  */
211 bool
212 Bumblebee2Camera::verify_guid(uint64_t ver_guid) const
213 {
214  if ( ! _opened ) throw Exception("Camera not opened");
215 
216  uint64_t tguid = ver_guid;
217  tguid >>= 32;
218  tguid &= 0xFFFFFFFF;
219  if ( tguid == 0xFFFFFFFF ) {
220  // serial number!
221  ver_guid &= 0xFFFFFFFF;
222  return (serial_no() == ver_guid);
223  } else {
224  return (guid() == ver_guid);
225  }
226 }
227 
228 
229 void
231 {
233 
234  printf("Serial: %u\n", serial_no());
235 #if (defined(__WORDSIZE) && __WORDSIZE == 64) || (defined(LONG_BIT) && LONG_BIT == 64)
236  printf("GUID: 0x%016lx\n", (long unsigned int)guid());
237 #else
238  printf("GUID: 0x%016llx\n", guid());
239 #endif
240 }
241 
242 void
244 {
245  try {
247  } catch (Exception &e) {
248  throw;
249  }
250 
251  if ( ! _opened ) {
252  throw Exception("Bumblebee2Camera::open: FirewireCamera::open dit not suceed");
253  }
254 
255  __buffer_deinterlaced = (unsigned char *)malloc(pixel_width() * pixel_height() * 2);
256  __buffer_rgb = malloc_buffer(RGB, pixel_width(), pixel_height() * 2);
257  __buffer = NULL;
258 
259 #if __BYTE_ORDER == __LITTLE_ENDIAN
260  dc1394error_t err;
261  typedef union {
262  uint32_t value;
263  struct {
264  uint32_t presence : 1;
265  uint32_t reserved1 : 21;
266  uint32_t mirror : 1;
267  uint32_t bayer_mono : 1;
268  uint32_t reserved2 : 7;
269  uint32_t data_format: 1;
270  } idf;
271  } idf_u;
272  idf_u value;
273  err = dc1394_get_control_register( _camera, PGR_REG_IMAGE_DATA_FORMAT, &(value.value) );
274  if ( err != DC1394_SUCCESS ) {
275  throw Exception("Bumblebee2::open: dc1394_get_control_register(PGR_REG_DATA_FORMAT) failed\n");
276  }
277  value.value &= PTG_Y16_Data_Format_PGR_specific;
278  value.idf.data_format = 0;
279  err = dc1394_set_control_register( _camera, PGR_REG_IMAGE_DATA_FORMAT, value.value );
280  if ( err != DC1394_SUCCESS ) {
281  throw Exception("Bumblebee2::open: Setting PGR-specific mode on little-endian system failed\n");
282  }
283 #endif
284 
285  get_bayer_tile();
286 }
287 
288 
289 void
291 {
292  if ( _opened ) {
294  if (__buffer_deinterlaced != NULL) {
295  free(__buffer_deinterlaced);
296  __buffer_deinterlaced = NULL;
297  }
298  if (__buffer_rgb != NULL) {
299  free(__buffer_rgb);
300  __buffer_rgb = NULL;
301  }
302  }
303 }
304 
305 void
307 {
308  try {
310  } catch (CaptureException &e) {
311  e.append("Bumblebee2Camera::capture: failed to retrieve image");
312  if ( ORIGINAL == __image_num ) __buffer = NULL;
313  throw;
314  }
315  if ( ORIGINAL == __image_num ) {
316  __buffer = _frame->image;
317  }
318 }
319 
320 
321 unsigned char *
323 {
324  return __buffer;
325 }
326 
327 
328 void
329 Bumblebee2Camera::set_image_number(unsigned int image_num)
330 {
331  __image_num = image_num;
332  switch ( image_num ) {
333  case DEINTERLACED: __buffer = __buffer_deinterlaced; break;
334  case RGB_IMAGE: __buffer = __buffer_rgb; break;
335  default: __buffer = NULL; break;
336  }
337 }
338 
339 
340 /** Check if connected camera is a Bumblebee2.
341  * @return true, if the connected camera is a Bumblebee2, false otherwise
342  */
343 bool
345 {
346  if ( ! _opened ) throw CameraNotOpenedException();
347 
348  return( strncmp( _camera->model, "Bumblebee2", strlen("Bumblebee2") ) == 0);
349 }
350 
351 
352 /** De-interlace the 16 bit data into 2 bayer tile pattern images. */
353 void
355 {
356  dc1394_deinterlace_stereo( _frame->image, __buffer_deinterlaced,
357  pixel_width(), 2 * pixel_height() );
358 }
359 
360 
361 /** Extract RGB color image from the bayer tile image.
362  * This will transform the bayer tile image to an RGB image using the
363  * nearest neighbour method.
364  * Note: this will alias colors on the top and bottom rows
365  */
366 void
368 {
369  dc1394_bayer_decoding_8bit( __buffer_deinterlaced, __buffer_rgb,
370  pixel_width(), 2 * pixel_height(),
371  __bayer_pattern, DC1394_BAYER_METHOD_NEAREST );
372 }
373 
374 
375 
376 
377 
378 /** De-interlace the 16 bit data into 2 bayer tile pattern images.
379  * Can be used for offline de-interlacing.
380  * @param raw16 In-buffer RAW16-encoded
381  * @param deinterlaced upon return contains the deinterlaced image
382  * @param width width of image in pixels
383  * @param height height of image in pixels
384  */
385 void
386 Bumblebee2Camera::deinterlace_stereo(unsigned char *raw16, unsigned char *deinterlaced,
387  unsigned int width, unsigned int height)
388 {
389  dc1394_deinterlace_stereo( raw16, deinterlaced, width, 2 * height );
390 }
391 
392 
393 /** Extract RGB color image from the bayer tile image.
394  * This will transform the bayer tile image to an RGB image using the
395  * nearest neighbour method.
396  * Note: this will alias colors on the top and bottom rows
397  * @param deinterlaced in-buffer with deinterlaced image
398  * @param rgb upon return contains RGB image
399  * @param width width of image in pixels
400  * @param height height of image in pixels
401  * @param bayer_pattern bayer pattern, one of
402  * - 0x59595959 (YYYY, no pattern)
403  * - 0x52474742 (RGGB)
404  * - 0x47524247 (GRBG)
405  * - 0x42474752 (BGGR)
406  * This depends on the used camera.
407  */
408 void
409 Bumblebee2Camera::decode_bayer(unsigned char *deinterlaced, unsigned char *rgb,
410  unsigned int width, unsigned int height,
411  bayer_pattern_t bayer_pattern)
412 {
413  dc1394color_filter_t dc_bayer_pattern;
414 
415  switch (bayer_pattern) {
416  default:
417  case BAYER_PATTERN_YYYY:
418  dc_bayer_pattern = (dc1394color_filter_t) 0;
419  break;
420  case BAYER_PATTERN_RGGB:
421  dc_bayer_pattern = DC1394_COLOR_FILTER_RGGB;
422  break;
423  case BAYER_PATTERN_GBRG:
424  dc_bayer_pattern = DC1394_COLOR_FILTER_GBRG;
425  break;
426  case BAYER_PATTERN_GRBG:
427  dc_bayer_pattern = DC1394_COLOR_FILTER_GRBG;
428  break;
429  case BAYER_PATTERN_BGGR:
430  dc_bayer_pattern = DC1394_COLOR_FILTER_BGGR;
431  break;
432  }
433 
434  dc1394_bayer_decoding_8bit( deinterlaced, rgb, width, 2 * height,
435  dc_bayer_pattern, DC1394_BAYER_METHOD_NEAREST );
436 }
437 
438 
439 /** Retrieve bayer tile.
440  * This is an internal method that access a special PTGrey register in the camera to
441  * determine the bayer tile mode.
442  */
443 void
444 Bumblebee2Camera::get_bayer_tile()
445 {
446  uint32_t value;
447  if (dc1394_get_control_register( _camera, PGR_BAYER_TILE_MAPPING_REGISTER, &value) != DC1394_SUCCESS ) {
448  throw Exception("Could not query bayer tile register");
449  }
450 
451  // Magic numbers are specific to PTGrey cameras
452  switch (value) {
453  default:
454  case 0x59595959: // YYYY
455  // no bayer
456  __bayer_pattern = (dc1394color_filter_t) 0;
457  break;
458  case 0x52474742: // RGGB
459  __bayer_pattern = DC1394_COLOR_FILTER_RGGB;
460  break;
461  case 0x47425247: // GBRG
462  __bayer_pattern = DC1394_COLOR_FILTER_GBRG;
463  break;
464  case 0x47524247: // GRBG
465  __bayer_pattern = DC1394_COLOR_FILTER_GRBG;
466  break;
467  case 0x42474752: // BGGR
468  __bayer_pattern = DC1394_COLOR_FILTER_BGGR;
469  break;
470  }
471 }
472 
473 
474 /** Retrieve config from camera.
475  * This method retrieves the config from the camera and writes it to a file such that
476  * the Triclops SDK can use it for context initialization.
477  * @param filename filename to write the config to
478  * @exception Exception thrown if there is an error when trying to retrieve the config
479  * or writing it to a file.
480  */
481 void
483 {
484  dc1394error_t err;
485  uint32_t value;
486 
487  err = dc1394_get_control_register( _camera, PGR_REG_CONFIG_LENGTH, &value );
488  if ( err != DC1394_SUCCESS ) {
489  throw Exception("dc1394_get_control_register(PGR_REG_CONFIG_LENGTH) failed\n");
490  }
491 
492  // the length of the config file
493  unsigned long file_size_bytes = value;
494  if( file_size_bytes == 0 ) {
495  throw Exception("File size == 0!\n" );
496  }
497 
498  FILE* file = fopen( filename, "w" );
499  if ( !file ) {
500  throw Exception("Can't open temporary file\n" );
501  }
502 
503  // Read the config file, and save it to the output file,
504  // while fixing endianness.
505  for( unsigned long offset = 0 ; offset < file_size_bytes; offset += 4 ) {
506  err = dc1394_get_control_register( _camera,
507  PGR_REG_CONFIG_DATA + offset,
508  &value );
509 
510  if( err != DC1394_SUCCESS ) {
511  Exception e("Failed to get control register");
512  e.append("Can't get control register 0x%x\n",
513  (int) (PGR_REG_CONFIG_DATA+offset) );
514  fclose( file );
515  throw e;
516  }
517 
518  for( int i = 24; i >= 0; i -= 8 ) {
519  fputc( ( (value>>i) & 0xFF ), file );
520  }
521  }
522  fclose( file );
523 }
524 
525 } // end namespace firevision
virtual ~Bumblebee2Camera()
Destructor.
Definition: bumblebee2.cpp:177
bool _opened
true if camera has been opened, false otherwise
Definition: firewire.h:116
virtual void open()
Open the camera.
Definition: bumblebee2.cpp:243
int _format7_width
Format7 width.
Definition: firewire.h:153
std::string get(std::string s) const
Get the value of the given parameter.
Definition: camargp.cpp:164
virtual unsigned int pixel_width()
Width of image in pixels.
Definition: firewire.cpp:396
virtual void parse_set_focus(const char *focus)
Parse focus and set value.
Definition: firewire.cpp:724
virtual void set_image_number(unsigned int image_num)
Set image number to retrieve.
Definition: bumblebee2.cpp:329
static const unsigned int RGB_IMAGE
From bayer tile decoded RGB image.
Definition: bumblebee2.h:43
virtual void print_info()
Print out camera information.
Definition: firewire.cpp:267
virtual unsigned int pixel_height()
Height of image in pixels.
Definition: firewire.cpp:417
static const unsigned int DEINTERLACED
Deinterlaced image.
Definition: bumblebee2.h:42
void write_triclops_config_from_camera_to_file(const char *filename)
Retrieve config from camera.
Definition: bumblebee2.cpp:482
Fawkes library namespace.
Capturing a frame failed.
void decode_bayer()
Extract RGB color image from the bayer tile image.
Definition: bumblebee2.cpp:367
virtual void close()
Close camera.
Definition: firewire.cpp:373
Camera argument parser.
Definition: camargp.h:38
dc1394video_frame_t * _frame
Last captured DC1394 video frame.
Definition: firewire.h:147
Camera not opened exception.
static const unsigned int ORIGINAL
Original image in RAW16.
Definition: bumblebee2.h:41
virtual uint32_t serial_no() const
Get BB2 serial no.
Definition: bumblebee2.cpp:188
virtual void parse_set_white_balance(const char *white_balance)
Parse white balance and set value.
Definition: firewire.cpp:751
bool is_bumblebee2()
Check if connected camera is a Bumblebee2.
Definition: bumblebee2.cpp:344
virtual void capture()
Capture an image.
Definition: bumblebee2.cpp:306
virtual uint64_t guid() const
Get Firewire GUID of camera.
Definition: firewire.cpp:291
virtual void parse_set_shutter(const char *shutter)
Parse shutter and set value.
Definition: firewire.cpp:793
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual void print_info()
Print out camera information.
Definition: bumblebee2.cpp:230
virtual void close()
Close camera.
Definition: bumblebee2.cpp:290
char * _model
Camera model, used in open to identify the camera, if empty first found camera is used...
Definition: firewire.h:178
int _format7_height
Format7 height.
Definition: firewire.h:155
void deinterlace_stereo()
De-interlace the 16 bit data into 2 bayer tile pattern images.
Definition: bumblebee2.cpp:354
dc1394color_coding_t _format7_coding
Format7 color coding.
Definition: firewire.h:149
virtual void open()
Open the camera.
Definition: firewire.cpp:108
virtual bool verify_guid(uint64_t ver_guid) const
Verify GUID validity.
Definition: bumblebee2.cpp:212
dc1394camera_t * _camera
DC1394 camera handle.
Definition: firewire.h:145
virtual void capture()
Capture an image.
Definition: firewire.cpp:316
int _format7_startx
Format7 ROI Start X coordinate.
Definition: firewire.h:157
int _num_buffers
Number of DMA buffers.
Definition: firewire.h:114
Firewire camera.
Definition: firewire.h:44
void append(const char *format,...)
Append messages to the message list.
Definition: exception.cpp:341
bool has(std::string s) const
Check if an parameter was given.
Definition: camargp.cpp:152
std::string cam_id() const
Get camera ID.
Definition: camargp.cpp:139
virtual unsigned char * buffer()
Get access to current image buffer.
Definition: bumblebee2.cpp:322
int _format7_starty
Format7 ROI Start Y coordinate.
Definition: firewire.h:159