MRPT  2.0.4
CImageGrabber_FlyCapture2.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
13 #include <mrpt/system/datetime.h>
15 
16 #if MRPT_HAS_FLYCAPTURE2
17 #include <FlyCapture2.h>
18 using namespace FlyCapture2;
19 #endif
20 #if MRPT_HAS_TRICLOPS
21 #include <fc2triclops.h>
22 #include <triclops.h>
23 using namespace Fc2Triclops;
24 #endif
25 
26 #if MRPT_HAS_OPENCV
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>
31 #endif
32 
33 #define CHECK_FC2_ERROR(_err) \
34  { \
35  if (_err != PGRERROR_OK) \
36  { \
37  THROW_EXCEPTION_FMT( \
38  "FlyCapture2 error:\n%s", _err.GetDescription()); \
39  } \
40  }
41 #define CHECK_TRICLOPS_ERROR(_err) \
42  { \
43  if (_err != TriclopsErrorOk) \
44  { \
45  THROW_EXCEPTION_FMT( \
46  "Triclops Error:\n'%s'", triclopsErrorToString(_err)); \
47  } \
48  }
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)
53 
54 using namespace mrpt::hwdrivers;
55 using namespace std;
56 
57 #if MRPT_HAS_FLYCAPTURE2
58 // Declare tables to convert strings to their #define values:
59 template <typename T>
60 struct fc2_str_val
61 {
62  const char* str;
63  T val;
64 };
65 
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[] = {
92  {"FRAMERATE_1_875", FlyCapture2::FRAMERATE_1_875},
93  {"FRAMERATE_3_75", FlyCapture2::FRAMERATE_3_75},
94  {"FRAMERATE_7_5", FlyCapture2::FRAMERATE_7_5},
95  {"FRAMERATE_15", FlyCapture2::FRAMERATE_15},
96  {"FRAMERATE_30", FlyCapture2::FRAMERATE_30},
97  {"FRAMERATE_60", FlyCapture2::FRAMERATE_60},
98  {"FRAMERATE_120", FlyCapture2::FRAMERATE_120},
99  {"FRAMERATE_240", FlyCapture2::FRAMERATE_240},
100  {"FRAMERATE_FORMAT7", FlyCapture2::FRAMERATE_FORMAT7}};
101 fc2_str_val<GrabMode> fc2_GrabMode_table[] = {{"DROP_FRAMES", DROP_FRAMES},
102  {"BUFFER_FRAMES", BUFFER_FRAMES}};
103 
104 #define GET_CONV_TABLE(type) \
105  vector<fc2_str_val<type>> fc2_vals_gen(type) \
106  { \
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]); \
110  return vec; \
111  }
112 GET_CONV_TABLE(VideoMode)
113 GET_CONV_TABLE(FrameRate)
114 GET_CONV_TABLE(GrabMode)
115 
116 template <typename T>
117 T fc2_defstr2num(const std::string& str)
118 {
119  vector<fc2_str_val<T>> fc2_vals = fc2_vals_gen(T());
120  const std::string s = mrpt::system::trim(str);
121  for (size_t i = 0; i < fc2_vals.size(); i++)
122  {
123  if (mrpt::system::strCmpI(fc2_vals[i].str, s.c_str()))
124  return fc2_vals[i].val;
125  }
126  THROW_EXCEPTION_FMT("Error: Unknown FlyCapture2 constant: %s", s.c_str());
127 }
128 
129 template <typename T>
130 const char* fc2_defnum2str(const T& val)
131 {
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;
136  else
138  "Error: Unknown FlyCapture2 enum: %i", static_cast<int>(val));
139 }
140 #endif
141 
142 // Options: TCaptureOptions_bumblebee
143 // -------------------------------------------------------------
144 TCaptureOptions_FlyCapture2::TCaptureOptions_FlyCapture2()
145  : videomode(), //("VIDEOMODE_640x480Y8"),
146  framerate(), // ("FRAMERATE_30"),
147  grabmode("BUFFER_FRAMES")
148 
149 {
150  memset(camera_guid, 0, 4 * sizeof(camera_guid[0]));
151 }
152 
154  const mrpt::config::CConfigFileBase& cfg, const std::string& sect,
155  const std::string& prefix)
156 {
157  camera_index =
158  cfg.read_int(sect, prefix + string("camera_index"), camera_index);
159  open_by_guid =
160  cfg.read_bool(sect, prefix + string("open_by_guid"), open_by_guid);
161 
162  if (open_by_guid)
163  {
164  string sGUID =
165  cfg.read_string(sect, prefix + string("camera_guid"), "", true);
166  vector<string> sGUIDparts;
167  mrpt::system::tokenize(sGUID, "- \t\r\n", sGUIDparts);
168  ASSERTMSG_(
169  sGUIDparts.size() == 4,
170  "GUID format error: must have four blocks like XXX-XXX-XXX-XXX");
171 
172  for (int i = 0; i < 4; i++)
173  sscanf(sGUIDparts[i].c_str(), "%X", &camera_guid[i]);
174  }
175 
176  videomode = cfg.read_string(sect, prefix + string("videomode"), videomode);
177  framerate = cfg.read_string(sect, prefix + string("framerate"), framerate);
178  grabmode = cfg.read_string(sect, prefix + string("grabmode"), grabmode);
179  numBuffers =
180  cfg.read_uint64_t(sect, prefix + string("numBuffers"), numBuffers);
181  grabTimeout =
182  cfg.read_int(sect, prefix + string("grabTimeout"), grabTimeout);
183 
185  sect, prefix + string("trigger_enabled"), trigger_enabled);
187  sect, prefix + string("trigger_polarity"), trigger_polarity);
189  cfg.read_int(sect, prefix + string("trigger_source"), trigger_source);
190  trigger_mode =
191  cfg.read_int(sect, prefix + string("trigger_mode"), trigger_mode);
192 
194  cfg.read_bool(sect, prefix + string("strobe_enabled"), strobe_enabled);
195  strobe_source =
196  cfg.read_int(sect, prefix + string("strobe_source"), strobe_source);
198  cfg.read_int(sect, prefix + string("strobe_polarity"), strobe_polarity);
199  strobe_delay =
200  cfg.read_float(sect, prefix + string("strobe_delay"), strobe_delay);
202  sect, prefix + string("strobe_duration"), strobe_duration);
203 
205  sect, prefix + string("autoexposure_auto"), autoexposure_auto);
207  sect, prefix + string("autoexposure_onOFf"), autoexposure_onOff);
209  sect, prefix + string("autoexposure_abs"), autoexposure_abs);
211  sect, prefix + string("autoexposure_EV"), autoexposure_EV);
212 
213  shutter_auto =
214  cfg.read_bool(sect, prefix + string("shutter_auto"), shutter_auto);
215  shutter_abs =
216  cfg.read_bool(sect, prefix + string("shutter_abs"), shutter_abs);
218  sect, prefix + string("shutter_time_ms"), shutter_time_ms);
219 
220  gain_auto = cfg.read_bool(sect, prefix + string("gain_auto"), gain_auto);
221  gain_abs = cfg.read_bool(sect, prefix + string("gain_abs"), gain_abs);
222  gain_dB = cfg.read_float(sect, prefix + string("gain_dB"), gain_dB);
223 
224  stereo_mode =
225  cfg.read_bool(sect, prefix + string("stereo_mode"), stereo_mode);
226  rect_width =
227  cfg.read_uint64_t(sect, prefix + string("rect_width"), rect_width);
228  rect_height =
229  cfg.read_uint64_t(sect, prefix + string("rect_height"), rect_height);
230 }
231 
232 // ---------------------------------------------------------------
233 /** Default constructor */
235 {
236 #if MRPT_HAS_FLYCAPTURE2
237  m_img_buffer = new FlyCapture2::Image();
238 #endif
239 }
240 
241 /** Constructor + open */
243  const TCaptureOptions_FlyCapture2& options)
244  : m_camera(nullptr),
245  m_camera_info(nullptr),
246  m_img_buffer(nullptr),
247  m_triclops(nullptr)
248 {
249 #if MRPT_HAS_FLYCAPTURE2
250  m_img_buffer = new FlyCapture2::Image();
251 #endif
252  this->open(options);
253 }
254 
255 /** Destructor */
257 {
258 #if MRPT_HAS_FLYCAPTURE2
259  this->close();
260  delete FC2_BUF_IMG;
261  m_img_buffer = nullptr;
262 #endif
263 }
264 
265 /** Tries to open the camera with the given options. Raises an exception on
266  * error. \sa close() */
268  const TCaptureOptions_FlyCapture2& options, const bool startCapture)
269 {
270 #if MRPT_HAS_FLYCAPTURE2
271  FlyCapture2::Error fe;
272 
273  cout << "[CImageGrabber_FlyCapture2::open] FlyCapture2 version: "
275 
276  this->close();
277  this->m_options = options;
278 
279  // Determine camera to open:
280  // -----------------------------------
281  PGRGuid guid;
283  {
284  // Open by GUID:
285  for (int i = 0; i < 4; i++) guid.value[i] = m_options.camera_guid[i];
286  }
287  else
288  {
289  // Open by camera index:
290  BusManager busMgr;
291  unsigned int numCameras;
292  fe = busMgr.GetNumOfCameras(&numCameras);
293  CHECK_FC2_ERROR(fe)
294 
295  if (m_options.camera_index >= numCameras)
297  "Error: camera_index to open is '%u', but only '%u' "
298  "cameras were detected in the system.",
299  m_options.camera_index, numCameras));
300 
301  fe = busMgr.GetCameraFromIndex(m_options.camera_index, &guid);
302  CHECK_FC2_ERROR(fe)
303  }
304 
305  // Connect to camera:
306  m_camera = new FlyCapture2::Camera();
307  m_camera_info = new FlyCapture2::CameraInfo();
308 
309  cout << mrpt::format(
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]);
313  fe = FC2_CAM->Connect(&guid);
314  CHECK_FC2_ERROR(fe)
315  fe = FC2_CAM->GetCameraInfo(FC2_CAM_INFO);
316  CHECK_FC2_ERROR(fe)
317 
318  const FlyCapture2::CameraInfo* ci = FC2_CAM_INFO;
319 
320  cout << mrpt::format(
321  "[CImageGrabber_FlyCapture2::open] Camera connected ok:\n"
322  " Serial number - %u\n"
323  " Camera model - %s\n"
324  " Camera vendor - %s\n"
325  " Sensor - %s\n"
326  " Resolution - %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);
331 
332  // Set camera config:
333  if (!m_options.videomode.empty() && !m_options.framerate.empty())
334  {
335  bool isSupported = false;
336 
337  if (!m_options.stereo_mode)
338  {
339  FlyCapture2::VideoMode vidMode =
340  fc2_defstr2num<FlyCapture2::VideoMode>(m_options.videomode);
341  FlyCapture2::FrameRate vidRate =
342  fc2_defstr2num<FlyCapture2::FrameRate>(m_options.framerate);
343 
344  fe = FC2_CAM->GetVideoModeAndFrameRateInfo(
345  vidMode, vidRate, &isSupported);
346  CHECK_FC2_ERROR(fe)
347 
348  if (!isSupported)
349  {
350  FlyCapture2::VideoMode curVidMode;
351  FlyCapture2::FrameRate curVidRate;
352  fe =
353  FC2_CAM->GetVideoModeAndFrameRate(&curVidMode, &curVidRate);
354 
356  "Camera mode '%s' + '%s' is not supported by this "
357  "camera. Current mode is %d, current rate is %d.",
358  m_options.videomode.c_str(), m_options.framerate.c_str(),
359  static_cast<int>(curVidMode),
360  static_cast<int>(curVidRate)));
361  }
362 
363  fe = FC2_CAM->SetVideoModeAndFrameRate(vidMode, vidRate);
364  CHECK_FC2_ERROR(fe)
365  }
366  else
367  {
368 #if MRPT_HAS_TRICLOPS
369  Fc2Triclops::ErrorType fte;
370  // Configure camera for Stereo mode
371  StereoCameraMode mode = TWO_CAMERA;
372  fte = setStereoMode(*(FC2_CAM), mode);
373  if (fte) handleFc2TriclopsError(fte, "setStereoMode");
374 
375  // Generate Triclops context
376 
377  m_triclops = new TriclopsContext;
378  fte = getContextFromCamera(FC2_CAM_INFO->serialNumber, TRI_CONTEXT);
379  if (fte != ERRORTYPE_OK)
380  handleFc2TriclopsError(fte, "getContextFromCamera");
381 
382  // ------------------------------------------------------
383  // TRICLOPS CONFIGURATION
384  // ------------------------------------------------------
385  // Current Format7 settings
386  /*
387  Format7ImageSettings f7settings;
388  unsigned int f7PacketSize;
389  float f7Percentage;
390  fe = FC2_CAM->GetFormat7Configuration(&f7settings, &f7PacketSize,
391  &f7Percentage);
392  CHECK_FC2_ERROR(fe)
393  */
394 
395  TriclopsError te;
396  // Set rectified resolution
397  te = triclopsSetResolution(
400  // Retrieve camera parameters
401  te = triclopsGetBaseline(*(TRI_CONTEXT), &m_baseline);
403  te = triclopsGetFocalLength(*(TRI_CONTEXT), &m_focalLength);
405  te = triclopsGetImageCenter(
408 #else
409  THROW_EXCEPTION("MRPT compiled without support for Triclops");
410 #endif
411  }
412  }
413 
414  {
415  FlyCapture2::VideoMode curVidMode;
416  FlyCapture2::FrameRate curVidRate;
417  fe = FC2_CAM->GetVideoModeAndFrameRate(&curVidMode, &curVidRate);
418  if (fe == PGRERROR_OK)
419  cout << mrpt::format(
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));
424  }
425 
426  // Set trigger:
427  FlyCapture2::TriggerModeInfo trigInfo;
428  FC2_CAM->GetTriggerModeInfo(&trigInfo);
429 
430  FlyCapture2::TriggerMode trig;
431  trig.onOff = m_options.trigger_enabled;
433  {
434  trig.mode = m_options.trigger_mode;
435  trig.polarity = m_options.trigger_polarity;
436  trig.source = m_options.trigger_source;
437  }
438  fe = FC2_CAM->SetTriggerMode(&trig);
439  CHECK_FC2_ERROR(fe)
440 
441  // Strobe:
443  {
444  FlyCapture2::StrobeControl strobe;
445 
446  strobe.onOff = m_options.strobe_enabled;
447  strobe.delay = m_options.strobe_delay;
448  strobe.duration = m_options.strobe_duration;
449  strobe.polarity = m_options.strobe_polarity;
450  strobe.source = m_options.strobe_source;
451 
452  fe = FC2_CAM->SetStrobe(&strobe);
453  CHECK_FC2_ERROR(fe)
454  }
455 
456  // Set configs:
457  FlyCapture2::FC2Config fc2conf;
458  FC2_CAM->GetConfiguration(&fc2conf);
459  CHECK_FC2_ERROR(fe)
460 
461  fc2conf.grabMode =
462  fc2_defstr2num<FlyCapture2::GrabMode>(m_options.grabmode);
463  if (m_options.grabTimeout >= 0) fc2conf.grabTimeout = m_options.grabTimeout;
464 
465  fc2conf.numBuffers = m_options.numBuffers;
466 
467  fe = FC2_CAM->SetConfiguration(&fc2conf);
468  CHECK_FC2_ERROR(fe)
469 
470  // Autoexposure:
471  {
472  FlyCapture2::Property p;
473  p.type = FlyCapture2::AUTO_EXPOSURE;
474  FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
476  p.autoManualMode = m_options.autoexposure_auto; // true=auto
477  p.onOff = m_options.autoexposure_onOff; // true=on
478  p.absControl = m_options.autoexposure_abs; // true=abs
479  p.absValue =
480  m_options.autoexposure_EV; // abs value in Exposure Value (EV)
481  fe = FC2_CAM->SetProperty(&p);
482  CHECK_FC2_ERROR(fe)
483  }
484 
485  // Brightness:
486  {
487  FlyCapture2::Property p;
488  p.type = FlyCapture2::BRIGHTNESS;
489  FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
491  p.autoManualMode = true; // true=auto
492  // p.absControl = true;
493  // p.absValue = Brightness;
494  fe = FC2_CAM->SetProperty(&p);
495  CHECK_FC2_ERROR(fe)
496  }
497 
498  // Shutter:
499  {
500  FlyCapture2::Property p;
501  p.type = FlyCapture2::SHUTTER;
502  FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
504  p.autoManualMode = m_options.shutter_auto; // true=auto
505  p.absControl = m_options.shutter_abs; // true=abs
506  p.absValue = m_options.shutter_time_ms;
507  // p.onOff = false;
508  fe = FC2_CAM->SetProperty(&p);
509  CHECK_FC2_ERROR(fe)
510  }
511 
512  // Gain:
513  {
514  FlyCapture2::Property p;
515  p.type = FlyCapture2::GAIN;
516  FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
518  p.autoManualMode = m_options.gain_auto; // true=auto
519  p.absControl = m_options.gain_abs; // true=abs
520  p.absValue = m_options.gain_dB; // abs value in dB (decibeles)
521  // p.onOff = false;
522  fe = FC2_CAM->SetProperty(&p);
523  CHECK_FC2_ERROR(fe)
524  }
525 
526  // Framecounter:
527  EmbeddedImageInfo eii;
528  fe = FC2_CAM->GetEmbeddedImageInfo(&eii);
529  if (fe == PGRERROR_OK)
530  {
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;
535 
536  // Enable all:
537  FC2_CAM->SetEmbeddedImageInfo(&eii);
538  }
539 
540  // Start:
541  if (startCapture) this->startCapture();
542 #else
543  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
544 #endif
545 }
546 
547 /** Start the actual image capture of the camera. Must be called after open(),
548  * only when "startCapture" was set to false. */
550 {
551 #if MRPT_HAS_FLYCAPTURE2
552  if (!m_camera)
553  {
554  THROW_EXCEPTION("Camera is not opened. Call open() first.");
555  }
556 
557  FlyCapture2::Error error = FC2_CAM->StartCapture();
559 
560 #else
561  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
562 #endif
563 }
564 
565 /** Starts a synchronous capture of several cameras, which must have been
566  * already opened. */
568  int numCameras, const CImageGrabber_FlyCapture2** cameras_array)
569 {
570 #if MRPT_HAS_FLYCAPTURE2
571 
572  std::vector<const FlyCapture2::Camera*> cam_ptrs(numCameras);
573 
574  for (int i = 0; i < numCameras; i++)
575  {
576  const CImageGrabber_FlyCapture2* obj = cameras_array[i];
577  if (!obj->m_camera)
578  {
580  "Camera #%i in list is not opened. Call open() first.", i);
581  }
582 
583  FlyCapture2::Camera* cam =
584  reinterpret_cast<FlyCapture2::Camera*>(obj->m_camera);
585  cam_ptrs[i] = cam;
586  }
587 
588  if (!cam_ptrs.empty())
589  {
590  FlyCapture2::Error error = FlyCapture2::Camera::StartSyncCapture(
591  cam_ptrs.size(), &cam_ptrs[0]);
593  }
594 #else
595  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
596 #endif
597 }
598 
599 /** Stop capture. */
601 {
602 #if MRPT_HAS_FLYCAPTURE2
603  if (m_camera)
604  {
605  Error error;
606 
607  // Stop grabbing:
608  error = FC2_CAM->StopCapture();
610  }
611 #else
612  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
613 #endif
614 }
615 
616 /** Closes the opened camera, if any. Called automatically on object
617  * destruction. */
619 {
620 #if MRPT_HAS_FLYCAPTURE2
621  try
622  {
623  this->stopCapture();
624  }
625  catch (...)
626  {
627  }
628 
629  // Disconnect the camera
630  try
631  {
632  if (m_camera) FC2_CAM->Disconnect();
633  }
634  catch (...)
635  {
636  }
637 
638  // Delete objects:
639  try
640  {
641  if (m_camera) delete FC2_CAM;
642  }
643  catch (...)
644  {
645  }
646  try
647  {
648  if (m_camera_info) delete FC2_CAM_INFO;
649  }
650  catch (...)
651  {
652  }
653 #if MRPT_HAS_TRICLOPS
654  try
655  {
656  if (m_triclops) delete TRI_CONTEXT;
657  }
658  catch (...)
659  {
660  }
661 #endif
662 
663  m_camera = nullptr;
664  m_camera_info = nullptr;
665 
666 #else
667  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
668 #endif
669 }
670 
671 /** Returns the PGR FlyCapture2 library version */
673 {
674 #if MRPT_HAS_FLYCAPTURE2
675  FlyCapture2::FC2Version fc2Version;
676  FlyCapture2::Utilities::GetLibraryVersion(&fc2Version);
677  return mrpt::format(
678  "%d.%d.%d.%d", fc2Version.major, fc2Version.minor, fc2Version.type,
679  fc2Version.build);
680 #else
681  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
682 #endif
683 }
684 
685 /*-------------------------------------------------------------
686  get the image - MONO
687  -------------------------------------------------------------*/
688 // Grab image from the camera. This method blocks until the next frame is
689 // captured.
690 // return: false on any error.
692  mrpt::obs::CObservationImage& out_observation)
693 {
694 #if MRPT_HAS_FLYCAPTURE2
695  if (!m_camera)
696  {
697  std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Camera is "
698  "not opened. Call open() first.\n";
699  return false;
700  }
701  try
702  {
703  FlyCapture2::Error error;
704  FlyCapture2::Image image;
705  error = FC2_CAM->RetrieveBuffer(&image);
707 
708  FlyCapture2::TimeStamp timestamp = image.GetTimeStamp();
709  // White balance, etc.
710  // FlyCapture2::ImageMetadata imd = image.GetMetadata();
711  // Determine if it's B/W or color:
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;
720  // Decode image:
721  error = image.Convert(
722  is_color ? PIXEL_FORMAT_BGR : PIXEL_FORMAT_MONO8, FC2_BUF_IMG);
724  // Convert PGR FlyCapture2 image ==> OpenCV format:
725  unsigned int img_rows, img_cols, img_stride;
726  FC2_BUF_IMG->GetDimensions(&img_rows, &img_cols, &img_stride);
727  out_observation.image.loadFromMemoryBuffer(
728  img_cols, img_rows, is_color, FC2_BUF_IMG->GetData());
729  // It seems timestamp is not always correctly filled in the incoming
730  // imgs:
731  if (timestamp.seconds != 0)
732  out_observation.timestamp = mrpt::system::time_tToTimestamp(
733  timestamp.seconds + 1e-6 * timestamp.microSeconds);
734  else
735  out_observation.timestamp = mrpt::system::now();
736  return true;
737  }
738  catch (const std::exception& e)
739  {
740  std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Error:\n"
741  << e.what() << std::endl;
742  return false;
743  }
744 #else
745  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
746 #endif
747 }
748 
749 /*-------------------------------------------------------------
750  get the image - STEREO
751  -------------------------------------------------------------*/
752 // Grab image from the camera. This method blocks until the next frame is
753 // captured.
754 // return: false on any error.
756  mrpt::obs::CObservationStereoImages& out_observation)
757 {
758 #if MRPT_HAS_FLYCAPTURE2 && MRPT_HAS_TRICLOPS && MRPT_HAS_OPENCV
759  if (!m_camera)
760  {
761  std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Camera is "
762  "not opened. Call open() first.\n";
763  return false;
764  }
765 
766  try
767  {
768  FlyCapture2::Error ferr;
769  Fc2Triclops::ErrorType fterr;
770  FlyCapture2::Image image;
771  ferr = FC2_CAM->RetrieveBuffer(&image);
772  CHECK_FC2_ERROR(ferr)
774  FlyCapture2::TimeStamp timestamp = image.GetTimeStamp();
775 
776  // White balance, etc.
777  // FlyCapture2::ImageMetadata imd = image.GetMetadata();
778 
779  // ------------------------------------------
780  // Extract images from common interleaved image:
781  // ------------------------------------------
782  FlyCapture2::Image rawImage[2];
783 
784  // Convert the pixel interleaved raw data to de-interleaved raw data
785  fterr = Fc2Triclops::unpackUnprocessedRawOrMono16Image(
786  image, true,
787  rawImage[0], // Right image
788  rawImage[1]); // Left image
789  if (fterr != Fc2Triclops::ERRORTYPE_OK)
790  {
791  Fc2Triclops::handleFc2TriclopsError(
792  fterr, "unprocessedRawOrMono16Image()");
793  return false;
794  }
795 
796  // Convert each raw image to RGBU image (for color images)
797  unsigned int img_rows, img_cols, img_stride;
798  for (int i = 0; i < 2; ++i)
799  {
800  FlyCapture2::Image rgbuImage;
801  ferr = rawImage[i].SetColorProcessing(FlyCapture2::HQ_LINEAR);
802  CHECK_FC2_ERROR(ferr)
803  ferr = rawImage[i].Convert(PIXEL_FORMAT_BGR, &rgbuImage);
804  CHECK_FC2_ERROR(ferr)
805 
806  // get unrectified images:
807  rgbuImage.GetDimensions(&img_rows, &img_cols, &img_stride);
808  uint8_t* ptrData = rgbuImage.GetData();
809 
810  // Convert PGR image ==> OpenCV format:
811  auto tmpImg =
812  cv::Mat(img_rows, img_cols, CV_8UC3, ptrData, img_stride);
813 
814  // Fill output stereo observation
815  auto& im =
816  (i == 0 ? out_observation.imageRight
817  : out_observation.imageLeft);
819  }
820 
821  out_observation.hasImageRight = true;
822 
823  // It seems timestamp is not always correctly filled in the incoming
824  // imgs:
825  if (timestamp.seconds != 0)
826  out_observation.timestamp = mrpt::system::time_tToTimestamp(
827  timestamp.seconds + 1e-6 * timestamp.microSeconds);
828  else
829  out_observation.timestamp = ts_retrieved;
830 
831  out_observation.rightCameraPose.x(m_baseline);
832  out_observation.rightCameraPose.y(0);
833  out_observation.rightCameraPose.z(0);
834 
835  out_observation.rightCameraPose.quat().r(1);
836  out_observation.rightCameraPose.quat().x(0);
837  out_observation.rightCameraPose.quat().y(0);
838  out_observation.rightCameraPose.quat().z(0);
839 
840  out_observation.cameraPose.x(0);
841  out_observation.cameraPose.y(0);
842  out_observation.cameraPose.z(0);
843 
844  out_observation.cameraPose.quat().r(1);
845  out_observation.cameraPose.quat().x(0);
846  out_observation.cameraPose.quat().y(0);
847  out_observation.cameraPose.quat().z(0);
848 
853  return true;
854  }
855  catch (const std::exception& e)
856  {
857  std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Error:\n"
858  << e.what() << std::endl;
859  return false;
860  }
861 #else
863  "MRPT compiled without support for FlyCapture2, Triclops or OpenCV");
864 #endif
865 }
void close()
Stop capture and closes the opened camera, if any.
bool hasImageRight
Whether imageRight actually contains data (Default upon construction: true)
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:58
bool getObservation(mrpt::obs::CObservationImage &out_observation)
Grab mono image from the camera.
Declares a class derived from "CObservation" that encapsules an image from a camera, whose relative pose to robot is also stored.
bool strobe_enabled
(default=false) Enable the generation of a strobe signal in GPIO.
bool gain_abs
(default=true) Numeric mode (absolute or integer values)
float autoexposure_EV
(default=0.0) Exposure Value, if autoexposure_auto=false
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
mrpt::img::CImage imageLeft
Image from the left camera (this image will be ALWAYS present)
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
void * m_img_buffer
Opaque pointer to the FlyCapture2::Image, used as a temporary buffer and to avoid mem alloc/reallocs...
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
A wrapper for Point Gray Research (PGR) FlyCapture2 API for capturing images from Firewire...
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:107
float m_baseline
Camera baseline (only for stereo cameras)
#define CHECK_TRICLOPS_ERROR(_err)
float strobe_duration
(default=1.0) Pulse durationin ms.
#define TRI_CONTEXT
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
Contains classes for various device interfaces.
std::string grabmode
(Default="BUFFER_FRAMES") A string with a grab mode, from the list available in FlyCapture2::GrabMode...
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
#define FC2_CAM
STL namespace.
void startCapture()
Start the actual image capture of the camera.
void tokenize(const std::string &inString, const std::string &inDelimiters, OUT_CONTAINER &outTokens, bool skipBlankTokens=true) noexcept
Tokenizes a string according to a set of delimiting characters.
CImageGrabber_FlyCapture2()
Constructor that does not open a camera.
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
#define CHECK_FC2_ERROR(_err)
mrpt::poses::CPose3DQuat cameraPose
The pose of the LEFT camera, relative to the robot.
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
int grabTimeout
(Default=5000) Time in milliseconds that RetrieveBuffer() and WaitForBufferEvent() will wait for an i...
mrpt::img::CImage CImage
Definition: utils/CImage.h:5
TCaptureOptions_FlyCapture2 m_options
Camera options.
This class allows loading and storing values and vectors of different types from a configuration text...
T r() const
Return r (real part) coordinate of the quaternion.
Definition: CQuaternion.h:101
void * m_triclops
Opaque pointer to the TriclopsContext objetc.
mrpt::img::CImage image
The image captured by the camera, that is, the main piece of information of this observation.
unsigned int trigger_source
(default=0) Refer to PGR docs.
unsigned int strobe_source
(default=0) Refer to PGR docs.
unsigned int rect_width
(default=800) Width for output rectified images
void * m_camera_info
Opaque pointer to the FlyCapture2::CameraInfo object.
std::string videomode
(Default="", which means default) A string with a video mode, from the list available in FlyCapture2:...
Observation class for either a pair of left+right or left+disparity images from a stereo camera...
int val
Definition: mrpt_jpeglib.h:957
uint64_t read_uint64_t(const std::string &section, const std::string &name, uint64_t defaultValue, bool failIfNotFound=false) const
Options used when creating a camera capture object of type CImageGrabber_FlyCapture2.
#define FC2_CAM_INFO
unsigned int rect_height
(default=600) Height for output rectified images
constexpr auto sect
bool autoexposure_abs
(default=true) Numeric mode (absolute or integer values)
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
bool stereo_mode
(default=false) Obtain images as stereo pairs with Flycapture2
bool trigger_enabled
(default=false) Enable non-free-running mode, only capturing when a given input trigger signal is det...
static void startSyncCapture(int numCameras, const CImageGrabber_FlyCapture2 **cameras_array)
Starts a synchronous capture of several cameras, which must have been already opened.
float shutter_time_ms
(default=4.0) Shutter time, if shutter_auto=false
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
bool shutter_abs
(default=true) Numeric mode (absolute or integer values)
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:105
void * m_camera
Opaque pointer to the FlyCapture2::Camera object.
static std::string getFC2version()
Returns the PGR FlyCapture2 library version.
void loadOptionsFrom(const mrpt::config::CConfigFileBase &configSource, const std::string &sectionName, const std::string &prefix=std::string())
Loads all the options from a config file.
#define FC2_BUF_IMG
unsigned int trigger_mode
(default=0) Refer to PGR docs.
std::string framerate
(Default="", which means default) A string with a framerate, from the list available in FlyCapture2::...
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
float gain_dB
(default=0.0) Sensor gain, if gain_auto=false
Deep copy: the copied object has a duplicate of all data, becoming independent.
Definition: img/CImage.h:78
mrpt::poses::CPose3DQuat rightCameraPose
The pose of the right camera, relative to the left one: Note that using the conventional reference co...
unsigned int camera_index
(Default=0) If open_by_guid==false, will open the i&#39;th camera based on this 0-based index...
std::string trim(const std::string &str)
Removes leading and trailing spaces.
unsigned int trigger_polarity
(default=0) Refer to PGR docs.
bool autoexposure_onOff
(default=true) Activate this feature
void setIntrinsicParamsFromValues(double fx, double fy, double cx, double cy)
Set the matrix of intrinsic params of the camera from the individual values of focal length and princ...
Definition: TCamera.h:111
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:109
mrpt::system::TTimeStamp time_tToTimestamp(const double t)
Transform from standard "time_t" (actually a double number, it can contain fractions of seconds) to T...
Definition: datetime.h:91
bool strCmpI(const std::string &s1, const std::string &s2)
Return true if the two strings are equal (case insensitive)
unsigned int camera_guid[4]
GUID of the camera to open, only when open_by_guid==true.
mrpt::img::TCamera leftCamera
Parameters for the left/right cameras: individual intrinsic and distortion parameters of the cameras...
unsigned int strobe_polarity
(default=0) Refer to PGR docs.
void open(const TCaptureOptions_FlyCapture2 &options, const bool startCapture=true)
Tries to open the camera with the given options, and starts capture.
mrpt::img::CImage imageRight
Image from the right camera, only contains a valid image if hasImageRight == true.
void loadFromMemoryBuffer(unsigned int width, unsigned int height, bool color, unsigned char *rawpixels, bool swapRedBlue=false)
Reads the image from raw pixels buffer in memory.
Definition: CImage.cpp:364
unsigned int numBuffers
(Default=30) Number of images that can be stored in the buffer, if enabled with grabMode.
bool open_by_guid
(Default=false) Set to true to force opening a camera by its GUID, in camera_guid ...



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020