16 #if MRPT_HAS_FLYCAPTURE2
17 #include <FlyCapture2.h>
18 using namespace FlyCapture2;
21 #include <fc2triclops.h>
23 using namespace Fc2Triclops;
27 #include <opencv2/imgproc/imgproc_c.h>
28 #include <opencv2/core/core.hpp>
29 #include <opencv2/highgui/highgui.hpp>
30 #include <opencv2/imgproc/imgproc.hpp>
33 #define CHECK_FC2_ERROR(_err) \
35 if (_err != PGRERROR_OK) \
37 THROW_EXCEPTION_FMT( \
38 "FlyCapture2 error:\n%s", _err.GetDescription()); \
41 #define CHECK_TRICLOPS_ERROR(_err) \
43 if (_err != TriclopsErrorOk) \
45 THROW_EXCEPTION_FMT( \
46 "Triclops Error:\n'%s'", triclopsErrorToString(_err)); \
49 #define FC2_CAM reinterpret_cast<FlyCapture2::Camera*>(m_camera)
50 #define FC2_CAM_INFO reinterpret_cast<FlyCapture2::CameraInfo*>(m_camera_info)
51 #define FC2_BUF_IMG reinterpret_cast<FlyCapture2::Image*>(m_img_buffer)
52 #define TRI_CONTEXT reinterpret_cast<TriclopsContext*>(m_triclops)
57 #if MRPT_HAS_FLYCAPTURE2
66 const fc2_str_val<VideoMode> fc2_VideoMode_table[] = {
67 {
"VIDEOMODE_160x120YUV444", VIDEOMODE_160x120YUV444},
68 {
"VIDEOMODE_320x240YUV422", VIDEOMODE_320x240YUV422},
69 {
"VIDEOMODE_640x480YUV411", VIDEOMODE_640x480YUV411},
70 {
"VIDEOMODE_640x480YUV422", VIDEOMODE_640x480YUV422},
71 {
"VIDEOMODE_640x480RGB", VIDEOMODE_640x480RGB},
72 {
"VIDEOMODE_640x480Y8", VIDEOMODE_640x480Y8},
73 {
"VIDEOMODE_640x480Y16", VIDEOMODE_640x480Y16},
74 {
"VIDEOMODE_800x600YUV422", VIDEOMODE_800x600YUV422},
75 {
"VIDEOMODE_800x600RGB", VIDEOMODE_800x600RGB},
76 {
"VIDEOMODE_800x600Y8", VIDEOMODE_800x600Y8},
77 {
"VIDEOMODE_800x600Y16", VIDEOMODE_800x600Y16},
78 {
"VIDEOMODE_1024x768YUV422", VIDEOMODE_1024x768YUV422},
79 {
"VIDEOMODE_1024x768RGB", VIDEOMODE_1024x768RGB},
80 {
"VIDEOMODE_1024x768Y8", VIDEOMODE_1024x768Y8},
81 {
"VIDEOMODE_1024x768Y16", VIDEOMODE_1024x768Y16},
82 {
"VIDEOMODE_1280x960YUV422", VIDEOMODE_1280x960YUV422},
83 {
"VIDEOMODE_1280x960RGB", VIDEOMODE_1280x960RGB},
84 {
"VIDEOMODE_1280x960Y8", VIDEOMODE_1280x960Y8},
85 {
"VIDEOMODE_1280x960Y16", VIDEOMODE_1280x960Y16},
86 {
"VIDEOMODE_1600x1200YUV422", VIDEOMODE_1600x1200YUV422},
87 {
"VIDEOMODE_1600x1200RGB", VIDEOMODE_1600x1200RGB},
88 {
"VIDEOMODE_1600x1200Y8", VIDEOMODE_1600x1200Y8},
89 {
"VIDEOMODE_1600x1200Y16", VIDEOMODE_1600x1200Y16},
90 {
"VIDEOMODE_FORMAT7", VIDEOMODE_FORMAT7}};
91 fc2_str_val<FrameRate> fc2_FrameRate_table[] = {
100 {
"FRAMERATE_FORMAT7", FlyCapture2::FRAMERATE_FORMAT7}};
101 fc2_str_val<GrabMode> fc2_GrabMode_table[] = {{
"DROP_FRAMES", DROP_FRAMES},
102 {
"BUFFER_FRAMES", BUFFER_FRAMES}};
104 #define GET_CONV_TABLE(type) \
105 vector<fc2_str_val<type>> fc2_vals_gen(type) \
107 size_t n = sizeof(fc2_##type##_table) / sizeof(fc2_##type##_table[0]); \
108 vector<fc2_str_val<type>> vec( \
109 &fc2_##type##_table[0], &fc2_##type##_table[n]); \
112 GET_CONV_TABLE(VideoMode)
113 GET_CONV_TABLE(FrameRate)
114 GET_CONV_TABLE(GrabMode)
116 template <
typename T>
117 T fc2_defstr2num(
const std::string& str)
119 vector<fc2_str_val<T>> fc2_vals = fc2_vals_gen(T());
121 for (
size_t i = 0; i < fc2_vals.size(); i++)
124 return fc2_vals[i].val;
129 template <
typename T>
130 const char* fc2_defnum2str(
const T&
val)
132 vector<fc2_str_val<T>> fc2_vals = fc2_vals_gen(T());
133 size_t i =
static_cast<int>(
val);
134 if (i < fc2_vals.size())
135 return fc2_vals[i].str;
138 "Error: Unknown FlyCapture2 enum: %i",
static_cast<int>(
val));
144 TCaptureOptions_FlyCapture2::TCaptureOptions_FlyCapture2()
147 grabmode(
"BUFFER_FRAMES")
155 const std::string& prefix)
166 vector<string> sGUIDparts;
169 sGUIDparts.size() == 4,
170 "GUID format error: must have four blocks like XXX-XXX-XXX-XXX");
172 for (
int i = 0; i < 4; i++)
173 sscanf(sGUIDparts[i].c_str(),
"%X", &
camera_guid[i]);
236 #if MRPT_HAS_FLYCAPTURE2
245 m_camera_info(nullptr),
246 m_img_buffer(nullptr),
249 #if MRPT_HAS_FLYCAPTURE2
258 #if MRPT_HAS_FLYCAPTURE2
270 #if MRPT_HAS_FLYCAPTURE2
271 FlyCapture2::Error fe;
273 cout <<
"[CImageGrabber_FlyCapture2::open] FlyCapture2 version: "
291 unsigned int numCameras;
292 fe = busMgr.GetNumOfCameras(&numCameras);
297 "Error: camera_index to open is '%u', but only '%u' "
298 "cameras were detected in the system.",
306 m_camera =
new FlyCapture2::Camera();
310 "[CImageGrabber_FlyCapture2::open] Opening camera with GUID= "
311 "%08X-%08X-%08X-%08X...\n",
312 guid.value[0], guid.value[1], guid.value[2], guid.value[3]);
321 "[CImageGrabber_FlyCapture2::open] Camera connected ok:\n"
322 " Serial number - %u\n"
323 " Camera model - %s\n"
324 " Camera vendor - %s\n"
327 " Firmware version - %s\n"
328 " Firmware build time - %s\n\n",
329 ci->serialNumber, ci->modelName, ci->vendorName, ci->sensorInfo,
330 ci->sensorResolution, ci->firmwareVersion, ci->firmwareBuildTime);
335 bool isSupported =
false;
339 FlyCapture2::VideoMode vidMode =
341 FlyCapture2::FrameRate vidRate =
344 fe =
FC2_CAM->GetVideoModeAndFrameRateInfo(
345 vidMode, vidRate, &isSupported);
350 FlyCapture2::VideoMode curVidMode;
351 FlyCapture2::FrameRate curVidRate;
353 FC2_CAM->GetVideoModeAndFrameRate(&curVidMode, &curVidRate);
356 "Camera mode '%s' + '%s' is not supported by this "
357 "camera. Current mode is %d, current rate is %d.",
359 static_cast<int>(curVidMode),
360 static_cast<int>(curVidRate)));
363 fe =
FC2_CAM->SetVideoModeAndFrameRate(vidMode, vidRate);
368 #if MRPT_HAS_TRICLOPS
369 Fc2Triclops::ErrorType fte;
371 StereoCameraMode mode = TWO_CAMERA;
372 fte = setStereoMode(*(
FC2_CAM), mode);
373 if (fte) handleFc2TriclopsError(fte,
"setStereoMode");
379 if (fte != ERRORTYPE_OK)
380 handleFc2TriclopsError(fte,
"getContextFromCamera");
397 te = triclopsSetResolution(
405 te = triclopsGetImageCenter(
415 FlyCapture2::VideoMode curVidMode;
416 FlyCapture2::FrameRate curVidRate;
417 fe =
FC2_CAM->GetVideoModeAndFrameRate(&curVidMode, &curVidRate);
418 if (fe == PGRERROR_OK)
420 "[CImageGrabber_FlyCapture2::open] Current camera mode is %s, "
421 "current rate is %s.\n",
422 fc2_defnum2str<FlyCapture2::VideoMode>(curVidMode),
423 fc2_defnum2str<FlyCapture2::FrameRate>(curVidRate));
427 FlyCapture2::TriggerModeInfo trigInfo;
428 FC2_CAM->GetTriggerModeInfo(&trigInfo);
430 FlyCapture2::TriggerMode trig;
438 fe =
FC2_CAM->SetTriggerMode(&trig);
444 FlyCapture2::StrobeControl strobe;
452 fe =
FC2_CAM->SetStrobe(&strobe);
457 FlyCapture2::FC2Config fc2conf;
458 FC2_CAM->GetConfiguration(&fc2conf);
467 fe =
FC2_CAM->SetConfiguration(&fc2conf);
472 FlyCapture2::Property p;
473 p.type = FlyCapture2::AUTO_EXPOSURE;
487 FlyCapture2::Property p;
488 p.type = FlyCapture2::BRIGHTNESS;
491 p.autoManualMode =
true;
500 FlyCapture2::Property p;
501 p.type = FlyCapture2::SHUTTER;
514 FlyCapture2::Property p;
515 p.type = FlyCapture2::GAIN;
527 EmbeddedImageInfo eii;
528 fe =
FC2_CAM->GetEmbeddedImageInfo(&eii);
529 if (fe == PGRERROR_OK)
531 if (eii.frameCounter.available) eii.frameCounter.onOff =
true;
532 if (eii.timestamp.available) eii.timestamp.onOff =
true;
533 if (eii.exposure.available) eii.exposure.onOff =
true;
534 if (eii.brightness.available) eii.brightness.onOff =
true;
537 FC2_CAM->SetEmbeddedImageInfo(&eii);
551 #if MRPT_HAS_FLYCAPTURE2
570 #if MRPT_HAS_FLYCAPTURE2
572 std::vector<const FlyCapture2::Camera*> cam_ptrs(numCameras);
574 for (
int i = 0; i < numCameras; i++)
580 "Camera #%i in list is not opened. Call open() first.", i);
583 FlyCapture2::Camera* cam =
584 reinterpret_cast<FlyCapture2::Camera*
>(obj->
m_camera);
588 if (!cam_ptrs.empty())
590 FlyCapture2::Error
error = FlyCapture2::Camera::StartSyncCapture(
591 cam_ptrs.size(), &cam_ptrs[0]);
602 #if MRPT_HAS_FLYCAPTURE2
620 #if MRPT_HAS_FLYCAPTURE2
653 #if MRPT_HAS_TRICLOPS
674 #if MRPT_HAS_FLYCAPTURE2
675 FlyCapture2::FC2Version fc2Version;
676 FlyCapture2::Utilities::GetLibraryVersion(&fc2Version);
678 "%d.%d.%d.%d", fc2Version.major, fc2Version.minor, fc2Version.type,
694 #if MRPT_HAS_FLYCAPTURE2
697 std::cerr <<
"[CImageGrabber_FlyCapture2::getObservation] Camera is "
698 "not opened. Call open() first.\n";
703 FlyCapture2::Error
error;
704 FlyCapture2::Image image;
708 FlyCapture2::TimeStamp timestamp = image.GetTimeStamp();
712 FlyCapture2::PixelFormat pf = image.GetPixelFormat();
713 const bool is_color =
714 pf == PIXEL_FORMAT_RGB8 || pf == PIXEL_FORMAT_RGB16 ||
715 pf == PIXEL_FORMAT_S_RGB16 || pf == PIXEL_FORMAT_RAW8 ||
716 pf == PIXEL_FORMAT_RAW16 || pf == PIXEL_FORMAT_RAW12 ||
717 pf == PIXEL_FORMAT_BGR || pf == PIXEL_FORMAT_BGRU ||
718 pf == PIXEL_FORMAT_RGBU || pf == PIXEL_FORMAT_BGR16 ||
719 pf == PIXEL_FORMAT_BGRU16 || pf == PIXEL_FORMAT_422YUV8_JPEG;
721 error = image.Convert(
722 is_color ? PIXEL_FORMAT_BGR : PIXEL_FORMAT_MONO8,
FC2_BUF_IMG);
725 unsigned int img_rows, img_cols, img_stride;
726 FC2_BUF_IMG->GetDimensions(&img_rows, &img_cols, &img_stride);
728 img_cols, img_rows, is_color,
FC2_BUF_IMG->GetData());
731 if (timestamp.seconds != 0)
733 timestamp.seconds + 1e-6 * timestamp.microSeconds);
738 catch (
const std::exception& e)
740 std::cerr <<
"[CImageGrabber_FlyCapture2::getObservation] Error:\n"
741 << e.what() << std::endl;
758 #if MRPT_HAS_FLYCAPTURE2 && MRPT_HAS_TRICLOPS && MRPT_HAS_OPENCV
761 std::cerr <<
"[CImageGrabber_FlyCapture2::getObservation] Camera is "
762 "not opened. Call open() first.\n";
768 FlyCapture2::Error ferr;
769 Fc2Triclops::ErrorType fterr;
770 FlyCapture2::Image image;
771 ferr =
FC2_CAM->RetrieveBuffer(&image);
774 FlyCapture2::TimeStamp timestamp = image.GetTimeStamp();
782 FlyCapture2::Image rawImage[2];
785 fterr = Fc2Triclops::unpackUnprocessedRawOrMono16Image(
789 if (fterr != Fc2Triclops::ERRORTYPE_OK)
791 Fc2Triclops::handleFc2TriclopsError(
792 fterr,
"unprocessedRawOrMono16Image()");
797 unsigned int img_rows, img_cols, img_stride;
798 for (
int i = 0; i < 2; ++i)
800 FlyCapture2::Image rgbuImage;
801 ferr = rawImage[i].SetColorProcessing(FlyCapture2::HQ_LINEAR);
803 ferr = rawImage[i].Convert(PIXEL_FORMAT_BGR, &rgbuImage);
807 rgbuImage.GetDimensions(&img_rows, &img_cols, &img_stride);
808 uint8_t* ptrData = rgbuImage.GetData();
812 cv::Mat(img_rows, img_cols, CV_8UC3, ptrData, img_stride);
825 if (timestamp.seconds != 0)
827 timestamp.seconds + 1e-6 * timestamp.microSeconds);
829 out_observation.
timestamp = ts_retrieved;
855 catch (
const std::exception& e)
857 std::cerr <<
"[CImageGrabber_FlyCapture2::getObservation] Error:\n"
858 << e.what() << std::endl;
863 "MRPT compiled without support for FlyCapture2, Triclops or OpenCV");