24 #include <fvcams/bumblebee2.h>
26 #include <fvcams/cam_exceptions.h>
27 #include <core/exception.h>
28 #include <fvutils/system/camargp.h>
29 #include <fvutils/color/conversions.h>
36 # include <sys/endian.h>
37 #elif defined(__MACH__) && defined(__APPLE__)
38 # include <sys/_endian.h>
43 #include <utils/math/angle.h>
47 #include <dc1394/utils.h>
48 #include <dc1394/register.h>
49 #include <dc1394/conversions.h>
51 using namespace fawkes;
53 namespace firevision {
90 const unsigned int Bumblebee2Camera::ORIGINAL = 0;
93 const unsigned int Bumblebee2Camera::DEINTERLACED = 1;
96 const unsigned int Bumblebee2Camera::RGB_IMAGE = 2;
101 #define PGR_BAYER_TILE_MAPPING_REGISTER (0x1040)
104 #define PGR_REG_CONFIG_LENGTH (0x1FFC)
107 #define PGR_REG_CONFIG_DATA (0x2000)
110 #define PGR_REG_UNIT_DIRECTORY_OFFSET (0x0424)
113 #define PGR_REG_IMAGE_DATA_FORMAT (0x1048)
115 #define PTG_Y16_Data_Format_PGR_specific (0xFFFFFFFE)
118 #define PGR_REG_SERIAL_NUMBER (0x1F20)
133 DC1394_VIDEO_MODE_FORMAT7_3,
134 DC1394_ISO_SPEED_400,
146 if ( cap->
has(
"nbufs") ) {
149 if ( cap->
has(
"width") ) {
152 if ( cap->
has(
"height") ) {
155 if ( cap->
has(
"startx") ) {
158 if ( cap->
has(
"starty") ) {
161 if ( cap->
has(
"focus") ) {
164 if ( cap->
has(
"white_balance") ) {
167 if ( cap->
has(
"shutter") ) {
171 __buffer_deinterlaced = NULL;
179 if (__buffer_deinterlaced != NULL) free(__buffer_deinterlaced);
180 if (__buffer_rgb != NULL) free(__buffer_rgb);
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");
216 uint64_t tguid = ver_guid;
219 if ( tguid == 0xFFFFFFFF ) {
221 ver_guid &= 0xFFFFFFFF;
224 return (
guid() == ver_guid);
235 #if (defined(__WORDSIZE) && __WORDSIZE == 64) || (defined(LONG_BIT) && LONG_BIT == 64)
236 printf(
"GUID: 0x%016lx\n", (
long unsigned int)
guid());
238 printf(
"GUID: 0x%016llx\n",
guid());
252 throw Exception(
"Bumblebee2Camera::open: FirewireCamera::open dit not suceed");
259 #if __BYTE_ORDER == __LITTLE_ENDIAN
264 uint32_t presence : 1;
265 uint32_t reserved1 : 21;
267 uint32_t bayer_mono : 1;
268 uint32_t reserved2 : 7;
269 uint32_t data_format: 1;
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");
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");
294 if (__buffer_deinterlaced != NULL) {
295 free(__buffer_deinterlaced);
296 __buffer_deinterlaced = NULL;
298 if (__buffer_rgb != NULL) {
311 e.
append(
"Bumblebee2Camera::capture: failed to retrieve image");
312 if (
ORIGINAL == __image_num ) __buffer = NULL;
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;
348 return( strncmp(
_camera->model,
"Bumblebee2", strlen(
"Bumblebee2") ) == 0);
356 dc1394_deinterlace_stereo(
_frame->image, __buffer_deinterlaced,
369 dc1394_bayer_decoding_8bit( __buffer_deinterlaced, __buffer_rgb,
371 __bayer_pattern, DC1394_BAYER_METHOD_NEAREST );
387 unsigned int width,
unsigned int height)
389 dc1394_deinterlace_stereo( raw16, deinterlaced, width, 2 * height );
410 unsigned int width,
unsigned int height,
411 bayer_pattern_t bayer_pattern)
413 dc1394color_filter_t dc_bayer_pattern;
415 switch (bayer_pattern) {
417 case BAYER_PATTERN_YYYY:
418 dc_bayer_pattern = (dc1394color_filter_t) 0;
420 case BAYER_PATTERN_RGGB:
421 dc_bayer_pattern = DC1394_COLOR_FILTER_RGGB;
423 case BAYER_PATTERN_GBRG:
424 dc_bayer_pattern = DC1394_COLOR_FILTER_GBRG;
426 case BAYER_PATTERN_GRBG:
427 dc_bayer_pattern = DC1394_COLOR_FILTER_GRBG;
429 case BAYER_PATTERN_BGGR:
430 dc_bayer_pattern = DC1394_COLOR_FILTER_BGGR;
434 dc1394_bayer_decoding_8bit( deinterlaced, rgb, width, 2 * height,
435 dc_bayer_pattern, DC1394_BAYER_METHOD_NEAREST );
444 Bumblebee2Camera::get_bayer_tile()
447 if (dc1394_get_control_register(
_camera, PGR_BAYER_TILE_MAPPING_REGISTER, &value) != DC1394_SUCCESS ) {
448 throw Exception(
"Could not query bayer tile register");
456 __bayer_pattern = (dc1394color_filter_t) 0;
459 __bayer_pattern = DC1394_COLOR_FILTER_RGGB;
462 __bayer_pattern = DC1394_COLOR_FILTER_GBRG;
465 __bayer_pattern = DC1394_COLOR_FILTER_GRBG;
468 __bayer_pattern = DC1394_COLOR_FILTER_BGGR;
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");
493 unsigned long file_size_bytes = value;
494 if( file_size_bytes == 0 ) {
498 FILE* file = fopen( filename,
"w" );
500 throw Exception(
"Can't open temporary file\n" );
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,
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) );
518 for(
int i = 24; i >= 0; i -= 8 ) {
519 fputc( ( (value>>i) & 0xFF ), file );
virtual ~Bumblebee2Camera()
Destructor.
bool _opened
true if camera has been opened, false otherwise
virtual void open()
Open the camera.
int _format7_width
Format7 width.
std::string get(std::string s) const
Get the value of the given parameter.
virtual unsigned int pixel_width()
Width of image in pixels.
virtual void parse_set_focus(const char *focus)
Parse focus and set value.
virtual void set_image_number(unsigned int image_num)
Set image number to retrieve.
static const unsigned int RGB_IMAGE
From bayer tile decoded RGB image.
virtual void print_info()
Print out camera information.
virtual unsigned int pixel_height()
Height of image in pixels.
static const unsigned int DEINTERLACED
Deinterlaced image.
void write_triclops_config_from_camera_to_file(const char *filename)
Retrieve config from camera.
Capturing a frame failed.
void decode_bayer()
Extract RGB color image from the bayer tile image.
virtual void close()
Close camera.
dc1394video_frame_t * _frame
Last captured DC1394 video frame.
Camera not opened exception.
static const unsigned int ORIGINAL
Original image in RAW16.
virtual uint32_t serial_no() const
Get BB2 serial no.
virtual void parse_set_white_balance(const char *white_balance)
Parse white balance and set value.
bool is_bumblebee2()
Check if connected camera is a Bumblebee2.
virtual void capture()
Capture an image.
virtual uint64_t guid() const
Get Firewire GUID of camera.
virtual void parse_set_shutter(const char *shutter)
Parse shutter and set value.
Base class for exceptions in Fawkes.
virtual void print_info()
Print out camera information.
virtual void close()
Close camera.
char * _model
Camera model, used in open to identify the camera, if empty first found camera is used...
int _format7_height
Format7 height.
void deinterlace_stereo()
De-interlace the 16 bit data into 2 bayer tile pattern images.
dc1394color_coding_t _format7_coding
Format7 color coding.
virtual void open()
Open the camera.
virtual bool verify_guid(uint64_t ver_guid) const
Verify GUID validity.
dc1394camera_t * _camera
DC1394 camera handle.
virtual void capture()
Capture an image.
int _format7_startx
Format7 ROI Start X coordinate.
int _num_buffers
Number of DMA buffers.
void append(const char *format,...)
Append messages to the message list.
bool has(std::string s) const
Check if an parameter was given.
std::string cam_id() const
Get camera ID.
virtual unsigned char * buffer()
Get access to current image buffer.
int _format7_starty
Format7 ROI Start Y coordinate.