Main MRPT website > C++ reference for MRPT 1.9.9
CImageGrabber_FlyCapture2.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
14 #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 <triclops.h>
22 #include <fc2triclops.h>
23 using namespace Fc2Triclops;
24 #endif
25 
26 #if MRPT_HAS_OPENCV
27 #include <opencv2/core/core.hpp>
28 #include <opencv2/highgui/highgui.hpp>
29 #include <opencv2/imgproc/imgproc.hpp>
30 #include <opencv2/imgproc/imgproc_c.h>
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  : camera_index(0),
146  open_by_guid(false),
147  videomode(), //("VIDEOMODE_640x480Y8"),
148  framerate(), // ("FRAMERATE_30"),
149  grabmode("BUFFER_FRAMES"),
150  numBuffers(30),
151  grabTimeout(-1),
152  trigger_enabled(false),
153  trigger_polarity(0),
154  trigger_source(0),
155  trigger_mode(0),
156  strobe_enabled(false),
157  strobe_source(0),
158  strobe_polarity(0),
159  strobe_delay(0.0f),
160  strobe_duration(1.0f),
161  autoexposure_auto(true),
162  autoexposure_onOff(true),
163  autoexposure_abs(true),
164  autoexposure_EV(0.0f),
165  shutter_auto(true),
166  shutter_abs(true),
167  shutter_time_ms(4.0f),
168  gain_auto(true),
169  gain_abs(true),
170  gain_dB(0.0f),
171  stereo_mode(false),
172  get_rectified(false),
173  rect_width(640),
174  rect_height(480)
175 {
176  memset(camera_guid, 0, 4 * sizeof(camera_guid[0]));
177 }
178 
180  const mrpt::config::CConfigFileBase& cfg, const std::string& sect,
181  const std::string& prefix)
182 {
183  camera_index =
184  cfg.read_int(sect, prefix + string("camera_index"), camera_index);
185  open_by_guid =
186  cfg.read_bool(sect, prefix + string("open_by_guid"), open_by_guid);
187 
188  if (open_by_guid)
189  {
190  string sGUID =
191  cfg.read_string(sect, prefix + string("camera_guid"), "", true);
192  vector<string> sGUIDparts;
193  mrpt::system::tokenize(sGUID, "- \t\r\n", sGUIDparts);
194  ASSERTMSG_(
195  sGUIDparts.size() == 4,
196  "GUID format error: must have four blocks like XXX-XXX-XXX-XXX");
197 
198  for (int i = 0; i < 4; i++)
199  sscanf(sGUIDparts[i].c_str(), "%X", &camera_guid[i]);
200  }
201 
202  videomode = cfg.read_string(sect, prefix + string("videomode"), videomode);
203  framerate = cfg.read_string(sect, prefix + string("framerate"), framerate);
204  grabmode = cfg.read_string(sect, prefix + string("grabmode"), grabmode);
205  numBuffers =
206  cfg.read_uint64_t(sect, prefix + string("numBuffers"), numBuffers);
207  grabTimeout =
208  cfg.read_int(sect, prefix + string("grabTimeout"), grabTimeout);
209 
211  sect, prefix + string("trigger_enabled"), trigger_enabled);
213  sect, prefix + string("trigger_polarity"), trigger_polarity);
215  cfg.read_int(sect, prefix + string("trigger_source"), trigger_source);
216  trigger_mode =
217  cfg.read_int(sect, prefix + string("trigger_mode"), trigger_mode);
218 
220  cfg.read_bool(sect, prefix + string("strobe_enabled"), strobe_enabled);
221  strobe_source =
222  cfg.read_int(sect, prefix + string("strobe_source"), strobe_source);
224  cfg.read_int(sect, prefix + string("strobe_polarity"), strobe_polarity);
225  strobe_delay =
226  cfg.read_float(sect, prefix + string("strobe_delay"), strobe_delay);
228  sect, prefix + string("strobe_duration"), strobe_duration);
229 
231  sect, prefix + string("autoexposure_auto"), autoexposure_auto);
233  sect, prefix + string("autoexposure_onOFf"), autoexposure_onOff);
235  sect, prefix + string("autoexposure_abs"), autoexposure_abs);
237  sect, prefix + string("autoexposure_EV"), autoexposure_EV);
238 
239  shutter_auto =
240  cfg.read_bool(sect, prefix + string("shutter_auto"), shutter_auto);
241  shutter_abs =
242  cfg.read_bool(sect, prefix + string("shutter_abs"), shutter_abs);
244  sect, prefix + string("shutter_time_ms"), shutter_time_ms);
245 
246  gain_auto = cfg.read_bool(sect, prefix + string("gain_auto"), gain_auto);
247  gain_abs = cfg.read_bool(sect, prefix + string("gain_abs"), gain_abs);
248  gain_dB = cfg.read_float(sect, prefix + string("gain_dB"), gain_dB);
249 
250  stereo_mode =
251  cfg.read_bool(sect, prefix + string("stereo_mode"), stereo_mode);
252  get_rectified =
253  cfg.read_bool(sect, prefix + string("get_rectified"), get_rectified);
254  rect_width =
255  cfg.read_uint64_t(sect, prefix + string("rect_width"), rect_width);
256  rect_height =
257  cfg.read_uint64_t(sect, prefix + string("rect_height"), rect_height);
258 }
259 
260 // ---------------------------------------------------------------
261 /** Default constructor */
263  : m_camera(nullptr),
264  m_camera_info(nullptr),
265  m_img_buffer(nullptr),
266  m_triclops(nullptr)
267 {
268 #if MRPT_HAS_FLYCAPTURE2
269  m_img_buffer = new FlyCapture2::Image();
270 #endif
271 }
272 
273 /** Constructor + open */
275  const TCaptureOptions_FlyCapture2& options)
276  : m_camera(nullptr),
277  m_camera_info(nullptr),
278  m_img_buffer(nullptr),
279  m_triclops(nullptr)
280 {
281 #if MRPT_HAS_FLYCAPTURE2
282  m_img_buffer = new FlyCapture2::Image();
283 #endif
284  this->open(options);
285 }
286 
287 /** Destructor */
289 {
290 #if MRPT_HAS_FLYCAPTURE2
291  this->close();
292  delete FC2_BUF_IMG;
293  m_img_buffer = nullptr;
294 #endif
295 }
296 
297 /** Tries to open the camera with the given options. Raises an exception on
298  * error. \sa close() */
300  const TCaptureOptions_FlyCapture2& options, const bool startCapture)
301 {
302 #if MRPT_HAS_FLYCAPTURE2
303  FlyCapture2::Error fe;
304 
305  cout << "[CImageGrabber_FlyCapture2::open] FlyCapture2 version: "
307 
308  this->close();
309  this->m_options = options;
310 
311  // Determine camera to open:
312  // -----------------------------------
313  PGRGuid guid;
315  {
316  // Open by GUID:
317  for (int i = 0; i < 4; i++) guid.value[i] = m_options.camera_guid[i];
318  }
319  else
320  {
321  // Open by camera index:
322  BusManager busMgr;
323  unsigned int numCameras;
324  fe = busMgr.GetNumOfCameras(&numCameras);
325  CHECK_FC2_ERROR(fe)
326 
327  if (m_options.camera_index >= numCameras)
329  mrpt::format(
330  "Error: camera_index to open is '%u', but only '%u' "
331  "cameras were detected in the system.",
332  m_options.camera_index, numCameras))
333 
334  fe = busMgr.GetCameraFromIndex(m_options.camera_index, &guid);
335  CHECK_FC2_ERROR(fe)
336  }
337 
338  // Connect to camera:
339  m_camera = new FlyCapture2::Camera();
340  m_camera_info = new FlyCapture2::CameraInfo();
341 
342  cout << mrpt::format(
343  "[CImageGrabber_FlyCapture2::open] Opening camera with GUID= "
344  "%08X-%08X-%08X-%08X...\n",
345  guid.value[0], guid.value[1], guid.value[2], guid.value[3]);
346  fe = FC2_CAM->Connect(&guid);
347  CHECK_FC2_ERROR(fe)
348  fe = FC2_CAM->GetCameraInfo(FC2_CAM_INFO);
349  CHECK_FC2_ERROR(fe)
350 
351  const FlyCapture2::CameraInfo* ci = FC2_CAM_INFO;
352 
353  cout << mrpt::format(
354  "[CImageGrabber_FlyCapture2::open] Camera connected ok:\n"
355  " Serial number - %u\n"
356  " Camera model - %s\n"
357  " Camera vendor - %s\n"
358  " Sensor - %s\n"
359  " Resolution - %s\n"
360  " Firmware version - %s\n"
361  " Firmware build time - %s\n\n",
362  ci->serialNumber, ci->modelName, ci->vendorName, ci->sensorInfo,
363  ci->sensorResolution, ci->firmwareVersion, ci->firmwareBuildTime);
364 
365  // Set camera config:
366  if (!m_options.videomode.empty() && !m_options.framerate.empty())
367  {
368  bool isSupported = false;
369 
370  if (!m_options.stereo_mode)
371  {
372  FlyCapture2::VideoMode vidMode =
373  fc2_defstr2num<FlyCapture2::VideoMode>(m_options.videomode);
374  FlyCapture2::FrameRate vidRate =
375  fc2_defstr2num<FlyCapture2::FrameRate>(m_options.framerate);
376 
377  fe = FC2_CAM->GetVideoModeAndFrameRateInfo(
378  vidMode, vidRate, &isSupported);
379  CHECK_FC2_ERROR(fe)
380 
381  if (!isSupported)
382  {
383  FlyCapture2::VideoMode curVidMode;
384  FlyCapture2::FrameRate curVidRate;
385  fe =
386  FC2_CAM->GetVideoModeAndFrameRate(&curVidMode, &curVidRate);
387 
389  mrpt::format(
390  "Camera mode '%s' + '%s' is not supported by this "
391  "camera. Current mode is %d, current rate is %d.",
392  m_options.videomode.c_str(),
393  m_options.framerate.c_str(),
394  static_cast<int>(curVidMode),
395  static_cast<int>(curVidRate)))
396  }
397 
398  fe = FC2_CAM->SetVideoModeAndFrameRate(vidMode, vidRate);
399  CHECK_FC2_ERROR(fe)
400  }
401  else
402  {
403 #if MRPT_HAS_TRICLOPS
404  Fc2Triclops::ErrorType fte;
405  // Configure camera for Stereo mode
406  StereoCameraMode mode = TWO_CAMERA;
407  fte = setStereoMode(*(FC2_CAM), mode);
408  if (fte) handleFc2TriclopsError(fte, "setStereoMode");
409 
410  // Generate Triclops context
411 
412  m_triclops = new TriclopsContext;
413  fte = getContextFromCamera(FC2_CAM_INFO->serialNumber, TRI_CONTEXT);
414  if (fte != ERRORTYPE_OK)
415  handleFc2TriclopsError(fte, "getContextFromCamera");
416 
417  // ------------------------------------------------------
418  // TRICLOPS CONFIGURATION
419  // ------------------------------------------------------
420  // Current Format7 settings
421  /*
422  Format7ImageSettings f7settings;
423  unsigned int f7PacketSize;
424  float f7Percentage;
425  fe = FC2_CAM->GetFormat7Configuration(&f7settings, &f7PacketSize,
426  &f7Percentage);
427  CHECK_FC2_ERROR(fe)
428  */
429 
430  TriclopsError te;
431  // Set rectified resolution
432  te = triclopsSetResolution(
435  // Retrieve camera parameters
436  te = triclopsGetBaseline(*(TRI_CONTEXT), &m_baseline);
438  te = triclopsGetFocalLength(*(TRI_CONTEXT), &m_focalLength);
440  te = triclopsGetImageCenter(
443 #else
444  THROW_EXCEPTION("MRPT compiled without support for Triclops");
445 #endif
446  }
447  }
448 
449  {
450  FlyCapture2::VideoMode curVidMode;
451  FlyCapture2::FrameRate curVidRate;
452  fe = FC2_CAM->GetVideoModeAndFrameRate(&curVidMode, &curVidRate);
453  if (fe == PGRERROR_OK)
454  cout << mrpt::format(
455  "[CImageGrabber_FlyCapture2::open] Current camera mode is %s, "
456  "current rate is %s.\n",
457  fc2_defnum2str<FlyCapture2::VideoMode>(curVidMode),
458  fc2_defnum2str<FlyCapture2::FrameRate>(curVidRate));
459  }
460 
461  // Set trigger:
462  FlyCapture2::TriggerModeInfo trigInfo;
463  FC2_CAM->GetTriggerModeInfo(&trigInfo);
464 
465  FlyCapture2::TriggerMode trig;
466  trig.onOff = m_options.trigger_enabled;
468  {
469  trig.mode = m_options.trigger_mode;
470  trig.polarity = m_options.trigger_polarity;
471  trig.source = m_options.trigger_source;
472  }
473  fe = FC2_CAM->SetTriggerMode(&trig);
474  CHECK_FC2_ERROR(fe)
475 
476  // Strobe:
478  {
479  FlyCapture2::StrobeControl strobe;
480 
481  strobe.onOff = m_options.strobe_enabled;
482  strobe.delay = m_options.strobe_delay;
483  strobe.duration = m_options.strobe_duration;
484  strobe.polarity = m_options.strobe_polarity;
485  strobe.source = m_options.strobe_source;
486 
487  fe = FC2_CAM->SetStrobe(&strobe);
488  CHECK_FC2_ERROR(fe)
489  }
490 
491  // Set configs:
492  FlyCapture2::FC2Config fc2conf;
493  FC2_CAM->GetConfiguration(&fc2conf);
494  CHECK_FC2_ERROR(fe)
495 
496  fc2conf.grabMode =
497  fc2_defstr2num<FlyCapture2::GrabMode>(m_options.grabmode);
498  if (m_options.grabTimeout >= 0) fc2conf.grabTimeout = m_options.grabTimeout;
499 
500  fc2conf.numBuffers = m_options.numBuffers;
501 
502  fe = FC2_CAM->SetConfiguration(&fc2conf);
503  CHECK_FC2_ERROR(fe)
504 
505  // Autoexposure:
506  {
507  FlyCapture2::Property p;
508  p.type = FlyCapture2::AUTO_EXPOSURE;
509  FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
511  p.autoManualMode = m_options.autoexposure_auto; // true=auto
512  p.onOff = m_options.autoexposure_onOff; // true=on
513  p.absControl = m_options.autoexposure_abs; // true=abs
514  p.absValue =
515  m_options.autoexposure_EV; // abs value in Exposure Value (EV)
516  fe = FC2_CAM->SetProperty(&p);
517  CHECK_FC2_ERROR(fe)
518  }
519 
520  // Brightness:
521  {
522  FlyCapture2::Property p;
523  p.type = FlyCapture2::BRIGHTNESS;
524  FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
526  p.autoManualMode = true; // true=auto
527  // p.absControl = true;
528  // p.absValue = Brightness;
529  fe = FC2_CAM->SetProperty(&p);
530  CHECK_FC2_ERROR(fe)
531  }
532 
533  // Shutter:
534  {
535  FlyCapture2::Property p;
536  p.type = FlyCapture2::SHUTTER;
537  FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
539  p.autoManualMode = m_options.shutter_auto; // true=auto
540  p.absControl = m_options.shutter_abs; // true=abs
541  p.absValue = m_options.shutter_time_ms;
542  // p.onOff = false;
543  fe = FC2_CAM->SetProperty(&p);
544  CHECK_FC2_ERROR(fe)
545  }
546 
547  // Gain:
548  {
549  FlyCapture2::Property p;
550  p.type = FlyCapture2::GAIN;
551  FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
553  p.autoManualMode = m_options.gain_auto; // true=auto
554  p.absControl = m_options.gain_abs; // true=abs
555  p.absValue = m_options.gain_dB; // abs value in dB (decibeles)
556  // p.onOff = false;
557  fe = FC2_CAM->SetProperty(&p);
558  CHECK_FC2_ERROR(fe)
559  }
560 
561  // Framecounter:
562  EmbeddedImageInfo eii;
563  fe = FC2_CAM->GetEmbeddedImageInfo(&eii);
564  if (fe == PGRERROR_OK)
565  {
566  if (eii.frameCounter.available) eii.frameCounter.onOff = true;
567  if (eii.timestamp.available) eii.timestamp.onOff = true;
568  if (eii.exposure.available) eii.exposure.onOff = true;
569  if (eii.brightness.available) eii.brightness.onOff = true;
570 
571  // Enable all:
572  FC2_CAM->SetEmbeddedImageInfo(&eii);
573  }
574 
575  // Start:
576  if (startCapture) this->startCapture();
577 #else
578  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
579 #endif
580 }
581 
582 /** Start the actual image capture of the camera. Must be called after open(),
583  * only when "startCapture" was set to false. */
585 {
586 #if MRPT_HAS_FLYCAPTURE2
587  if (!m_camera)
588  {
589  THROW_EXCEPTION("Camera is not opened. Call open() first.");
590  }
591 
592  FlyCapture2::Error error = FC2_CAM->StartCapture();
594 
595 #else
596  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
597 #endif
598 }
599 
600 /** Starts a synchronous capture of several cameras, which must have been
601  * already opened. */
603  int numCameras, const CImageGrabber_FlyCapture2** cameras_array)
604 {
605 #if MRPT_HAS_FLYCAPTURE2
606 
607  std::vector<const FlyCapture2::Camera*> cam_ptrs(numCameras);
608 
609  for (int i = 0; i < numCameras; i++)
610  {
611  const CImageGrabber_FlyCapture2* obj = cameras_array[i];
612  if (!obj->m_camera)
613  {
615  "Camera #%i in list is not opened. Call open() first.", i)
616  }
617 
618  FlyCapture2::Camera* cam =
619  reinterpret_cast<FlyCapture2::Camera*>(obj->m_camera);
620  cam_ptrs[i] = cam;
621  }
622 
623  if (!cam_ptrs.empty())
624  {
625  FlyCapture2::Error error = FlyCapture2::Camera::StartSyncCapture(
626  cam_ptrs.size(), &cam_ptrs[0]);
628  }
629 #else
630  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
631 #endif
632 }
633 
634 /** Stop capture. */
636 {
637 #if MRPT_HAS_FLYCAPTURE2
638  if (m_camera)
639  {
640  Error error;
641 
642  // Stop grabbing:
643  error = FC2_CAM->StopCapture();
645  }
646 #else
647  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
648 #endif
649 }
650 
651 /** Closes the opened camera, if any. Called automatically on object
652  * destruction. */
654 {
655 #if MRPT_HAS_FLYCAPTURE2
656  try
657  {
658  this->stopCapture();
659  }
660  catch (...)
661  {
662  }
663 
664  // Disconnect the camera
665  try
666  {
667  if (m_camera) FC2_CAM->Disconnect();
668  }
669  catch (...)
670  {
671  }
672 
673  // Delete objects:
674  try
675  {
676  if (m_camera) delete FC2_CAM;
677  }
678  catch (...)
679  {
680  }
681  try
682  {
683  if (m_camera_info) delete FC2_CAM_INFO;
684  }
685  catch (...)
686  {
687  }
688 #if MRPT_HAS_TRICLOPS
689  try
690  {
691  if (m_triclops) delete TRI_CONTEXT;
692  }
693  catch (...)
694  {
695  }
696 #endif
697 
698  m_camera = nullptr;
699  m_camera_info = nullptr;
700 
701 #else
702  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
703 #endif
704 }
705 
706 /** Returns the PGR FlyCapture2 library version */
708 {
709 #if MRPT_HAS_FLYCAPTURE2
710  FlyCapture2::FC2Version fc2Version;
711  FlyCapture2::Utilities::GetLibraryVersion(&fc2Version);
712  return mrpt::format(
713  "%d.%d.%d.%d", fc2Version.major, fc2Version.minor, fc2Version.type,
714  fc2Version.build);
715 #else
716  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
717 #endif
718 }
719 
720 /*-------------------------------------------------------------
721  get the image - MONO
722  -------------------------------------------------------------*/
723 // Grab image from the camera. This method blocks until the next frame is
724 // captured.
725 // return: false on any error.
727  mrpt::obs::CObservationImage& out_observation)
728 {
729 #if MRPT_HAS_FLYCAPTURE2
730  if (!m_camera)
731  {
732  std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Camera is "
733  "not opened. Call open() first.\n";
734  return false;
735  }
736  try
737  {
738  FlyCapture2::Error error;
739  FlyCapture2::Image image;
740  error = FC2_CAM->RetrieveBuffer(&image);
742  FlyCapture2::TimeStamp timestamp = image.GetTimeStamp();
743  // White balance, etc.
744  // FlyCapture2::ImageMetadata imd = image.GetMetadata();
745  // Determine if it's B/W or color:
746  FlyCapture2::PixelFormat pf = image.GetPixelFormat();
747  const bool is_color =
748  pf == PIXEL_FORMAT_RGB8 || pf == PIXEL_FORMAT_RGB16 ||
749  pf == PIXEL_FORMAT_S_RGB16 || pf == PIXEL_FORMAT_RAW8 ||
750  pf == PIXEL_FORMAT_RAW16 || pf == PIXEL_FORMAT_RAW12 ||
751  pf == PIXEL_FORMAT_BGR || pf == PIXEL_FORMAT_BGRU ||
752  pf == PIXEL_FORMAT_RGBU || pf == PIXEL_FORMAT_BGR16 ||
753  pf == PIXEL_FORMAT_BGRU16 || pf == PIXEL_FORMAT_422YUV8_JPEG;
754  // Decode image:
755  error = image.Convert(
756  is_color ? PIXEL_FORMAT_BGR : PIXEL_FORMAT_MONO8, FC2_BUF_IMG);
758  // Convert PGR FlyCapture2 image ==> OpenCV format:
759  unsigned int img_rows, img_cols, img_stride;
760  FC2_BUF_IMG->GetDimensions(&img_rows, &img_cols, &img_stride);
761  out_observation.image.loadFromMemoryBuffer(
762  img_cols, img_rows, is_color, FC2_BUF_IMG->GetData());
763  // It seems timestamp is not always correctly filled in the incoming
764  // imgs:
765  if (timestamp.seconds != 0)
766  out_observation.timestamp = mrpt::system::time_tToTimestamp(
767  timestamp.seconds + 1e-6 * timestamp.microSeconds);
768  else
769  out_observation.timestamp = mrpt::system::now();
770  return true;
771  }
772  catch (std::exception& e)
773  {
774  std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Error:\n"
775  << e.what() << std::endl;
776  return false;
777  }
778 #else
779  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
780 #endif
781 }
782 
783 /*-------------------------------------------------------------
784  get the image - STEREO
785  -------------------------------------------------------------*/
786 // Grab image from the camera. This method blocks until the next frame is
787 // captured.
788 // return: false on any error.
790  mrpt::obs::CObservationStereoImages& out_observation)
791 {
792 #if MRPT_HAS_FLYCAPTURE2 && MRPT_HAS_TRICLOPS && MRPT_HAS_OPENCV
793  if (!m_camera)
794  {
795  std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Camera is "
796  "not opened. Call open() first.\n";
797  return false;
798  }
799 
800  try
801  {
802  FlyCapture2::Error ferr;
803  Fc2Triclops::ErrorType fterr;
804  TriclopsError te;
805  FlyCapture2::Image image;
806  ferr = FC2_CAM->RetrieveBuffer(&image);
807  CHECK_FC2_ERROR(ferr)
809  FlyCapture2::TimeStamp timestamp = image.GetTimeStamp();
810 
811  // White balance, etc.
812  // FlyCapture2::ImageMetadata imd = image.GetMetadata();
813 
814  // ------------------------------------------
815  // Extract images from common interleaved image:
816  // ------------------------------------------
817  IplImage* imageIpl[2]; // Output pair of images
818  FlyCapture2::Image rawImage[2];
819 
820  // Convert the pixel interleaved raw data to de-interleaved raw data
821  fterr = Fc2Triclops::unpackUnprocessedRawOrMono16Image(
822  image, true,
823  rawImage[0], // Right image
824  rawImage[1]); // Left image
825  if (fterr != Fc2Triclops::ERRORTYPE_OK)
826  {
827  Fc2Triclops::handleFc2TriclopsError(
828  fterr, "unprocessedRawOrMono16Image()");
829  return false;
830  }
831 
832  // Convert each raw image to RGBU image (for color images)
833  unsigned int img_rows, img_cols, img_stride;
834  for (int i = 0; i < 2; ++i)
835  {
836  FlyCapture2::Image rgbuImage;
837  ferr = rawImage[i].SetColorProcessing(FlyCapture2::HQ_LINEAR);
838  CHECK_FC2_ERROR(ferr)
839  ferr = rawImage[i].Convert(PIXEL_FORMAT_BGRU, &rgbuImage);
840  CHECK_FC2_ERROR(ferr)
841 
842  unsigned char* data::Ptr; // To store Ipl converted image pointer
843  if (m_options.get_rectified) // If rectified
844  {
845  // Use the rgbu single image to build up a packed (rbgu)
846  // TriclopsInput.
847  // A packed triclops input will contain a single image with 32
848  // bpp.
849  TriclopsInput triclopsColorInput;
850  te = triclopsBuildPackedTriclopsInput(
851  rgbuImage.GetCols(), rgbuImage.GetRows(),
852  rgbuImage.GetStride(),
853  (unsigned long)image.GetTimeStamp().seconds,
854  (unsigned long)image.GetTimeStamp().microSeconds,
855  rgbuImage.GetData(), &triclopsColorInput);
856 
857  // Do rectification
858  TriclopsPackedColorImage rectPackColImg;
859  te = triclopsRectifyPackedColorImage(
860  *(TRI_CONTEXT), i == 0 ? TriCam_RIGHT : TriCam_LEFT,
861  const_cast<TriclopsInput*>(&triclopsColorInput),
862  &rectPackColImg);
864 
865  // Set image properties for reallocation
866  img_rows = rectPackColImg.nrows;
867  img_cols = rectPackColImg.ncols;
868  img_stride = rectPackColImg.rowinc;
869  data::Ptr = (unsigned char*)rectPackColImg.data;
870  }
871  else // If not rectified
872  {
873  rgbuImage.GetDimensions(&img_rows, &img_cols, &img_stride);
874  data::Ptr = rgbuImage.GetData();
875  }
876  // Convert PGR image ==> OpenCV format:
877  IplImage* tmpImage =
878  cvCreateImage(cvSize(img_cols, img_rows), IPL_DEPTH_8U, 4);
879 
880  // Copy image data
881  memcpy(tmpImage->imageData, data::Ptr, img_rows * img_stride);
882  tmpImage->widthStep = img_stride;
883  // Convert images to BGR (3 channels) and set origins
884  imageIpl[i] =
885  cvCreateImage(cvSize(img_cols, img_rows), IPL_DEPTH_8U, 3);
886  cvCvtColor(tmpImage, imageIpl[i], CV_BGRA2BGR);
887  imageIpl[i]->origin = tmpImage->origin;
888  // Release temp images
889  cvReleaseImage(&tmpImage);
890  }
891 
892  /*-------------------------------------------------------------
893  Fill output stereo observation
894  -------------------------------------------------------------*/
895  out_observation.imageRight.setFromIplImage(imageIpl[0]); // Right cam.
896  out_observation.imageLeft.setFromIplImage(imageIpl[1]); // Left cam.
897 
898  // It seems timestamp is not always correctly filled in the incoming
899  // imgs:
900  if (timestamp.seconds != 0)
901  out_observation.timestamp = mrpt::system::time_tToTimestamp(
902  timestamp.seconds + 1e-6 * timestamp.microSeconds);
903  else
904  out_observation.timestamp = ts_retrieved;
905 
906  out_observation.rightCameraPose.x(m_baseline);
907  out_observation.rightCameraPose.y(0);
908  out_observation.rightCameraPose.z(0);
909 
910  out_observation.rightCameraPose.quat().r(1);
911  out_observation.rightCameraPose.quat().x(0);
912  out_observation.rightCameraPose.quat().y(0);
913  out_observation.rightCameraPose.quat().z(0);
914 
915  out_observation.cameraPose.x(0);
916  out_observation.cameraPose.y(0);
917  out_observation.cameraPose.z(0);
918 
919  out_observation.cameraPose.quat().r(1);
920  out_observation.cameraPose.quat().x(0);
921  out_observation.cameraPose.quat().y(0);
922  out_observation.cameraPose.quat().z(0);
923 
928  return true;
929  }
930  catch (std::exception& e)
931  {
932  std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Error:\n"
933  << e.what() << std::endl;
934  return false;
935  }
936 #else
938  "MRPT compiled without support for FlyCapture2, Triclops or OpenCV")
939 #endif
940 }
void close()
Stop capture and closes the opened camera, if any.
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
Definition: CPose3DQuat.h:61
bool getObservation(mrpt::obs::CObservationImage &out_observation)
Grab mono image from the camera.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
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:41
void * m_img_buffer
Opaque pointer to the FlyCapture2::Image, used as a temporary buffer and to avoid mem alloc/reallocs...
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:91
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:75
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...
GLenum GLsizei GLenum GLenum const GLvoid * image
Definition: glext.h:3551
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
#define FC2_CAM
STL namespace.
GLdouble s
Definition: glext.h:3676
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
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.
int grabTimeout
(Default=5000) Time in milliseconds that RetrieveBuffer() and WaitForBufferEvent() will wait for an i...
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 coordinate of the quaternion.
Definition: CQuaternion.h:87
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=640) 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:955
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.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
#define FC2_CAM_INFO
unsigned int rect_height
(default=480) Height for output rectified images
bool autoexposure_abs
(default=true) Numeric mode (absolute or integer values)
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
uint64_t TimeStamp
A real-time timestamp (ms)
Definition: xsens_time.h:24
bool stereo_mode
(default=false) Obtain images as stereo pairs with Flycapture2
GLsizei const GLchar ** string
Definition: glext.h:4101
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.
GLint mode
Definition: glext.h:5669
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:89
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
bool get_rectified
(default=false) Rectify stereo images (needs Triclops installed)
mrpt::poses::CPose3DQuat rightCameraPose
The pose of the right camera, relative to the left one: Note that using the conventional reference co...
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:31
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:105
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:93
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.cpp:50
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...
GLfloat GLfloat p
Definition: glext.h:6305
void setFromIplImage(void *iplImage)
Reads the image from a OpenCV IplImage object (WITHOUT making a copy).
Definition: CImage.cpp:363
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 memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Definition: os.cpp:356
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:385
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 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019