Main MRPT website > C++ reference for MRPT 1.5.6
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) { if (_err != PGRERROR_OK) { THROW_EXCEPTION_FMT("FlyCapture2 error:\n%s",_err.GetDescription()) } }
34 #define CHECK_TRICLOPS_ERROR(_err) \
35 { if( _err != TriclopsErrorOk ) \
36  { THROW_EXCEPTION_FMT("Triclops Error:\n'%s'",triclopsErrorToString( _err )) } \
37 }
38 #define FC2_CAM reinterpret_cast<FlyCapture2::Camera*>(m_camera)
39 #define FC2_CAM_INFO reinterpret_cast<FlyCapture2::CameraInfo*>(m_camera_info)
40 #define FC2_BUF_IMG reinterpret_cast<FlyCapture2::Image*>(m_img_buffer)
41 #define TRI_CONTEXT reinterpret_cast<TriclopsContext*>(m_triclops)
42 
43 using namespace mrpt::hwdrivers;
44 using namespace std;
45 
46 #if MRPT_HAS_FLYCAPTURE2
47 // Declare tables to convert strings to their #define values:
48 template <typename T>
49 struct fc2_str_val {
50  const char* str;
51  T val;
52 };
53 
54 const fc2_str_val<VideoMode> fc2_VideoMode_table[] = {
55  { "VIDEOMODE_160x120YUV444",VIDEOMODE_160x120YUV444},
56  { "VIDEOMODE_320x240YUV422",VIDEOMODE_320x240YUV422},
57  { "VIDEOMODE_640x480YUV411",VIDEOMODE_640x480YUV411},
58  { "VIDEOMODE_640x480YUV422",VIDEOMODE_640x480YUV422},
59  { "VIDEOMODE_640x480RGB",VIDEOMODE_640x480RGB},
60  { "VIDEOMODE_640x480Y8",VIDEOMODE_640x480Y8},
61  { "VIDEOMODE_640x480Y16",VIDEOMODE_640x480Y16},
62  { "VIDEOMODE_800x600YUV422",VIDEOMODE_800x600YUV422},
63  { "VIDEOMODE_800x600RGB",VIDEOMODE_800x600RGB},
64  { "VIDEOMODE_800x600Y8",VIDEOMODE_800x600Y8},
65  { "VIDEOMODE_800x600Y16",VIDEOMODE_800x600Y16},
66  { "VIDEOMODE_1024x768YUV422",VIDEOMODE_1024x768YUV422},
67  { "VIDEOMODE_1024x768RGB",VIDEOMODE_1024x768RGB},
68  { "VIDEOMODE_1024x768Y8",VIDEOMODE_1024x768Y8},
69  { "VIDEOMODE_1024x768Y16",VIDEOMODE_1024x768Y16},
70  { "VIDEOMODE_1280x960YUV422",VIDEOMODE_1280x960YUV422},
71  { "VIDEOMODE_1280x960RGB",VIDEOMODE_1280x960RGB},
72  { "VIDEOMODE_1280x960Y8",VIDEOMODE_1280x960Y8},
73  { "VIDEOMODE_1280x960Y16",VIDEOMODE_1280x960Y16},
74  { "VIDEOMODE_1600x1200YUV422",VIDEOMODE_1600x1200YUV422},
75  { "VIDEOMODE_1600x1200RGB",VIDEOMODE_1600x1200RGB},
76  { "VIDEOMODE_1600x1200Y8",VIDEOMODE_1600x1200Y8},
77  { "VIDEOMODE_1600x1200Y16",VIDEOMODE_1600x1200Y16},
78  { "VIDEOMODE_FORMAT7", VIDEOMODE_FORMAT7}
79 };
80 fc2_str_val<FrameRate> fc2_FrameRate_table[] = {
81  { "FRAMERATE_1_875",FlyCapture2::FRAMERATE_1_875},
82  { "FRAMERATE_3_75",FlyCapture2::FRAMERATE_3_75},
83  { "FRAMERATE_7_5",FlyCapture2::FRAMERATE_7_5},
84  { "FRAMERATE_15",FlyCapture2::FRAMERATE_15},
85  { "FRAMERATE_30",FlyCapture2::FRAMERATE_30},
86  { "FRAMERATE_60",FlyCapture2::FRAMERATE_60},
87  { "FRAMERATE_120",FlyCapture2::FRAMERATE_120},
88  { "FRAMERATE_240",FlyCapture2::FRAMERATE_240},
89  { "FRAMERATE_FORMAT7",FlyCapture2::FRAMERATE_FORMAT7}
90 };
91 fc2_str_val<GrabMode> fc2_GrabMode_table[] = {
92  { "DROP_FRAMES",DROP_FRAMES},
93  { "BUFFER_FRAMES",BUFFER_FRAMES}
94 };
95 
96 #define GET_CONV_TABLE(type) \
97  vector< fc2_str_val<type> > fc2_vals_gen( type ) { \
98  size_t n = sizeof( fc2_##type##_table ) / sizeof( fc2_##type##_table[0] ); \
99  vector< fc2_str_val<type> > vec( &fc2_##type##_table[0], &fc2_##type##_table[n] ); \
100  return vec; }
101 GET_CONV_TABLE(VideoMode)
102 GET_CONV_TABLE(FrameRate)
103 GET_CONV_TABLE(GrabMode)
104 
105 template <typename T>
106 T fc2_defstr2num(const std::string &str)
107 {
108  vector< fc2_str_val<T> > fc2_vals = fc2_vals_gen( T() );
109  const std::string s = mrpt::system::trim(str);
110  for (size_t i=0;i<fc2_vals.size();i++)
111  {
112  if (mrpt::system::strCmpI(fc2_vals[i].str,s.c_str()))
113  return fc2_vals[i].val;
114  }
115  THROW_EXCEPTION_FMT("Error: Unknown FlyCapture2 constant: %s",s.c_str())
116 }
117 
118 
119 template <typename T>
120 const char* fc2_defnum2str(const T &val)
121 {
122  vector< fc2_str_val<T> > fc2_vals = fc2_vals_gen( T() );
123  size_t i = static_cast<int>(val);
124  if (i < fc2_vals.size())
125  return fc2_vals[i].str;
126  else
127  THROW_EXCEPTION_FMT("Error: Unknown FlyCapture2 enum: %i",static_cast<int>(val))
128 }
129 #endif
130 
131 
132 // Options: TCaptureOptions_bumblebee
133 // -------------------------------------------------------------
134 TCaptureOptions_FlyCapture2::TCaptureOptions_FlyCapture2() :
135  camera_index (0),
136  open_by_guid (false),
137  videomode(), //("VIDEOMODE_640x480Y8"),
138  framerate(), // ("FRAMERATE_30"),
139  grabmode("BUFFER_FRAMES"),
140  numBuffers(30),
141  grabTimeout(-1),
142  trigger_enabled(false),
143  trigger_polarity(0),
144  trigger_source(0),
145  trigger_mode(0),
146  strobe_enabled(false),
147  strobe_source(0),
148  strobe_polarity(0),
149  strobe_delay(0.0f),
150  strobe_duration(1.0f),
151  autoexposure_auto(true),
152  autoexposure_onOff(true),
153  autoexposure_abs(true),
154  autoexposure_EV(0.0f),
155  shutter_auto(true),
156  shutter_abs(true),
157  shutter_time_ms(4.0f),
158  gain_auto(true),
159  gain_abs(true),
160  gain_dB(0.0f),
161  stereo_mode(false),
162  get_rectified(false),
163  rect_width(640),
164  rect_height(480)
165 {
166  memset(camera_guid,0,4*sizeof(camera_guid[0]));
167 }
168 
170  const mrpt::utils::CConfigFileBase & cfg,
171  const std::string & sect,
172  const std::string & prefix )
173 {
174  camera_index = cfg.read_int(sect, prefix+string("camera_index"), camera_index);
175  open_by_guid = cfg.read_bool(sect, prefix+string("open_by_guid"), open_by_guid);
176 
177  if (open_by_guid)
178  {
179  string sGUID = cfg.read_string(sect, prefix+string("camera_guid"), "", true );
180  vector<string> sGUIDparts;
181  mrpt::system::tokenize(sGUID,"- \t\r\n",sGUIDparts);
182  ASSERTMSG_(sGUIDparts.size()==4, "GUID format error: must have four blocks like XXX-XXX-XXX-XXX")
183 
184  for (int i=0;i<4;i++)
185  sscanf(sGUIDparts[i].c_str(),"%X", &camera_guid[i]);
186  }
187 
188  videomode = cfg.read_string(sect, prefix+string("videomode"), videomode);
189  framerate = cfg.read_string(sect, prefix+string("framerate"), framerate);
190  grabmode = cfg.read_string(sect, prefix+string("grabmode"), grabmode);
191  numBuffers = cfg.read_uint64_t(sect, prefix+string("numBuffers"), numBuffers);
192  grabTimeout = cfg.read_int(sect, prefix+string("grabTimeout"), grabTimeout);
193 
194  trigger_enabled = cfg.read_bool(sect, prefix+string("trigger_enabled"), trigger_enabled);
195  trigger_polarity = cfg.read_int(sect, prefix+string("trigger_polarity"), trigger_polarity);
196  trigger_source = cfg.read_int(sect, prefix+string("trigger_source"), trigger_source);
197  trigger_mode = cfg.read_int(sect, prefix+string("trigger_mode"), trigger_mode);
198 
199  strobe_enabled = cfg.read_bool(sect, prefix+string("strobe_enabled"), strobe_enabled);
200  strobe_source = cfg.read_int(sect, prefix+string("strobe_source"), strobe_source);
201  strobe_polarity = cfg.read_int(sect, prefix+string("strobe_polarity"), strobe_polarity);
202  strobe_delay = cfg.read_float(sect, prefix+string("strobe_delay"), strobe_delay);
203  strobe_duration = cfg.read_float(sect, prefix+string("strobe_duration"), strobe_duration);
204 
205  autoexposure_auto = cfg.read_bool(sect, prefix+string("autoexposure_auto"), autoexposure_auto);
206  autoexposure_onOff = cfg.read_bool(sect, prefix+string("autoexposure_onOFf"), autoexposure_onOff);
207  autoexposure_abs = cfg.read_bool(sect, prefix+string("autoexposure_abs"), autoexposure_abs);
208  autoexposure_EV = cfg.read_float(sect, prefix+string("autoexposure_EV"), autoexposure_EV);
209 
210  shutter_auto = cfg.read_bool(sect, prefix+string("shutter_auto"), shutter_auto);
211  shutter_abs = cfg.read_bool(sect, prefix+string("shutter_abs"), shutter_abs);
212  shutter_time_ms = cfg.read_float(sect, prefix+string("shutter_time_ms"), shutter_time_ms);
213 
214  gain_auto = cfg.read_bool(sect, prefix+string("gain_auto"), gain_auto);
215  gain_abs = cfg.read_bool(sect, prefix+string("gain_abs"), gain_abs);
216  gain_dB = cfg.read_float(sect, prefix+string("gain_dB"), gain_dB);
217 
218  stereo_mode = cfg.read_bool(sect, prefix+string("stereo_mode"), stereo_mode);
219  get_rectified = cfg.read_bool(sect, prefix+string("get_rectified"), get_rectified);
220  rect_width = cfg.read_uint64_t(sect, prefix+string("rect_width"), rect_width);
221  rect_height = cfg.read_uint64_t(sect, prefix+string("rect_height"), rect_height);
222 }
223 
224 
225 // ---------------------------------------------------------------
226 /** Default constructor */
228  m_camera(NULL),
229  m_camera_info(NULL),
230  m_img_buffer(NULL),
231  m_triclops(NULL)
232 {
233 #if MRPT_HAS_FLYCAPTURE2
234  m_img_buffer = new FlyCapture2::Image();
235 #endif
236 }
237 
238 /** Constructor + open */
240  m_camera(NULL),
241  m_camera_info(NULL),
242  m_img_buffer(NULL),
243  m_triclops(NULL)
244 {
245 #if MRPT_HAS_FLYCAPTURE2
246  m_img_buffer = new FlyCapture2::Image();
247 #endif
248  this->open(options);
249 }
250 
251 /** Destructor */
253 {
254 #if MRPT_HAS_FLYCAPTURE2
255  this->close();
256  delete FC2_BUF_IMG; m_img_buffer = NULL;
257 #endif
258 }
259 
260 
261 /** Tries to open the camera with the given options. Raises an exception on error. \sa close() */
262 void CImageGrabber_FlyCapture2::open( const TCaptureOptions_FlyCapture2 &options, const bool startCapture )
263 {
264 #if MRPT_HAS_FLYCAPTURE2
265  FlyCapture2::Error fe;
266 
267  cout << "[CImageGrabber_FlyCapture2::open] FlyCapture2 version: " << CImageGrabber_FlyCapture2::getFC2version() << std::endl;
268 
269  this->close();
270  this->m_options = options;
271 
272  // Determine camera to open:
273  // -----------------------------------
274  PGRGuid guid;
276  {
277  // Open by GUID:
278  for (int i=0;i<4;i++)
279  guid.value[i] = m_options.camera_guid[i];
280  }
281  else
282  {
283  // Open by camera index:
284  BusManager busMgr;
285  unsigned int numCameras;
286  fe = busMgr.GetNumOfCameras(&numCameras);
287  CHECK_FC2_ERROR(fe)
288 
289  if (m_options.camera_index>=numCameras)
290  THROW_EXCEPTION(mrpt::format("Error: camera_index to open is '%u', but only '%u' cameras were detected in the system.",m_options.camera_index,numCameras))
291 
292  fe = busMgr.GetCameraFromIndex(m_options.camera_index, &guid);
293  CHECK_FC2_ERROR(fe)
294  }
295 
296  // Connect to camera:
297  m_camera = new FlyCapture2::Camera();
298  m_camera_info = new FlyCapture2::CameraInfo();
299 
300  cout << mrpt::format("[CImageGrabber_FlyCapture2::open] Opening camera with GUID= %08X-%08X-%08X-%08X...\n", guid.value[0],guid.value[1],guid.value[2],guid.value[3]);
301  fe = FC2_CAM->Connect(&guid);
302  CHECK_FC2_ERROR(fe)
303  fe = FC2_CAM->GetCameraInfo(FC2_CAM_INFO);
304  CHECK_FC2_ERROR(fe)
305 
306  const FlyCapture2::CameraInfo *ci = FC2_CAM_INFO;
307 
308  cout << mrpt::format(
309  "[CImageGrabber_FlyCapture2::open] Camera connected ok:\n"
310  " Serial number - %u\n"
311  " Camera model - %s\n"
312  " Camera vendor - %s\n"
313  " Sensor - %s\n"
314  " Resolution - %s\n"
315  " Firmware version - %s\n"
316  " Firmware build time - %s\n\n",
317  ci->serialNumber,
318  ci->modelName,
319  ci->vendorName,
320  ci->sensorInfo,
321  ci->sensorResolution,
322  ci->firmwareVersion,
323  ci->firmwareBuildTime );
324 
325  // Set camera config:
326  if (!m_options.videomode.empty() && !m_options.framerate.empty())
327  {
328  bool isSupported = false;
329 
330  if (!m_options.stereo_mode)
331  {
332  FlyCapture2::VideoMode vidMode = fc2_defstr2num<FlyCapture2::VideoMode>(m_options.videomode);
333  FlyCapture2::FrameRate vidRate = fc2_defstr2num<FlyCapture2::FrameRate>(m_options.framerate);
334 
335  fe = FC2_CAM->GetVideoModeAndFrameRateInfo(vidMode,vidRate, &isSupported);
336  CHECK_FC2_ERROR(fe)
337 
338  if (!isSupported)
339  {
340  FlyCapture2::VideoMode curVidMode;
341  FlyCapture2::FrameRate curVidRate;
342  fe = FC2_CAM->GetVideoModeAndFrameRate(&curVidMode,&curVidRate);
343 
344  THROW_EXCEPTION(mrpt::format("Camera mode '%s' + '%s' is not supported by this camera. Current mode is %d, current rate is %d.",m_options.videomode.c_str(),m_options.framerate.c_str(),static_cast<int>(curVidMode),static_cast<int>(curVidRate) ))
345  }
346 
347  fe = FC2_CAM->SetVideoModeAndFrameRate(vidMode,vidRate);
348  CHECK_FC2_ERROR(fe)
349  }
350  else
351  {
352 #if MRPT_HAS_TRICLOPS
353  Fc2Triclops::ErrorType fte;
354  // Configure camera for Stereo mode
355  StereoCameraMode mode = TWO_CAMERA;
356  fte = setStereoMode( *(FC2_CAM), mode );
357  if ( fte )
358  handleFc2TriclopsError(fte, "setStereoMode");
359 
360  //Generate Triclops context
361 
362  m_triclops= new TriclopsContext;
363  fte = getContextFromCamera( FC2_CAM_INFO->serialNumber,
364  TRI_CONTEXT );
365  if (fte != ERRORTYPE_OK)
366  handleFc2TriclopsError(fte, "getContextFromCamera");
367 
368  // ------------------------------------------------------
369  // TRICLOPS CONFIGURATION
370  // ------------------------------------------------------
371  // Current Format7 settings
372  /*
373  Format7ImageSettings f7settings;
374  unsigned int f7PacketSize;
375  float f7Percentage;
376  fe = FC2_CAM->GetFormat7Configuration(&f7settings, &f7PacketSize, &f7Percentage);
377  CHECK_FC2_ERROR(fe)
378  */
379 
380  TriclopsError te;
381  // Set rectified resolution
382  te = triclopsSetResolution( *(TRI_CONTEXT), m_options.rect_height, m_options.rect_width );
383  CHECK_TRICLOPS_ERROR( te );
384  // Retrieve camera parameters
385  te = triclopsGetBaseline( *(TRI_CONTEXT), &m_baseline );
386  CHECK_TRICLOPS_ERROR( te );
387  te = triclopsGetFocalLength( *(TRI_CONTEXT), &m_focalLength );
388  CHECK_TRICLOPS_ERROR( te );
389  te = triclopsGetImageCenter( *(TRI_CONTEXT), &m_centerRow, &m_centerCol);
390  CHECK_TRICLOPS_ERROR( te );
391 #else
392  THROW_EXCEPTION("MRPT compiled without support for Triclops")
393 #endif
394  }
395  }
396 
397  {
398  FlyCapture2::VideoMode curVidMode;
399  FlyCapture2::FrameRate curVidRate;
400  fe = FC2_CAM->GetVideoModeAndFrameRate(&curVidMode,&curVidRate);
401  if (fe==PGRERROR_OK)
402  cout << mrpt::format("[CImageGrabber_FlyCapture2::open] Current camera mode is %s, current rate is %s.\n",
403  fc2_defnum2str<FlyCapture2::VideoMode>(curVidMode),
404  fc2_defnum2str<FlyCapture2::FrameRate>(curVidRate));
405  }
406 
407 
408  // Set trigger:
409  FlyCapture2::TriggerModeInfo trigInfo;
410  FC2_CAM->GetTriggerModeInfo(&trigInfo);
411 
412  FlyCapture2::TriggerMode trig;
413  trig.onOff = m_options.trigger_enabled;
415  {
416  trig.mode = m_options.trigger_mode;
417  trig.polarity = m_options.trigger_polarity;
418  trig.source = m_options.trigger_source;
419  }
420  fe = FC2_CAM->SetTriggerMode(&trig);
421  CHECK_FC2_ERROR(fe)
422 
423  // Strobe:
425  {
426  FlyCapture2::StrobeControl strobe;
427 
428  strobe.onOff = m_options.strobe_enabled;
429  strobe.delay = m_options.strobe_delay;
430  strobe.duration = m_options.strobe_duration;
431  strobe.polarity = m_options.strobe_polarity;
432  strobe.source = m_options.strobe_source;
433 
434  fe = FC2_CAM->SetStrobe(&strobe);
435  CHECK_FC2_ERROR(fe)
436  }
437 
438  // Set configs:
439  FlyCapture2::FC2Config fc2conf;
440  FC2_CAM->GetConfiguration(&fc2conf);
441  CHECK_FC2_ERROR(fe)
442 
443  fc2conf.grabMode = fc2_defstr2num<FlyCapture2::GrabMode>(m_options.grabmode);
444  if (m_options.grabTimeout>=0)
445  fc2conf.grabTimeout = m_options.grabTimeout;
446 
447  fc2conf.numBuffers = m_options.numBuffers;
448 
449  fe = FC2_CAM->SetConfiguration( &fc2conf);
450  CHECK_FC2_ERROR(fe)
451 
452  // Autoexposure:
453  {
454  FlyCapture2::Property p;
455  p.type = FlyCapture2::AUTO_EXPOSURE;
456  FlyCapture2::Error error = FC2_CAM->GetProperty( &p );
457  CHECK_FC2_ERROR(error)
458  p.autoManualMode = m_options.autoexposure_auto; // true=auto
459  p.onOff = m_options.autoexposure_onOff; // true=on
460  p.absControl = m_options.autoexposure_abs; // true=abs
461  p.absValue = m_options.autoexposure_EV; // abs value in Exposure Value (EV)
462  fe = FC2_CAM->SetProperty (&p);
463  CHECK_FC2_ERROR(fe)
464  }
465 
466  // Brightness:
467  {
468  FlyCapture2::Property p;
469  p.type = FlyCapture2::BRIGHTNESS;
470  FlyCapture2::Error error = FC2_CAM->GetProperty( &p );
471  CHECK_FC2_ERROR(error)
472  p.autoManualMode = true; // true=auto
473  //p.absControl = true;
474  //p.absValue = Brightness;
475  fe = FC2_CAM->SetProperty (&p);
476  CHECK_FC2_ERROR(fe)
477  }
478 
479  // Shutter:
480  {
481  FlyCapture2::Property p;
482  p.type = FlyCapture2::SHUTTER;
483  FlyCapture2::Error error = FC2_CAM->GetProperty( &p );
484  CHECK_FC2_ERROR(error)
485  p.autoManualMode = m_options.shutter_auto; // true=auto
486  p.absControl = m_options.shutter_abs; // true=abs
487  p.absValue = m_options.shutter_time_ms;
488  //p.onOff = false;
489  fe = FC2_CAM->SetProperty (&p);
490  CHECK_FC2_ERROR(fe)
491  }
492 
493  // Gain:
494  {
495  FlyCapture2::Property p;
496  p.type = FlyCapture2::GAIN;
497  FlyCapture2::Error error = FC2_CAM->GetProperty( &p );
498  CHECK_FC2_ERROR(error)
499  p.autoManualMode = m_options.gain_auto; // true=auto
500  p.absControl = m_options.gain_abs; // true=abs
501  p.absValue = m_options.gain_dB; // abs value in dB (decibeles)
502  //p.onOff = false;
503  fe = FC2_CAM->SetProperty (&p);
504  CHECK_FC2_ERROR(fe)
505  }
506 
507  // Framecounter:
508  EmbeddedImageInfo eii;
509  fe = FC2_CAM->GetEmbeddedImageInfo(&eii);
510  if (fe == PGRERROR_OK)
511  {
512  if (eii.frameCounter.available) eii.frameCounter.onOff = true;
513  if (eii.timestamp.available) eii.timestamp.onOff = true;
514  if (eii.exposure.available) eii.exposure.onOff = true;
515  if (eii.brightness.available) eii.brightness.onOff = true;
516 
517  // Enable all:
518  FC2_CAM->SetEmbeddedImageInfo(&eii);
519  }
520 
521  // Start:
522  if (startCapture)
523  this->startCapture();
524 #else
525  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2")
526 #endif
527 }
528 
529 /** Start the actual image capture of the camera. Must be called after open(), only when "startCapture" was set to false. */
531 {
532 #if MRPT_HAS_FLYCAPTURE2
533  if (!m_camera) { THROW_EXCEPTION("Camera is not opened. Call open() first.") }
534 
535  FlyCapture2::Error error = FC2_CAM->StartCapture();
536  CHECK_FC2_ERROR(error)
537 
538 #else
539  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2")
540 #endif
541 }
542 
543 /** Starts a synchronous capture of several cameras, which must have been already opened. */
544 void CImageGrabber_FlyCapture2::startSyncCapture( int numCameras, const CImageGrabber_FlyCapture2 **cameras_array )
545 {
546 #if MRPT_HAS_FLYCAPTURE2
547 
548  std::vector<const FlyCapture2::Camera*> cam_ptrs(numCameras);
549 
550  for (int i=0;i<numCameras;i++)
551  {
552  const CImageGrabber_FlyCapture2 *obj = cameras_array[i];
553  if (!obj->m_camera) { THROW_EXCEPTION_FMT("Camera #%i in list is not opened. Call open() first.",i) }
554 
555  FlyCapture2::Camera *cam = reinterpret_cast<FlyCapture2::Camera*>(obj->m_camera);
556  cam_ptrs[i] = cam;
557  }
558 
559  if (!cam_ptrs.empty())
560  {
561  FlyCapture2::Error error = FlyCapture2::Camera::StartSyncCapture(cam_ptrs.size(), &cam_ptrs[0]);
562  CHECK_FC2_ERROR(error)
563  }
564 #else
565  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2")
566 #endif
567 }
568 
569 /** Stop capture. */
571 {
572 #if MRPT_HAS_FLYCAPTURE2
573  if (m_camera)
574  {
575  Error error;
576 
577  // Stop grabbing:
578  error = FC2_CAM->StopCapture();
579  CHECK_FC2_ERROR(error)
580  }
581 #else
582  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2")
583 #endif
584 }
585 
586 
587 /** Closes the opened camera, if any. Called automatically on object destruction. */
589 {
590 #if MRPT_HAS_FLYCAPTURE2
591  try {
592  this->stopCapture();
593  }
594  catch (...) { }
595 
596  // Disconnect the camera
597  try {
598  if (m_camera) FC2_CAM->Disconnect();
599  }
600  catch (...) { }
601 
602  // Delete objects:
603  try { if (m_camera) delete FC2_CAM; } catch (...) {}
604  try { if (m_camera_info) delete FC2_CAM_INFO; } catch (...) {}
605 #if MRPT_HAS_TRICLOPS
606  try { if (m_triclops) delete TRI_CONTEXT; } catch (...) {}
607 #endif
608 
609  m_camera=NULL;
610  m_camera_info=NULL;
611 
612 #else
613  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2")
614 #endif
615 }
616 
617 
618 /** Returns the PGR FlyCapture2 library version */
620 {
621 #if MRPT_HAS_FLYCAPTURE2
622  FlyCapture2::FC2Version fc2Version;
623  FlyCapture2::Utilities::GetLibraryVersion( &fc2Version );
624  return mrpt::format("%d.%d.%d.%d", fc2Version.major, fc2Version.minor, fc2Version.type, fc2Version.build);
625 #else
626  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2")
627 #endif
628 }
629 
630 /*-------------------------------------------------------------
631  get the image - MONO
632  -------------------------------------------------------------*/
633 // Grab image from the camera. This method blocks until the next frame is captured.
634 // return: false on any error.
636 {
637 #if MRPT_HAS_FLYCAPTURE2
638 if (!m_camera) {
639 std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Camera is not opened. Call open() first.\n";
640 return false;
641 }
642 try
643 {
644 FlyCapture2::Error error;
645 FlyCapture2::Image image;
646 error = FC2_CAM->RetrieveBuffer( &image );
647 CHECK_FC2_ERROR(error)
648 FlyCapture2::TimeStamp timestamp = image.GetTimeStamp();
649 // White balance, etc.
650 //FlyCapture2::ImageMetadata imd = image.GetMetadata();
651 // Determine if it's B/W or color:
652 FlyCapture2::PixelFormat pf = image.GetPixelFormat();
653 const bool is_color =
654 pf==PIXEL_FORMAT_RGB8 || pf==PIXEL_FORMAT_RGB16 || pf==PIXEL_FORMAT_S_RGB16 ||
655 pf==PIXEL_FORMAT_RAW8 || pf==PIXEL_FORMAT_RAW16 || pf==PIXEL_FORMAT_RAW12 ||
656 pf==PIXEL_FORMAT_BGR || pf==PIXEL_FORMAT_BGRU || pf==PIXEL_FORMAT_RGBU ||
657 pf==PIXEL_FORMAT_BGR16 || pf==PIXEL_FORMAT_BGRU16 || pf==PIXEL_FORMAT_422YUV8_JPEG;
658 // Decode image:
659 error = image.Convert(is_color ? PIXEL_FORMAT_BGR : PIXEL_FORMAT_MONO8, FC2_BUF_IMG);
660 CHECK_FC2_ERROR(error)
661 // Convert PGR FlyCapture2 image ==> OpenCV format:
662 unsigned int img_rows, img_cols, img_stride;
663 FC2_BUF_IMG->GetDimensions( &img_rows, &img_cols, &img_stride);
664 out_observation.image.loadFromMemoryBuffer(img_cols,img_rows, is_color, FC2_BUF_IMG->GetData() );
665 // It seems timestamp is not always correctly filled in the incoming imgs:
666 if (timestamp.seconds!=0)
667 out_observation.timestamp = mrpt::system::time_tToTimestamp( timestamp.seconds + 1e-6*timestamp.microSeconds );
668 else out_observation.timestamp = mrpt::system::now();
669 return true;
670 }
671 catch( std::exception &e)
672 {
673 std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Error:\n" << e.what() << std::endl;
674 return false;
675 }
676 #else
677 THROW_EXCEPTION("MRPT compiled without support for FlyCapture2")
678 #endif
679 }
680 
681 /*-------------------------------------------------------------
682  get the image - STEREO
683  -------------------------------------------------------------*/
684 // Grab image from the camera. This method blocks until the next frame is captured.
685 // return: false on any error.
687 {
688 #if MRPT_HAS_FLYCAPTURE2 && MRPT_HAS_TRICLOPS && MRPT_HAS_OPENCV
689  if (!m_camera) {
690  std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Camera is not opened. Call open() first.\n";
691  return false;
692  }
693 
694  try
695  {
696  FlyCapture2::Error ferr;
697  Fc2Triclops::ErrorType fterr;
698  TriclopsError te;
699  FlyCapture2::Image image;
700  ferr = FC2_CAM->RetrieveBuffer( &image );
701  CHECK_FC2_ERROR(ferr)
703  FlyCapture2::TimeStamp timestamp = image.GetTimeStamp();
704 
705  // White balance, etc.
706  //FlyCapture2::ImageMetadata imd = image.GetMetadata();
707 
708  // ------------------------------------------
709  // Extract images from common interleaved image:
710  // ------------------------------------------
711  IplImage* imageIpl[2]; // Output pair of images
712  FlyCapture2::Image rawImage[2];
713 
714  // Convert the pixel interleaved raw data to de-interleaved raw data
715  fterr = Fc2Triclops::unpackUnprocessedRawOrMono16Image(
716  image,
717  true,
718  rawImage[0], // Right image
719  rawImage[1] ); // Left image
720  if (fterr != Fc2Triclops::ERRORTYPE_OK)
721  {
722  Fc2Triclops::handleFc2TriclopsError(fterr, "unprocessedRawOrMono16Image()");
723  return false;
724  }
725 
726  // Convert each raw image to RGBU image (for color images)
727  unsigned int img_rows, img_cols, img_stride;
728  for ( int i = 0; i < 2; ++i )
729  {
730  FlyCapture2::Image rgbuImage;
731  ferr = rawImage[i].SetColorProcessing(FlyCapture2::HQ_LINEAR);
732  CHECK_FC2_ERROR(ferr)
733  ferr = rawImage[i].Convert(PIXEL_FORMAT_BGRU, &rgbuImage);
734  CHECK_FC2_ERROR(ferr)
735 
736  unsigned char* dataPtr; // To store Ipl converted image pointer
737  if(m_options.get_rectified) // If rectified
738  {
739  // Use the rgbu single image to build up a packed (rbgu) TriclopsInput.
740  // A packed triclops input will contain a single image with 32 bpp.
741  TriclopsInput triclopsColorInput;
742  te = triclopsBuildPackedTriclopsInput(
743  rgbuImage.GetCols(),
744  rgbuImage.GetRows(),
745  rgbuImage.GetStride(),
746  (unsigned long)image.GetTimeStamp().seconds,
747  (unsigned long)image.GetTimeStamp().microSeconds,
748  rgbuImage.GetData(),
749  &triclopsColorInput );
750 
751  // Do rectification
752  TriclopsPackedColorImage rectPackColImg;
753  te = triclopsRectifyPackedColorImage(
754  *(TRI_CONTEXT),
755  i==0 ? TriCam_RIGHT : TriCam_LEFT,
756  const_cast<TriclopsInput *>(&triclopsColorInput),
757  &rectPackColImg );
759 
760  // Set image properties for reallocation
761  img_rows = rectPackColImg.nrows;
762  img_cols = rectPackColImg.ncols;
763  img_stride = rectPackColImg.rowinc;
764  dataPtr = (unsigned char*)rectPackColImg.data;
765  }
766  else // If not rectified
767  {
768  rgbuImage.GetDimensions(&img_rows,&img_cols,&img_stride);
769  dataPtr = rgbuImage.GetData();
770  }
771  // Convert PGR image ==> OpenCV format:
772  IplImage *tmpImage = cvCreateImage( cvSize( img_cols, img_rows ), IPL_DEPTH_8U, 4 );
773 
774  // Copy image data
775  memcpy( tmpImage->imageData, dataPtr, img_rows*img_stride );
776  tmpImage->widthStep = img_stride;
777  // Convert images to BGR (3 channels) and set origins
778  imageIpl[i] = cvCreateImage( cvSize( img_cols, img_rows ), IPL_DEPTH_8U, 3 );
779  cvCvtColor( tmpImage, imageIpl[i], CV_BGRA2BGR );
780  imageIpl[i]->origin = tmpImage->origin;
781  // Release temp images
782  cvReleaseImage( &tmpImage );
783  }
784 
785  /*-------------------------------------------------------------
786  Fill output stereo observation
787  -------------------------------------------------------------*/
788  out_observation.imageRight.setFromIplImage( imageIpl[0] ); // Right cam.
789  out_observation.imageLeft.setFromIplImage ( imageIpl[1] ); // Left cam.
790 
791  // It seems timestamp is not always correctly filled in the incoming imgs:
792  if (timestamp.seconds!=0)
793  out_observation.timestamp = mrpt::system::time_tToTimestamp( timestamp.seconds + 1e-6*timestamp.microSeconds );
794  else out_observation.timestamp = ts_retrieved;
795 
796  out_observation.rightCameraPose.x( m_baseline );
797  out_observation.rightCameraPose.y( 0 );
798  out_observation.rightCameraPose.z( 0 );
799 
800  out_observation.rightCameraPose.quat().r( 1 );
801  out_observation.rightCameraPose.quat().x( 0 );
802  out_observation.rightCameraPose.quat().y( 0 );
803  out_observation.rightCameraPose.quat().z( 0 );
804 
805  out_observation.cameraPose.x( 0 );
806  out_observation.cameraPose.y( 0 );
807  out_observation.cameraPose.z( 0 );
808 
809  out_observation.cameraPose.quat().r( 1 );
810  out_observation.cameraPose.quat().x( 0 );
811  out_observation.cameraPose.quat().y( 0 );
812  out_observation.cameraPose.quat().z( 0 );
813 
816  return true;
817  }
818  catch( std::exception &e)
819  {
820  std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Error:\n" << e.what() << std::endl;
821  return false;
822  }
823 #else
824  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2, Triclops or OpenCV")
825 #endif
826 }
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
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:52
bool getObservation(mrpt::obs::CObservationImage &out_observation)
Grab mono image from the camera.
float strobe_delay
(default=0.0) Delay in ms. Refer to PGR docs.
Declares a class derived from "CObservation" that encapsules an image from a camera, whose relative pose to robot is also stored.
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
bool strobe_enabled
(default=false) Enable the generation of a strobe signal in GPIO. Refer to PGR docs.
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)
T r() const
Return r coordinate of the quaternion.
Definition: CQuaternion.h:75
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...
float m_baseline
Camera baseline (only for stereo cameras)
#define CHECK_TRICLOPS_ERROR(_err)
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:77
#define THROW_EXCEPTION(msg)
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
float strobe_duration
(default=1.0) Pulse durationin ms. Refer to PGR docs.
#define TRI_CONTEXT
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
std::string grabmode
(Default="BUFFER_FRAMES") A string with a grab mode, from the list available in FlyCapture2::GrabMode...
#define FC2_CAM
GLenum mode
Definition: glew.h:5050
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...
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
mrpt::poses::CPose3DQuat cameraPose
The pose of the LEFT camera, relative to the robot.
GLdouble s
Definition: glew.h:1295
int grabTimeout
(Default=5000) Time in milliseconds that RetrieveBuffer() and WaitForBufferEvent() will wait for an i...
TCaptureOptions_FlyCapture2 m_options
Camera options.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
void * m_triclops
Opaque pointer to the TriclopsContext objetc. NULL if no context is active.
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:387
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. NULL if no camera is grabbing.
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...
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
GLhandleARB obj
Definition: glew.h:3276
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:365
bool stereo_mode
(default=false) Obtain images as stereo pairs with Flycapture2
GLfloat GLfloat p
Definition: glew.h:10113
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
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...
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. Where available, this should contain the accurate satellite-based time...
bool shutter_abs
(default=true) Numeric mode (absolute or integer values)
void * m_camera
Opaque pointer to the FlyCapture2::Camera object. NULL if no camera is grabbing.
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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::...
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:78
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...
GLenum GLsizei GLenum GLenum const GLvoid * image
Definition: glew.h:2881
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:76
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'th camera based on this 0-based index...
std::string BASE_IMPEXP trim(const std::string &str)
Removes leading and trailing spaces.
unsigned int trigger_polarity
(default=0) Refer to PGR docs.
GLuint GLfloat * val
Definition: glew.h:7785
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
bool autoexposure_onOff
(default=true) Activate this feature
mrpt::system::TTimeStamp BASE_IMPEXP 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:48
bool BASE_IMPEXP 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)
void BASE_IMPEXP tokenize(const std::string &inString, const std::string &inDelimiters, std::deque< std::string > &outTokens, bool skipBlankTokens=true) MRPT_NO_THROWS
Tokenizes a string according to a set of delimiting characters.
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:102
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.
unsigned int numBuffers
(Default=30) Number of images that can be stored in the buffer, if enabled with grabMode.
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092
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.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018