12 #include <mrpt/config.h>
16 #if MRPT_HAS_LIBDC1394_2
17 #include <dc1394/control.h>
18 #include <dc1394/conversions.h>
19 #include <dc1394/register.h>
20 #include <dc1394/utils.h>
27 #define THE_CAMERA static_cast<dc1394camera_t*>(m_dc1394camera)
28 #define THE_CONTEXT static_cast<dc1394_t*>(m_dc1394_lib_context)
33 CImageGrabber_dc1394::CImageGrabber_dc1394(
34 uint64_t cameraGUID, uint16_t cameraUnit,
40 #if MRPT_HAS_LIBDC1394_2
46 dc1394camera_list_t* list;
50 if (err != DC1394_SUCCESS)
52 cerr <<
"[CImageGrabber_dc1394] ERROR: Failed to enumerate cameras "
53 "(Maybe your user has no rights to access IEEE1394?)."
60 cerr <<
"[CImageGrabber_dc1394] ERROR: No cameras found." << endl;
71 dc1394_camera_free_list(list);
72 cerr <<
"[CImageGrabber_dc1394] ERROR: Failed to initialize camera "
74 << list->ids[0].guid <<
"\n";
81 for (uint32_t i = 0; i < list->num; i++)
83 if (list->ids[i].guid == cameraGUID &&
84 list->ids[i].unit == cameraUnit)
87 dc1394_camera_new_unit(
THE_CONTEXT, cameraGUID, cameraUnit);
90 dc1394_camera_free_list(list);
91 cerr <<
"[CImageGrabber_dc1394] ERROR: Failed to "
92 "initialize camera with GUID "
93 << list->ids[0].guid <<
"\n";
102 dc1394_camera_free_list(list);
103 cerr <<
"[CImageGrabber_dc1394] ERROR: Camera with GUID="
104 << cameraGUID <<
" and UNIT=" << cameraUnit <<
" not found.\n";
108 dc1394_camera_free_list(list);
117 dc1394video_modes_t modes;
118 err = dc1394_video_get_supported_modes(
THE_CAMERA, &modes);
119 if (err != DC1394_SUCCESS)
121 cerr <<
"[CImageGrabber_dc1394] ERROR: Could not get list of modes."
127 if (options.
mode7 >= 0)
131 cout <<
"[CImageGrabber_dc1394] Mode is mode7: " << options.
mode7
136 #define TEST_MODE(W, H, COLORMODEL) \
138 options.frame_width == W && options.frame_height == H && \
139 options.color_coding == COLOR_CODING_##COLORMODEL) m_desired_mode = \
140 DC1394_VIDEO_MODE_##W##x##H##_##COLORMODEL;
145 TEST_MODE(160, 120, YUV444)
146 TEST_MODE(320, 240, YUV422)
147 TEST_MODE(640, 480, YUV411)
148 TEST_MODE(640, 480, YUV422)
149 TEST_MODE(640, 480, RGB8)
150 TEST_MODE(640, 480, MONO8)
151 TEST_MODE(640, 480, MONO16)
152 TEST_MODE(800, 600, YUV422)
153 TEST_MODE(800, 600, RGB8)
154 TEST_MODE(800, 600, MONO8)
155 TEST_MODE(800, 600, MONO16)
156 TEST_MODE(1024, 768, YUV422)
157 TEST_MODE(1024, 768, RGB8)
158 TEST_MODE(1024, 768, MONO8)
159 TEST_MODE(1024, 768, MONO16)
160 TEST_MODE(1280, 960, YUV422)
161 TEST_MODE(1280, 960, RGB8)
162 TEST_MODE(1280, 960, MONO8)
163 TEST_MODE(1280, 960, MONO16)
164 TEST_MODE(1600, 1200, YUV422)
165 TEST_MODE(1600, 1200, RGB8)
166 TEST_MODE(1600, 1200, MONO8)
167 TEST_MODE(1600, 1200, MONO16)
170 if (
verbose) cout <<
"------ Supported video modes ------" << endl;
171 bool valid_video_mode =
false;
172 for (uint32_t i = 0; i < modes.num; i++)
175 switch (modes.modes[i])
177 case DC1394_VIDEO_MODE_160x120_YUV444:
178 mode =
"160x120_YUV444";
180 case DC1394_VIDEO_MODE_320x240_YUV422:
181 mode =
"320x240_YUV422";
183 case DC1394_VIDEO_MODE_640x480_YUV411:
184 mode =
"640x480_YUV411";
186 case DC1394_VIDEO_MODE_640x480_YUV422:
187 mode =
"640x480_YUV422";
189 case DC1394_VIDEO_MODE_640x480_RGB8:
190 mode =
"640x480_RGB8";
192 case DC1394_VIDEO_MODE_640x480_MONO8:
193 mode =
"640x480_MONO8";
195 case DC1394_VIDEO_MODE_640x480_MONO16:
196 mode =
"640x480_MONO16";
198 case DC1394_VIDEO_MODE_800x600_YUV422:
199 mode =
"800x600_YUV422";
201 case DC1394_VIDEO_MODE_800x600_RGB8:
202 mode =
"800x600_RGB8";
204 case DC1394_VIDEO_MODE_800x600_MONO8:
205 mode =
"800x600_MONO8";
207 case DC1394_VIDEO_MODE_1024x768_YUV422:
208 mode =
"1024x768_YUV422";
210 case DC1394_VIDEO_MODE_1024x768_RGB8:
211 mode =
"1024x768_RGB8";
213 case DC1394_VIDEO_MODE_1024x768_MONO8:
214 mode =
"1024x768_MONO8";
216 case DC1394_VIDEO_MODE_800x600_MONO16:
217 mode =
"800x600_MONO16";
219 case DC1394_VIDEO_MODE_1024x768_MONO16:
220 mode =
"1024x768_MONO16";
222 case DC1394_VIDEO_MODE_1280x960_YUV422:
223 mode =
"1280x960_YUV422";
225 case DC1394_VIDEO_MODE_1280x960_RGB8:
226 mode =
"1280x960_RGB8";
228 case DC1394_VIDEO_MODE_1280x960_MONO8:
229 mode =
"1280x960_MONO8";
231 case DC1394_VIDEO_MODE_1600x1200_YUV422:
232 mode =
"1600x1200_YUV422";
234 case DC1394_VIDEO_MODE_1600x1200_RGB8:
235 mode =
"1600x1200_RGB8";
237 case DC1394_VIDEO_MODE_1600x1200_MONO8:
238 mode =
"1600x1200_MONO8";
240 case DC1394_VIDEO_MODE_1280x960_MONO16:
241 mode =
"1280x960_MONO16";
243 case DC1394_VIDEO_MODE_1600x1200_MONO16:
244 mode =
"1600x1200_MONO16";
246 case DC1394_VIDEO_MODE_EXIF:
249 case DC1394_VIDEO_MODE_FORMAT7_0:
252 case DC1394_VIDEO_MODE_FORMAT7_1:
255 case DC1394_VIDEO_MODE_FORMAT7_2:
258 case DC1394_VIDEO_MODE_FORMAT7_3:
261 case DC1394_VIDEO_MODE_FORMAT7_4:
264 case DC1394_VIDEO_MODE_FORMAT7_5:
267 case DC1394_VIDEO_MODE_FORMAT7_6:
270 case DC1394_VIDEO_MODE_FORMAT7_7:
274 cerr <<
"[CImageGrabber_dc1394] ERROR: Requested video mode is "
283 cout << mode <<
" (*)" << endl;
285 cout << mode << endl;
288 if (!valid_video_mode)
291 "[CImageGrabber_dc1394] ERROR: Requested mode %ix%i "
292 "color_model:%i is not available for this camera.",
313 err = dc1394_video_set_iso_speed(
THE_CAMERA, DC1394_ISO_SPEED_400);
314 if (err != DC1394_SUCCESS)
316 cerr <<
"[CImageGrabber_dc1394] ERROR: Could not set iso speed."
324 if (err != DC1394_SUCCESS)
326 cerr <<
"[CImageGrabber_dc1394] ERROR: Could not set video mode."
331 dc1394framerate_t the_framerate;
335 the_framerate = DC1394_FRAMERATE_1_875;
338 the_framerate = DC1394_FRAMERATE_3_75;
341 the_framerate = DC1394_FRAMERATE_7_5;
344 the_framerate = DC1394_FRAMERATE_15;
347 the_framerate = DC1394_FRAMERATE_30;
350 the_framerate = DC1394_FRAMERATE_60;
353 the_framerate = DC1394_FRAMERATE_120;
356 the_framerate = DC1394_FRAMERATE_240;
360 cerr <<
"[CImageGrabber_dc1394] ERROR: Requested framerate is not "
366 err = dc1394_video_set_framerate(
THE_CAMERA, the_framerate);
367 if (err != DC1394_SUCCESS)
369 cerr <<
"[CImageGrabber_dc1394] ERROR: Could not set framerate."
374 err = dc1394_capture_setup(
375 THE_CAMERA, SIZE_RING_BUFFER, DC1394_CAPTURE_FLAGS_DEFAULT);
376 if (err != DC1394_SUCCESS)
378 cerr <<
"[CImageGrabber_dc1394] ERROR: Could not setup camera-\nmake "
379 "sure that the video mode and framerate are\nsupported by your "
385 cout <<
"------ Other options ------" << endl;
387 if ((err = dc1394_video_get_iso_channel(
THE_CAMERA, &iso_chan)) ==
389 if (
verbose) cout <<
"ISO Channel: " << iso_chan << endl;
391 dc1394speed_t iso_speed;
392 if ((err = dc1394_video_get_iso_speed(
THE_CAMERA, &iso_speed)) ==
394 if (
verbose) cout <<
"ISO Speed: " << iso_speed << endl;
397 #define SET_TRIGGER(opt, OPT, TYPE) \
398 if (options.trigger_##opt >= 0) \
400 err = dc1394_external_trigger_set_##opt( \
402 static_cast<dc1394trigger_##opt##_t>( \
403 DC1394_TRIGGER_##TYPE##_MIN + options.trigger_##opt)); \
406 "[CImageGrabber_dc1394::changeCaptureOptions] Could not set " \
409 SET_TRIGGER(mode, MODE, MODE)
410 SET_TRIGGER(source, SOURCE, SOURCE)
411 SET_TRIGGER(polarity, POLARITY, ACTIVE)
414 err = dc1394_external_trigger_set_power(
418 "[CImageGrabber_dc1394::changeCaptureOptions] Could not set "
426 err = dc1394_video_set_transmission(
THE_CAMERA, DC1394_ON);
427 if (err != DC1394_SUCCESS)
429 cerr <<
"[CImageGrabber_dc1394] ERROR: Could not start camera iso "
443 dc1394featureset_t features;
444 if ((err = dc1394_feature_get_all(
THE_CAMERA, &features)) ==
446 dc1394_feature_print_all(&features, stdout);
451 "[CImageGrabber_dc1394] ERROR: MRPT compiled with "
452 "MRPT_HAS_LIBDC1394_2=0 !");
462 #if MRPT_HAS_LIBDC1394_2
466 dc1394_video_set_transmission(
THE_CAMERA, DC1394_OFF);
497 #if MRPT_HAS_LIBDC1394_2
498 dc1394video_frame_t* frame =
nullptr;
501 err = dc1394_video_set_transmission(
THE_CAMERA, DC1394_ON);
502 if (err != DC1394_SUCCESS)
504 cerr <<
"[CImageGrabber_dc1394] ERROR: Could not start camera iso "
512 dc1394_capture_dequeue(
THE_CAMERA, DC1394_CAPTURE_POLICY_WAIT, &frame);
515 if (err != DC1394_SUCCESS)
517 cerr <<
"[CImageGrabber_dc1394] ERROR: Could not capture a frame"
524 const unsigned int width = frame->size[0];
525 const unsigned int height = frame->size[1];
535 auto* new_frame =
static_cast<dc1394video_frame_t*
>(
536 calloc(1,
sizeof(dc1394video_frame_t)));
537 new_frame->color_coding = DC1394_COLOR_CODING_RGB8;
538 dc1394_convert_frames(frame, new_frame);
542 width, height,
true, new_frame->image,
true );
545 free(new_frame->image);
551 auto* imageBuf =
new uint8_t[width * height * 2];
552 auto* imageBufRGB =
new uint8_t[width * height * 2 * 3];
554 if ((err = dc1394_deinterlace_stereo(
555 frame->image, imageBuf, width, 2 * height)) != DC1394_SUCCESS)
557 cerr <<
"[CImageGrabber_dc1394] ERROR: Could not deinterlace "
563 if ((err = dc1394_bayer_decoding_8bit(
564 imageBuf, imageBufRGB, width, 2 * height,
565 DC1394_COLOR_FILTER_GBRG,
567 DC1394_BAYER_METHOD_HQLINEAR)) != DC1394_SUCCESS)
569 cerr <<
"[CImageGrabber_dc1394] ERROR: Could not apply Bayer "
576 width, height,
true, imageBufRGB);
581 delete[] imageBufRGB;
585 err = dc1394_capture_enqueue(
THE_CAMERA, frame);
586 if (err != DC1394_SUCCESS)
588 cerr <<
"[CImageGrabber_dc1394] ERROR: Could not enqueue the ring "
596 THROW_EXCEPTION(
"The MRPT has been compiled with MRPT_HAS_LIBDC1394_2=0 !");
611 #if MRPT_HAS_LIBDC1394_2
612 dc1394video_frame_t* frame =
nullptr;
616 dc1394_capture_dequeue(
THE_CAMERA, DC1394_CAPTURE_POLICY_WAIT, &frame);
617 if (err != DC1394_SUCCESS)
619 cerr <<
"[CImageGrabber_dc1394] ERROR: Could not capture a frame"
626 const unsigned int width = frame->size[0];
627 const unsigned int height = frame->size[1];
632 "Call to getObservation(stereo) but the camera was not set as "
638 auto* imageBuf =
new uint8_t[width * height * 2];
639 auto* imageBufRGB =
new uint8_t[width * height * 2 * 3];
641 if ((err = dc1394_deinterlace_stereo(
642 frame->image, imageBuf, width, 2 * height)) != DC1394_SUCCESS)
644 cerr <<
"[CImageGrabber_dc1394] ERROR: Could not deinterlace "
650 if ((err = dc1394_bayer_decoding_8bit(
651 imageBuf, imageBufRGB, width, 2 * height,
652 DC1394_COLOR_FILTER_GBRG,
654 DC1394_BAYER_METHOD_HQLINEAR)) != DC1394_SUCCESS)
656 cerr <<
"[CImageGrabber_dc1394] ERROR: Could not apply Bayer "
663 width, height,
true, imageBufRGB);
666 imageBufRGB + width * height * 3);
669 delete[] imageBufRGB;
677 THROW_EXCEPTION(
"The MRPT has been compiled with MRPT_HAS_LIBDC1394_2=0 !");
692 #if MRPT_HAS_LIBDC1394_2
695 #define SET_MODE(feat, FEAT) \
696 if (options.feat##_mode >= 0) \
698 err = dc1394_feature_set_mode( \
699 THE_CAMERA, DC1394_FEATURE_##FEAT, \
700 static_cast<dc1394feature_mode_t>( \
701 DC1394_FEATURE_MODE_MIN + options.feat##_mode)); \
704 "[CImageGrabber_dc1394::changeCaptureOptions] Could not set feat " \
707 SET_MODE(shutter, SHUTTER)
709 SET_MODE(gamma, GAMMA)
710 SET_MODE(brightness, BRIGHTNESS)
711 SET_MODE(exposure, EXPOSURE)
712 SET_MODE(sharpness, SHARPNESS)
713 SET_MODE(white_balance, WHITE_BALANCE)
717 #define SET_VALUE(feat, FEAT) \
718 if (options.feat >= 0) \
720 err = dc1394_feature_set_value( \
721 THE_CAMERA, DC1394_FEATURE_##FEAT, options.feat); \
724 "[CImageGrabber_dc1394::changeCaptureOptions] Could not set feat " \
727 SET_VALUE(shutter, SHUTTER)
728 SET_VALUE(gain, GAIN)
729 SET_VALUE(gamma, GAMMA)
730 SET_VALUE(brightness, BRIGHTNESS)
731 SET_VALUE(exposure, EXPOSURE)
732 SET_VALUE(sharpness, SHARPNESS)
733 SET_VALUE(white_balance, WHITE_BALANCE)
738 THROW_EXCEPTION(
"The MRPT has been compiled with MRPT_HAS_LIBDC1394_2=0 !");
752 #if MRPT_HAS_LIBDC1394_2
754 err = dc1394_software_trigger_set_power(
THE_CAMERA, (dc1394switch_t)level);
757 "[CImageGrabber_dc1394::setSoftwareTriggerLevel] Could not set "
758 "software trigger level");
762 THROW_EXCEPTION(
"The MRPT has been compiled with MRPT_HAS_LIBDC1394_2=0 !");
774 #if MRPT_HAS_LIBDC1394_2
776 dc1394_t* lib_context =
nullptr;
777 dc1394camera_list_t* list =
nullptr;
782 lib_context = dc1394_new();
784 throw std::runtime_error(
785 "[CImageGrabber_dc1394] ERROR: Failed to enumerate cameras "
786 "(Maybe your user has no rights to access IEEE1394?).");
791 err = dc1394_camera_enumerate(lib_context, &list);
792 if (err != DC1394_SUCCESS)
793 throw std::runtime_error(
794 "[CImageGrabber_dc1394] ERROR: Failed to enumerate cameras "
795 "(Maybe your user has no rights to access IEEE1394?).");
797 for (
unsigned int i = 0; i < list->num; i++)
801 info.
guid = list->ids[i].guid;
802 info.
unit = list->ids[i].unit;
805 dc1394camera_t* cam = dc1394_camera_new_unit(
806 lib_context, list->ids[i].guid, list->ids[i].unit);
808 throw std::runtime_error(
format(
809 "[CImageGrabber_dc1394] ERROR: Failed to query camera "
811 static_cast<unsigned int>(list->ids[i].guid)));
823 for (
int j = 0; j < DC1394_VIDEO_MODE_FORMAT7_NUM; j++)
826 info.
vendor = std::string(cam->vendor ? cam->vendor :
"");
827 info.
model = std::string(cam->model ? cam->model :
"");
840 dc1394_camera_free(cam);
842 out_list.push_back(info);
846 dc1394_free(lib_context);
847 lib_context =
nullptr;
848 dc1394_camera_free_list(list);
853 if (list) dc1394_camera_free_list(list);
854 if (lib_context) dc1394_free(lib_context);
858 THROW_EXCEPTION(
"The MRPT has been compiled with MRPT_HAS_LIBDC1394_2=0 !");