24 #include "bumblebee2.h"
26 #include <core/exception.h>
27 #include <fvcams/cam_exceptions.h>
28 #include <fvutils/system/camargp.h>
36 # include <sys/endian.h>
37 #elif defined(__MACH__) && defined(__APPLE__)
38 # include <sys/_endian.h>
43 #include <dc1394/register.h>
44 #include <dc1394/utils.h>
45 #include <utils/math/angle.h>
51 namespace firevision {
84 const unsigned int Bumblebee2Camera::ORIGINAL = 0;
87 const unsigned int Bumblebee2Camera::DEINTERLACED = 1;
90 const unsigned int Bumblebee2Camera::RGB_IMAGE = 2;
94 #define PGR_BAYER_TILE_MAPPING_REGISTER (0x1040)
96 #define PGR_SENSOR_BOARD_INFO_REGISTER (0x1f28)
99 #define PGR_REG_CONFIG_LENGTH (0x1FFC)
102 #define PGR_REG_CONFIG_DATA (0x2000)
105 #define PGR_REG_UNIT_DIRECTORY_OFFSET (0x0424)
108 #define PGR_REG_IMAGE_DATA_FORMAT (0x1048)
110 #define PTG_Y16_Data_Format_PGR_specific (0xFFFFFFFE)
113 #define PGR_REG_SERIAL_NUMBER (0x1F20)
128 DC1394_VIDEO_MODE_FORMAT7_3,
129 DC1394_ISO_SPEED_400,
134 _supports_color =
true;
135 _auto_acquire_sensor_info =
false;
144 if (cap->
has(
"nbufs")) {
147 if (cap->
has(
"width")) {
150 if (cap->
has(
"height")) {
153 if (cap->
has(
"startx")) {
156 if (cap->
has(
"starty")) {
159 if (cap->
has(
"focus")) {
162 if (cap->
has(
"white_balance")) {
165 if (cap->
has(
"shutter")) {
169 buffer_deinterlaced_ = NULL;
179 DC1394_VIDEO_MODE_FORMAT7_3,
180 DC1394_ISO_SPEED_400,
183 _auto_acquire_sensor_info =
true;
185 _model = strdup(
"Bumblebee2");
196 if (buffer_deinterlaced_ != NULL)
197 free(buffer_deinterlaced_);
198 if (buffer_rgb_ != NULL)
212 dc1394error_t err = dc1394_get_control_register(
_camera, PGR_REG_SERIAL_NUMBER, &value);
213 if (err != DC1394_SUCCESS) {
215 "Bumblebee2::serial_no: dc1394_get_control_register(PGR_REG_SERIAL_NUMBER) failed\n");
236 uint64_t tguid = ver_guid;
239 if (tguid == 0xFFFFFFFF) {
241 ver_guid &= 0xFFFFFFFF;
244 return (
guid() == ver_guid);
249 Bumblebee2Camera::get_sensor_info()
255 err = dc1394_get_control_register(
_camera, PGR_SENSOR_BOARD_INFO_REGISTER, &value);
256 if (err != DC1394_SUCCESS) {
257 throw Exception(
"Failed to read sensor borad info register");
260 unsigned char ucSensorInfo = 0xf & value;
262 switch (ucSensorInfo) {
265 throw Exception(
"Illegal sensor board info detected!");
267 _supports_color =
true;
272 _supports_color =
false;
277 _supports_color =
true;
282 _supports_color =
false;
287 _supports_color =
true;
292 _supports_color =
false;
305 #if (defined(__WORDSIZE) && __WORDSIZE == 64) || (defined(LONG_BIT) && LONG_BIT == 64)
306 printf(
"GUID: 0x%016lx\n", (
long unsigned int)
guid());
308 printf(
"GUID: 0x%016llx\n",
guid());
316 dc1394camera_list_t *list;
318 if (dc1394_camera_enumerate(
_dc1394, &list) != DC1394_SUCCESS) {
319 throw Exception(
"Could not enumerate cameras");
324 for (
unsigned int i = 0; i < list->num; ++i) {
325 dc1394camera_t *tmpcam = dc1394_camera_new(
_dc1394, list->ids[i].guid);
326 if (strncmp(
"Bumblebee2", tmpcam->model, strlen(
"Bumblebee2")) == 0) {
331 dc1394_camera_free(tmpcam);
347 if (_auto_acquire_sensor_info) {
354 throw Exception(
"Bumblebee2Camera::open: FirewireCamera::open dit not suceed");
358 buffer_deinterlaced_ = (
unsigned char *)malloc(
buffer_size);
362 #if BYTE_ORDER_ == LITTLE_ENDIAN_
368 uint32_t presence : 1;
369 uint32_t reserved1 : 21;
371 uint32_t bayer_mono : 1;
372 uint32_t reserved2 : 7;
373 uint32_t data_format : 1;
377 err = dc1394_get_control_register(
_camera, PGR_REG_IMAGE_DATA_FORMAT, &(value.value));
378 if (err != DC1394_SUCCESS) {
379 throw Exception(
"Bumblebee2::open: dc1394_get_control_register(PGR_REG_DATA_FORMAT) failed\n");
381 value.value &= PTG_Y16_Data_Format_PGR_specific;
382 value.idf.data_format = 0;
383 err = dc1394_set_control_register(
_camera, PGR_REG_IMAGE_DATA_FORMAT, value.value);
384 if (err != DC1394_SUCCESS) {
385 throw Exception(
"Bumblebee2::open: Setting PGR-specific mode on little-endian system failed\n");
397 if (buffer_deinterlaced_ != NULL) {
398 free(buffer_deinterlaced_);
399 buffer_deinterlaced_ = NULL;
401 if (buffer_rgb_ != NULL) {
414 e.
append(
"Bumblebee2Camera::capture: failed to retrieve image");
433 image_num_ = image_num;
435 case DEINTERLACED: buffer_ = buffer_deinterlaced_;
break;
436 case RGB_IMAGE: buffer_ = buffer_rgb_;
break;
437 default: buffer_ = NULL;
break;
450 return (strncmp(
_camera->model,
"Bumblebee2", strlen(
"Bumblebee2")) == 0);
468 dc1394_bayer_decoding_8bit(buffer_deinterlaced_,
473 DC1394_BAYER_METHOD_NEAREST);
485 unsigned char *deinterlaced,
489 dc1394_deinterlace_stereo(raw16, deinterlaced, width, 2 * height);
512 bayer_pattern_t bayer_pattern)
514 dc1394color_filter_t dc_bayer_pattern;
516 switch (bayer_pattern) {
518 case BAYER_PATTERN_YYYY: dc_bayer_pattern = (dc1394color_filter_t)0;
break;
519 case BAYER_PATTERN_RGGB: dc_bayer_pattern = DC1394_COLOR_FILTER_RGGB;
break;
520 case BAYER_PATTERN_GBRG: dc_bayer_pattern = DC1394_COLOR_FILTER_GBRG;
break;
521 case BAYER_PATTERN_GRBG: dc_bayer_pattern = DC1394_COLOR_FILTER_GRBG;
break;
522 case BAYER_PATTERN_BGGR: dc_bayer_pattern = DC1394_COLOR_FILTER_BGGR;
break;
525 dc1394_bayer_decoding_8bit(
526 deinterlaced, rgb, width, 2 * height, dc_bayer_pattern, DC1394_BAYER_METHOD_NEAREST);
534 Bumblebee2Camera::get_bayer_tile()
537 if (dc1394_get_control_register(
_camera, PGR_BAYER_TILE_MAPPING_REGISTER, &value)
539 throw Exception(
"Could not query bayer tile register");
547 bayer_pattern_ = (dc1394color_filter_t)0;
550 bayer_pattern_ = DC1394_COLOR_FILTER_RGGB;
553 bayer_pattern_ = DC1394_COLOR_FILTER_GBRG;
556 bayer_pattern_ = DC1394_COLOR_FILTER_GRBG;
559 bayer_pattern_ = DC1394_COLOR_FILTER_BGGR;
577 err = dc1394_get_control_register(
_camera, PGR_REG_CONFIG_LENGTH, &value);
578 if (err != DC1394_SUCCESS) {
579 throw Exception(
"dc1394_get_control_register(PGR_REG_CONFIG_LENGTH) failed\n");
583 unsigned long file_size_bytes = value;
584 if (file_size_bytes == 0) {
588 FILE *file = fopen(filename,
"w");
590 throw Exception(
"Can't open temporary file\n");
595 for (
unsigned long offset = 0; offset < file_size_bytes; offset += 4) {
596 err = dc1394_get_control_register(
_camera, PGR_REG_CONFIG_DATA + offset, &value);
598 if (err != DC1394_SUCCESS) {
599 Exception e(
"Failed to get control register");
600 e.
append(
"Can't get control register 0x%x\n", (
int)(PGR_REG_CONFIG_DATA + offset));
605 for (
int i = 24; i >= 0; i -= 8) {
606 fputc(((value >> i) & 0xFF), file);
Base class for exceptions in Fawkes.
void append(const char *format,...) noexcept
Append messages to the message list.
virtual void capture()
Capture an image.
void deinterlace_stereo()
De-interlace the 16 bit data into 2 bayer tile pattern images.
void decode_bayer()
Extract RGB color image from the bayer tile image.
bool is_bumblebee2()
Check if connected camera is a Bumblebee2.
virtual void open_device()
Open the desired device.
virtual void close()
Close camera.
virtual void open()
Open the camera.
virtual uint32_t serial_no() const
Get BB2 serial no.
static const unsigned int RGB_IMAGE
From bayer tile decoded RGB image.
virtual bool verify_guid(uint64_t ver_guid) const
Verify GUID validity.
static const unsigned int DEINTERLACED
Deinterlaced image.
virtual void set_image_number(unsigned int image_num)
Set image number to retrieve.
virtual void print_info()
Print out camera information.
Bumblebee2Camera()
Constructor.
virtual ~Bumblebee2Camera()
Destructor.
virtual unsigned char * buffer()
Get access to current image buffer.
static const unsigned int ORIGINAL
Original image in RAW16.
void write_triclops_config_from_camera_to_file(const char *filename)
Retrieve config from camera.
std::string cam_id() const
Get camera ID.
bool has(std::string s) const
Check if an parameter was given.
std::string get(std::string s) const
Get the value of the given parameter.
Camera not opened exception.
Capturing a frame failed.
int _format7_starty
Format7 ROI Start Y coordinate.
bool _device_opened
true if device has been opened, false otherwise
virtual void print_info()
Print out camera information.
virtual void close()
Close camera.
virtual void parse_set_white_balance(const char *white_balance)
Parse white balance and set value.
virtual unsigned int pixel_width()
Width of image in pixels.
int _format7_width
Format7 width.
int _format7_startx
Format7 ROI Start X coordinate.
virtual void capture()
Capture an image.
dc1394camera_t * _camera
DC1394 camera handle.
virtual void parse_set_shutter(const char *shutter)
Parse shutter and set value.
virtual unsigned int pixel_height()
Height of image in pixels.
virtual void open()
Open the camera.
virtual unsigned int buffer_size()
Size of buffer.
char * _model
Camera model, used in open to identify the camera, if empty first found camera is used.
dc1394_t * _dc1394
DC1394 main context.
bool _opened
true if camera has been opened, false otherwise
dc1394color_coding_t _format7_coding
Format7 color coding.
virtual uint64_t guid() const
Get Firewire GUID of camera.
int _format7_height
Format7 height.
int _num_buffers
Number of DMA buffers.
virtual void parse_set_focus(const char *focus)
Parse focus and set value.
dc1394video_frame_t * _frame
Last captured DC1394 video frame.
Fawkes library namespace.