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-2017, 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::utils::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 }
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:32
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:60
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:135
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 read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
bool gain_abs
(default=true) Numeric mode (absolute or integer values)
float autoexposure_EV
(default=0.0) Exposure Value, if autoexposure_auto=false
mrpt::utils::CImage imageLeft
Image from the left camera (this image will be ALWAYS present)
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
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)
#define THROW_EXCEPTION(msg)
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
float strobe_duration
(default=1.0) Pulse durationin ms.
#define TRI_CONTEXT
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:76
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
std::string read_string(const std::string &section, const std::string &name, const std::string &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.
CImageGrabber_FlyCapture2()
Constructor that does not open a camera.
#define CHECK_FC2_ERROR(_err)
This class allows loading and storing values and vectors of different types from a configuration text...
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
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.
T r() const
Return r coordinate of the quaternion.
Definition: CQuaternion.h:87
void * m_triclops
Opaque pointer to the TriclopsContext objetc.
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:390
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:...
void loadOptionsFrom(const mrpt::utils::CConfigFileBase &configSource, const std::string &sectionName, const std::string &prefix=std::string())
Loads all the options from a config file.
Observation class for either a pair of left+right or left+disparity images from a stereo camera...
int val
Definition: mrpt_jpeglib.h:955
Options used when creating a camera capture object of type CImageGrabber_FlyCapture2.
#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)
uint64_t TimeStamp
A real-time timestamp (ms)
Definition: xsens_time.h:24
void setFromIplImage(void *iplImage)
Reads the image from a OpenCV IplImage object (WITHOUT making a copy).
Definition: CImage.cpp:368
bool stereo_mode
(default=false) Obtain images as stereo pairs with Flycapture2
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::utils::TCamera leftCamera
Parameters for the left/right cameras: individual intrinsic and distortion parameters of the cameras...
mrpt::utils::CImage imageRight
Image from the right camera, only contains a valid image if hasImageRight == true.
bool trigger_enabled
(default=false) Enable non-free-running mode, only capturing when a given input trigger signal is det...
void tokenize(const std::string &inString, const std::string &inDelimiters, std::deque< std::string > &outTokens, bool skipBlankTokens=true) noexcept
Tokenizes a string according to a set of delimiting characters.
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:58
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.
#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::...
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...
mrpt::utils::CImage image
The image captured by the camera, that is, the main piece of information of this observation.
uint64_t read_uint64_t(const std::string &section, const std::string &name, uint64_t defaultValue, bool failIfNotFound=false) const
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
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:49
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.
#define ASSERTMSG_(f, __ERROR_MSG)
GLfloat GLfloat p
Definition: glext.h:6305
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:118
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.
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:355
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: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019