Main MRPT website > C++ reference for MRPT 1.5.6
CCameraSensor.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 
12 #include <mrpt/system/os.h>
15 #include <mrpt/system/filesystem.h>
17 #include <mrpt/utils/CConfigFile.h>
19 #include <mrpt/obs/CSensoryFrame.h>
20 #include <mrpt/obs/CRawlog.h>
23 #include <mrpt/gui/WxUtils.h>
24 #include <mrpt/gui/WxSubsystem.h>
25 
26 using namespace mrpt;
27 using namespace mrpt::hwdrivers;
28 using namespace mrpt::gui;
29 using namespace mrpt::utils;
30 using namespace mrpt::obs;
31 using namespace mrpt::system;
32 using namespace std;
33 
35 
36 /* -----------------------------------------------------
37  Constructor
38  ----------------------------------------------------- */
40  mrpt::utils::COutputLogger("CCameraSensor"),
41  m_sensorPose (),
42  m_grabber_type ("opencv"),
43  m_capture_grayscale (false),
44  m_cv_camera_index (0),
45  m_cv_camera_type ("CAMERA_CV_AUTODETECT"),
46  m_cv_options (),
47  // ---
48  m_dc1394_camera_guid (0),
49  m_dc1394_camera_unit (0),
50  m_dc1394_options (),
51  m_preview_decimation (0),
52  m_preview_reduction (1),
53  // ---
54  m_bumblebee_dc1394_camera_guid(0),
55  m_bumblebee_dc1394_camera_unit(0),
56  m_bumblebee_dc1394_framerate(15),
57  // ---
58  m_svs_camera_index(0),
59  m_svs_options (),
60  // ---
61  m_sr_open_from_usb (true),
62  m_sr_save_3d (true),
63  m_sr_save_range_img (true),
64  m_sr_save_intensity_img (true),
65  m_sr_save_confidence (true),
66  // ---
67  m_kinect_save_3d (true), // These options are also used for OpenNI2 grabber
68  m_kinect_save_range_img (true),
69  m_kinect_save_intensity_img(true),
70  m_kinect_video_rgb (true),
71  // ---
72  m_fcs_start_synch_capture(false),
73  // ---
74  m_img_dir_url (""),
75  m_img_dir_left_format ("imL_%04d.jpg"),
76  m_img_dir_right_format ("imR_%04d.jpg"),
77  m_img_dir_start_index (0),
78  m_img_dir_end_index (100),
79  m_img_dir_is_stereo (true),
80  m_img_dir_counter (0),
81  // ---
82  m_external_images_own_thread(false),
83  m_cap_cv (NULL),
84  m_cap_dc1394 (NULL),
85  m_cap_flycap (NULL),
86  m_cap_flycap_stereo_l(NULL),
87  m_cap_flycap_stereo_r(NULL),
88  m_cap_bumblebee_dc1394(NULL),
89  m_cap_svs (NULL),
90  m_cap_ffmpeg (NULL),
91  m_cap_rawlog (NULL),
92  m_cap_swissranger (NULL),
93  m_cap_kinect (NULL),
94  m_cap_openni2 (NULL),
95  m_cap_image_dir (NULL),
96  m_cap_duo3d (NULL),
97  m_camera_grab_decimator (0),
98  m_camera_grab_decimator_counter(0),
99  m_preview_counter (0),
100  m_external_image_saver_count( mrpt::system::getNumberOfProcessors() ),
101  m_threadImagesSaverShouldEnd(false),
102  m_hook_pre_save (NULL),
103  m_hook_pre_save_param(NULL)
104 {
105  m_sensorLabel = "CAMERA";
107 }
108 
109 /* -----------------------------------------------------
110  initialize
111  ----------------------------------------------------- */
113 {
114  cout << "[CCameraSensor::initialize] Opening camera..." << endl;
115  close();
116 
117  // Select type of device
118  m_grabber_type = trim( lowerCase( m_grabber_type ) );
119  m_cv_camera_type= trim( upperCase(m_cv_camera_type) );
120 
121  if (m_grabber_type=="opencv")
122  {
123  // OpenCV driver:
124  mrpt::utils::CTypeSelector camera_type(
125  "CAMERA_CV_AUTODETECT,CAMERA_CV_DC1394,CAMERA_CV_VFL,CAMERA_CV_VFW,CAMERA_CV_MIL",
126  "CAMERA_CV_AUTODETECT");
127 
128  int idx = camera_type.checkTypeIndex(m_cv_camera_type);
129  if (idx<0)
130  {
131  m_state = CGenericSensor::ssError;
132  THROW_EXCEPTION_FMT("Invalid value of :'%s'",m_cv_camera_type.c_str())
133  }
134 
135  cout << format("[CCameraSensor::initialize] opencv camera, index: %i type: %i...\n", int(m_cv_camera_index),idx);
136  m_cap_cv = new CImageGrabber_OpenCV( m_cv_camera_index, TCameraType(idx), m_cv_options );
137 
138  if (!m_cap_cv->isOpen())
139  {
140  m_state = CGenericSensor::ssError;
141  THROW_EXCEPTION("[CCameraSensor::initialize] ERROR: Couldn't open OpenCV camera.")
142  }
143  }
144  else if (m_grabber_type=="dc1394")
145  {
146  //m_cap_dc1394
147  cout << format("[CCameraSensor::initialize] dc1394 camera, GUID: 0x%lX UNIT:%d...\n", long(m_dc1394_camera_guid),m_dc1394_camera_unit);
148  m_cap_dc1394 = new CImageGrabber_dc1394( m_dc1394_camera_guid, m_dc1394_camera_unit, m_dc1394_options, true /* verbose */ );
149 
150  if (!m_cap_dc1394->isOpen())
151  {
152  m_state = CGenericSensor::ssError;
153  THROW_EXCEPTION("[CCameraSensor::initialize] ERROR: Couldn't open dc1394 camera.")
154  }
155  }
156  else if (m_grabber_type=="bumblebee_dc1394")
157  {
158  cout << format("[CCameraSensor::initialize] bumblebee_libdc1394 camera: GUID:0x%08X Index:%i FPS:%f...\n", (unsigned int)( m_bumblebee_dc1394_camera_guid ), m_bumblebee_dc1394_camera_unit, m_bumblebee_dc1394_framerate );
159  m_cap_bumblebee_dc1394 = new CStereoGrabber_Bumblebee_libdc1394( m_bumblebee_dc1394_camera_guid,m_bumblebee_dc1394_camera_unit, m_bumblebee_dc1394_framerate );
160  }
161  else if(m_grabber_type=="svs")
162  {
163  cout << format("[CCameraSensor::initialize] SVS camera: %u...\n", (unsigned int)( m_svs_camera_index ) );
164  m_cap_svs = new CStereoGrabber_SVS( m_svs_camera_index, m_svs_options );
165  }
166  else if (m_grabber_type=="ffmpeg")
167  {
168  //m_cap_ffmpeg
169  cout << format("[CCameraSensor::initialize] FFmpeg stream: %s...\n", m_ffmpeg_url.c_str() );
170  m_cap_ffmpeg = new CFFMPEG_InputStream();
171 
172  if (!m_cap_ffmpeg->openURL( m_ffmpeg_url, m_capture_grayscale ))
173  {
174  m_state = CGenericSensor::ssError;
175  THROW_EXCEPTION_FMT("Error opening FFmpeg stream: %s", m_ffmpeg_url.c_str())
176  }
177  }
178  else if (m_grabber_type=="swissranger")
179  {
180  cout << "[CCameraSensor::initialize] SwissRanger camera...\n";
181  m_cap_swissranger = new CSwissRanger3DCamera();
182 
183  m_cap_swissranger->setOpenFromUSB( m_sr_open_from_usb );
184  m_cap_swissranger->setOpenIPAddress(m_sr_ip_address);
185 
186  m_cap_swissranger->setSave3D(m_sr_save_3d);
187  m_cap_swissranger->setSaveRangeImage(m_sr_save_range_img);
188  m_cap_swissranger->setSaveIntensityImage(m_sr_save_intensity_img);
189  m_cap_swissranger->setSaveConfidenceImage(m_sr_save_confidence);
190 
191  if (!m_path_for_external_images.empty())
192  m_cap_swissranger->setPathForExternalImages( m_path_for_external_images );
193 
194  // Open it:
195  try
196  {
197  m_cap_swissranger->initialize(); // This will launch an exception if needed.
198  } catch (std::exception &)
199  {
200  m_state = CGenericSensor::ssError;
201  throw;
202  }
203  }
204  else if (m_grabber_type=="kinect")
205  {
206  cout << "[CCameraSensor::initialize] Kinect camera...\n";
207  m_cap_kinect = new CKinect();
208  m_cap_kinect->enableGrab3DPoints( m_kinect_save_3d );
209  m_cap_kinect->enableGrabDepth ( m_kinect_save_range_img );
210  m_cap_kinect->enableGrabRGB( m_kinect_save_intensity_img );
211  m_cap_kinect->setVideoChannel( m_kinect_video_rgb ? CKinect::VIDEO_CHANNEL_RGB : CKinect::VIDEO_CHANNEL_IR );
212 
213  if (!m_path_for_external_images.empty())
214  m_cap_kinect->setPathForExternalImages( m_path_for_external_images );
215 
216  // Open it:
217  try
218  {
219  m_cap_kinect->initialize(); // This will launch an exception if needed.
220  } catch (std::exception &)
221  {
222  m_state = CGenericSensor::ssError;
223  throw;
224  }
225  }
226  else if (m_grabber_type=="openni2")
227  {
228  cout << "[CCameraSensor::initialize] OpenNI2 sensor...\n";
229  m_cap_openni2 = new COpenNI2Sensor();
230  m_cap_openni2->enableGrab3DPoints( m_kinect_save_3d ); // It uses the same options as the Kinect grabber
231  m_cap_openni2->enableGrabDepth ( m_kinect_save_range_img );
232  m_cap_openni2->enableGrabRGB( m_kinect_save_intensity_img );
233 
234  if (!m_path_for_external_images.empty())
235  m_cap_openni2->setPathForExternalImages( m_path_for_external_images );
236 
237  // Open it:
238  try
239  {
240  m_cap_openni2->initialize(); // This will launch an exception if needed.
241  } catch (std::exception &e)
242  {
243  m_state = CGenericSensor::ssError;
244  throw e;
245  }
246  }
247  else if (m_grabber_type=="image_dir" )
248  {
249  //m_cap_image_dir
250  cout << format("[CCameraSensor::initialize] Image dir: %s...\n", m_img_dir_url.c_str() );
251  m_cap_image_dir = new std::string();
252  }
253  else if (m_grabber_type=="rawlog")
254  {
255  //m_cap_rawlog
256  cout << format("[CCameraSensor::initialize] Rawlog stream: %s...\n", m_rawlog_file.c_str() );
257  m_cap_rawlog = new CFileGZInputStream();
258 
259  if (! m_cap_rawlog->open(m_rawlog_file) )
260  {
261  m_state = CGenericSensor::ssError;
262  THROW_EXCEPTION_FMT("Error opening rawlog file: %s", m_rawlog_file.c_str())
263  }
264  // File open OK.
265  // Localize the external images directory of this rawlog, if it exists:
266  m_rawlog_detected_images_dir = CRawlog::detectImagesDirectory(m_rawlog_file);
267  }
268  else if (m_grabber_type=="flycap")
269  {
270  cout << "[CCameraSensor::initialize] PGR FlyCapture2 camera...\n";
271  try
272  {
273  // Open camera and start capture:
274  m_cap_flycap = new CImageGrabber_FlyCapture2( m_flycap_options );
275  } catch (std::exception &)
276  {
277  m_state = CGenericSensor::ssError;
278  throw;
279  }
280  }
281  else if (m_grabber_type=="flycap_stereo")
282  {
283  cout << "[CCameraSensor::initialize] PGR FlyCapture2 stereo camera...\n";
284  try
285  {
286  // Open camera and start capture:
287  m_cap_flycap_stereo_l = new CImageGrabber_FlyCapture2();
288  m_cap_flycap_stereo_r = new CImageGrabber_FlyCapture2();
289 
290  cout << "[CCameraSensor::initialize] PGR FlyCapture2 stereo camera: Opening LEFT camera...\n";
291  m_cap_flycap_stereo_l->open(m_flycap_stereo_options[0], false /* don't start grabbing */ );
292 
293  cout << "[CCameraSensor::initialize] PGR FlyCapture2 stereo camera: Opening RIGHT camera...\n";
294  m_cap_flycap_stereo_r->open(m_flycap_stereo_options[1], false /* don't start grabbing */ );
295 
296  // Now, start grabbing "simultaneously":
297  if (m_fcs_start_synch_capture)
298  {
299  const CImageGrabber_FlyCapture2 *cams[2];
300  cams[0] = m_cap_flycap_stereo_l;
301  cams[1] = m_cap_flycap_stereo_r;
303  }
304  else
305  {
306  m_cap_flycap_stereo_l->startCapture();
307  m_cap_flycap_stereo_r->startCapture();
308  }
309 
310  } catch (std::exception &)
311  {
312  m_state = CGenericSensor::ssError;
313  throw;
314  }
315  }
316  else if (m_grabber_type=="duo3d")
317  {
318  //m_cap_duo3D
319  cout << format("[CCameraSensor::initialize] DUO3D stereo camera ...\n" );
320 
321  // Open it:
322  try
323  {
324  m_cap_duo3d = new CDUO3DCamera( m_duo3d_options );
325  } catch (std::exception &e)
326  {
327  m_state = CGenericSensor::ssError;
328  throw e;
329  }
330  }
331  else
332  THROW_EXCEPTION_FMT("Unknown 'grabber_type' found: %s", m_grabber_type.c_str() )
333 
334  // Change state:
335  cout << "[CCameraSensor::initialize] Done!" << endl;
336  m_state = CGenericSensor::ssWorking;
337 
338 
339  // Launch independent thread?
340  if (m_external_images_own_thread)
341  {
342  m_threadImagesSaverShouldEnd = false;
343 
344  m_threadImagesSaver.clear();
345  m_threadImagesSaver.resize(m_external_image_saver_count);
346 
347  m_toSaveList.clear();
348  m_toSaveList.resize(m_external_image_saver_count);
349 
350  for (unsigned int i=0;i<m_external_image_saver_count;++i)
352 
353  }
354 
355 }
356 
357 /* -----------------------------------------------------
358  close
359  ----------------------------------------------------- */
361 {
362  delete_safe(m_cap_cv);
363  delete_safe(m_cap_dc1394);
364  delete_safe(m_cap_flycap);
365  delete_safe(m_cap_flycap_stereo_l);
366  delete_safe(m_cap_flycap_stereo_r);
367  delete_safe(m_cap_bumblebee_dc1394);
368  delete_safe(m_cap_ffmpeg);
369  delete_safe(m_cap_rawlog);
370  delete_safe(m_cap_swissranger);
371  delete_safe(m_cap_kinect);
372  delete_safe(m_cap_svs);
373  delete_safe(m_cap_image_dir);
374  delete_safe(m_cap_duo3d);
375 
377 
378  // Wait for threads:
379  if (!m_threadImagesSaver.empty())
380  {
381  m_threadImagesSaverShouldEnd = true;
382  for (size_t i=0;i<m_threadImagesSaver.size();i++)
383  mrpt::system::joinThread( m_threadImagesSaver[i] );
384  }
385 }
386 
387 /* -----------------------------------------------------
388  loadConfig_sensorSpecific
389  ----------------------------------------------------- */
391  const mrpt::utils::CConfigFileBase &configSource,
392  const std::string &iniSection )
393 {
394  // At this point, my parent class CGenericSensor has already loaded its params:
395  // Since cameras are special, we'll take control over "m_grab_decimation" so
396  // external image files are not saved just to be discarded later on...
397  if (m_grab_decimation>0)
398  {
399  m_camera_grab_decimator = m_grab_decimation;
400  m_camera_grab_decimator_counter = 0;
401  // Reset in parent:
402  m_grab_decimation = 0;
403  }
404  else
405  m_camera_grab_decimator = m_camera_grab_decimator_counter = 0;
406 
407  m_grabber_type = configSource.read_string_first_word( iniSection, "grabber_type", m_grabber_type );
408  MRPT_LOAD_HERE_CONFIG_VAR( preview_decimation, int, m_preview_decimation , configSource, iniSection )
409  MRPT_LOAD_HERE_CONFIG_VAR( preview_reduction, int, m_preview_reduction, configSource, iniSection )
410 
411  // OpenCV options:
412  m_cv_camera_type= configSource.read_string_first_word( iniSection, "cv_camera_type", m_cv_camera_type );
413  m_cv_camera_index = configSource.read_int( iniSection, "cv_camera_index", m_cv_camera_index);
414 
415  m_cv_options.frame_width = configSource.read_int( iniSection, "cv_frame_width", m_cv_options.frame_width );
416  m_cv_options.frame_height = configSource.read_int( iniSection, "cv_frame_height", m_cv_options.frame_height );
417  m_cv_options.gain = configSource.read_double( iniSection, "cv_gain", m_cv_options.gain );
418  m_cv_options.ieee1394_fps = configSource.read_double( iniSection, "cv_fps", m_cv_options.ieee1394_fps );
419 
420  m_capture_grayscale = configSource.read_bool( iniSection, "capture_grayscale", false );
421 
422  m_cv_options.ieee1394_grayscale = m_capture_grayscale;
423 
424  // dc1394 options:
425  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_camera_guid, uint64_t, m_dc1394_camera_guid, configSource, iniSection )
426  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_camera_unit, int, m_dc1394_camera_unit, configSource, iniSection )
427 
428  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_frame_width, int, m_dc1394_options.frame_width, configSource, iniSection )
429  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_frame_height, int, m_dc1394_options.frame_height, configSource, iniSection )
430 
431  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_mode7, int, m_dc1394_options.mode7, configSource, iniSection )
432 
433  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_shutter, int, m_dc1394_options.shutter, configSource, iniSection )
434  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_gain, int, m_dc1394_options.gain, configSource, iniSection )
435  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_gamma, int, m_dc1394_options.gamma, configSource, iniSection )
436  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_brightness, int, m_dc1394_options.brightness, configSource, iniSection )
437  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_exposure, int, m_dc1394_options.exposure, configSource, iniSection )
438  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_sharpness, int, m_dc1394_options.sharpness, configSource, iniSection )
439  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_white_balance, int, m_dc1394_options.white_balance, configSource, iniSection )
440 
441  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_shutter_mode, int, m_dc1394_options.shutter_mode, configSource, iniSection )
442  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_gain_mode, int, m_dc1394_options.gain_mode, configSource, iniSection )
443  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_gamma_mode, int, m_dc1394_options.gamma_mode, configSource, iniSection )
444  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_brightness_mode, int, m_dc1394_options.brightness_mode, configSource, iniSection )
445  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_exposure_mode, int, m_dc1394_options.exposure_mode, configSource, iniSection )
446  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_sharpness_mode, int, m_dc1394_options.sharpness_mode, configSource, iniSection )
447  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_white_balance_mode, int, m_dc1394_options.white_balance_mode, configSource, iniSection )
448 
449  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_trigger_power, int, m_dc1394_options.trigger_power, configSource, iniSection )
450  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_trigger_mode, int, m_dc1394_options.trigger_mode, configSource, iniSection )
451  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_trigger_source, int, m_dc1394_options.trigger_source, configSource, iniSection )
452  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_trigger_polarity, int, m_dc1394_options.trigger_polarity, configSource, iniSection )
453  MRPT_LOAD_HERE_CONFIG_VAR( dc1394_ring_buffer_size, int, m_dc1394_options.ring_buffer_size, configSource, iniSection )
454 
455  // Bumblebee_dc1394 options:
456  MRPT_LOAD_HERE_CONFIG_VAR( bumblebee_dc1394_camera_guid, uint64_t, m_bumblebee_dc1394_camera_guid, configSource, iniSection )
457  MRPT_LOAD_HERE_CONFIG_VAR( bumblebee_dc1394_camera_unit, int, m_bumblebee_dc1394_camera_unit, configSource, iniSection )
458  MRPT_LOAD_HERE_CONFIG_VAR( bumblebee_dc1394_framerate, double, m_bumblebee_dc1394_framerate, configSource, iniSection )
459 
460  // SVS options:
461  m_svs_camera_index = configSource.read_int( iniSection, "svs_camera_index", m_svs_camera_index );
462  m_svs_options.frame_width = configSource.read_int( iniSection, "svs_frame_width", m_svs_options.frame_width );
463  m_svs_options.frame_height = configSource.read_int( iniSection, "svs_frame_height", m_svs_options.frame_height );
464  m_svs_options.framerate = configSource.read_double(iniSection,"svs_framerate",m_svs_options.framerate );
465  m_svs_options.m_NDisp = configSource.read_int( iniSection, "svs_NDisp", m_svs_options.m_NDisp );
466  m_svs_options.m_Corrsize = configSource.read_int( iniSection, "svs_Corrsize", m_svs_options.m_Corrsize );
467  m_svs_options.m_LR = configSource.read_int( iniSection, "svs_LR", m_svs_options.m_LR );
468  m_svs_options.m_Thresh = configSource.read_int( iniSection, "svs_Thresh", m_svs_options.m_Thresh );
469  m_svs_options.m_Unique = configSource.read_int( iniSection, "svs_Unique", m_svs_options.m_Unique );
470  m_svs_options.m_Horopter = configSource.read_int( iniSection, "svs_Horopter", m_svs_options.m_Horopter );
471  m_svs_options.m_SpeckleSize = configSource.read_int( iniSection, "svs_SpeckleSize", m_svs_options.m_SpeckleSize );
472  m_svs_options.m_procesOnChip = configSource.read_bool( iniSection, "svs_procesOnChip",m_svs_options.m_procesOnChip);
473  m_svs_options.m_calDisparity = configSource.read_bool( iniSection, "svs_calDisparity",m_svs_options.m_calDisparity);
474 
475  // FFmpeg options:
476  m_ffmpeg_url = mrpt::system::trim( configSource.read_string( iniSection, "ffmpeg_url", m_ffmpeg_url ) );
477 
478  // Rawlog options:
479  m_rawlog_file = mrpt::system::trim( configSource.read_string( iniSection, "rawlog_file", m_rawlog_file ) );
480  m_rawlog_camera_sensor_label = mrpt::system::trim( configSource.read_string( iniSection, "rawlog_camera_sensor_label", m_rawlog_camera_sensor_label ) );
481 
482  // Image directory options:
483  m_img_dir_url = mrpt::system::trim( configSource.read_string( iniSection, "image_dir_url", m_img_dir_url ) );
484  m_img_dir_left_format = mrpt::system::trim( configSource.read_string( iniSection, "left_format", m_img_dir_left_format ) );
485  m_img_dir_right_format = mrpt::system::trim( configSource.read_string( iniSection, "right_format", "" ) );
486  m_img_dir_start_index = configSource.read_int( iniSection, "start_index", m_img_dir_start_index );;
487  m_img_dir_end_index = configSource.read_int( iniSection, "end_index", m_img_dir_end_index );
488 
489  m_img_dir_is_stereo = !m_img_dir_right_format.empty();
490  m_img_dir_counter = m_img_dir_start_index;
491 
492  // DUO3D Camera options:
493  m_duo3d_options.loadOptionsFrom( configSource, "DUO3DOptions" );
494 
495  // SwissRanger options:
496  m_sr_open_from_usb = configSource.read_bool( iniSection, "sr_use_usb", m_sr_open_from_usb );
497  m_sr_ip_address = configSource.read_string( iniSection, "sr_IP", m_sr_ip_address );
498 
499  m_sr_save_3d = configSource.read_bool( iniSection, "sr_grab_3d", m_sr_save_3d );
500  m_sr_save_intensity_img = configSource.read_bool( iniSection, "sr_grab_grayscale", m_sr_save_intensity_img );
501  m_sr_save_range_img = configSource.read_bool( iniSection, "sr_grab_range", m_sr_save_range_img );
502  m_sr_save_confidence = configSource.read_bool( iniSection, "sr_grab_confidence", m_sr_save_confidence );
503 
504  m_kinect_save_3d = configSource.read_bool( iniSection, "kinect_grab_3d", m_kinect_save_3d );
505  m_kinect_save_intensity_img = configSource.read_bool( iniSection, "kinect_grab_intensity", m_kinect_save_intensity_img );
506  m_kinect_save_range_img = configSource.read_bool( iniSection, "kinect_grab_range", m_kinect_save_range_img );
507  m_kinect_video_rgb = configSource.read_bool( iniSection, "kinect_video_rgb", m_kinect_video_rgb);
508 
509  // FlyCap:
510  m_flycap_options.loadOptionsFrom(configSource,iniSection, "flycap_");
511 
512  // FlyCap stereo
513  m_fcs_start_synch_capture = configSource.read_bool( iniSection, "fcs_start_synch_capture", m_fcs_start_synch_capture );
514  m_flycap_stereo_options[0].loadOptionsFrom(configSource,iniSection, "fcs_LEFT_");
515  m_flycap_stereo_options[1].loadOptionsFrom(configSource,iniSection, "fcs_RIGHT_");
516 
517  // Special stuff: FPS
518  map<double,grabber_dc1394_framerate_t> map_fps;
520  map_fps[1.875] = FRAMERATE_1_875;
521  map_fps[3.75] = FRAMERATE_3_75;
522  map_fps[7.5] = FRAMERATE_7_5;
523  map_fps[15] = FRAMERATE_15;
524  map_fps[30] = FRAMERATE_30;
525  map_fps[60] = FRAMERATE_60;
526  map_fps[120] = FRAMERATE_120;
527  map_fps[240] = FRAMERATE_240;
528 
529  // ... for dc1394
530  double the_fps = configSource.read_double( iniSection, "dc1394_framerate", 15.0 );
531  it_fps = map_fps.find( the_fps );
532  if ( it_fps == map_fps.end() )
533  THROW_EXCEPTION_FMT("ERROR: DC1394 framerate seems to be not a valid number: %f",the_fps);
534 
535  m_dc1394_options.framerate = it_fps->second;
536 
537  // Special stuff: color encoding:
538  map<string,grabber_dc1394_color_coding_t> map_color;
540 #define ADD_COLOR_MAP(c) map_color[#c] = c;
547 
548  string the_color_coding = mrpt::system::upperCase( configSource.read_string_first_word( iniSection, "dc1394_color_coding", "COLOR_CODING_YUV422" ) );
549  it_color = map_color.find(the_color_coding );
550  if (it_color == map_color.end())
551  THROW_EXCEPTION_FMT("ERROR: Color coding seems not to be valid : '%s'",the_color_coding.c_str() );
552  m_dc1394_options.color_coding = it_color->second;
553 
554 
555  m_external_images_format = mrpt::system::trim( configSource.read_string( iniSection, "external_images_format", m_external_images_format ) );
556  m_external_images_jpeg_quality = configSource.read_int( iniSection, "external_images_jpeg_quality", m_external_images_jpeg_quality );
557  m_external_images_own_thread = configSource.read_bool( iniSection, "external_images_own_thread", m_external_images_own_thread );
558  m_external_image_saver_count = configSource.read_int( iniSection, "external_images_own_thread_count", m_external_image_saver_count );
559 
560  // Sensor pose:
561  m_sensorPose.setFromValues(
562  configSource.read_float(iniSection,"pose_x",0),
563  configSource.read_float(iniSection,"pose_y",0),
564  configSource.read_float(iniSection,"pose_z",0),
565  DEG2RAD( configSource.read_float(iniSection,"pose_yaw",0) ),
566  DEG2RAD( configSource.read_float(iniSection,"pose_pitch",0) ),
567  DEG2RAD( configSource.read_float(iniSection,"pose_roll",0) )
568  );
569 }
570 
571 /* -----------------------------------------------------
572  Destructor
573  ----------------------------------------------------- */
575 {
576  close();
577 
578  m_preview_win1.clear();
579  m_preview_win2.clear();
580 }
581 /* -----------------------------------------------------
582  getNextFrame
583 ----------------------------------------------------- */
584 CObservationPtr CCameraSensor::getNextFrame( )
585 {
586  vector<CSerializablePtr> out_obs;
587  getNextFrame(out_obs);
588  return static_cast<CObservationPtr>(out_obs[0]);
589 }
590 
591 /* -----------------------------------------------------
592  getNextFrame
593 ----------------------------------------------------- */
594 void CCameraSensor::getNextFrame( vector<CSerializablePtr> & out_obs )
595 {
596  CObservationImagePtr obs;
597  CObservationStereoImagesPtr stObs;
598  CObservation3DRangeScanPtr obs3D; // 3D range image, also with an intensity channel
599  CObservationIMUPtr obsIMU; // IMU observation grabbed by DUO3D cameras
600 
601  bool capture_ok = false;
602 
603  if (m_cap_cv)
604  {
605  obs = CObservationImage::Create();
606  if (!m_cap_cv->getObservation( *obs ))
607  { // Error
608  m_state = CGenericSensor::ssError;
609  THROW_EXCEPTION("Error grabbing image");
610  }
611  else capture_ok = true;
612  }
613  else if (m_cap_dc1394)
614  {
615  obs = CObservationImage::Create();
616  if (!m_cap_dc1394->getObservation( *obs ))
617  { // Error
618  m_state = CGenericSensor::ssError;
619  THROW_EXCEPTION("Error grabbing image");
620  }
621  else capture_ok = true;
622  }
623  else if (m_cap_swissranger)
624  {
625  obs3D = CObservation3DRangeScan::Create();
626 
627  bool there_is_obs, hardware_error;
628  m_cap_swissranger->getNextObservation(*obs3D, there_is_obs, hardware_error);
629 
630  if (!there_is_obs || hardware_error)
631  { // Error
632  m_state = CGenericSensor::ssError;
633  THROW_EXCEPTION("Error grabbing image from SwissRanger camera.");
634  }
635  else capture_ok = true;
636  }
637  else if (m_cap_kinect)
638  {
639  obs3D = CObservation3DRangeScan::Create();
640 
641  // Specially at start-up, there may be a delay grabbing so a few calls return false: add a timeout.
643  double max_timeout = 3.0; // seconds
644 
645  // If we have an "MRPT_CCAMERA_KINECT_TIMEOUT_MS" environment variable, use that timeout instead:
646  const char *envVal = getenv("MRPT_CCAMERA_KINECT_TIMEOUT_MS");
647  if (envVal) max_timeout = atoi(envVal)*0.001;
648 
649  bool there_is_obs, hardware_error;
650  do
651  {
652  m_cap_kinect->getNextObservation(*obs3D, there_is_obs, hardware_error);
653  if (!there_is_obs) mrpt::system::sleep(1);
654  } while (!there_is_obs && mrpt::system::timeDifference(t0,mrpt::system::now())<max_timeout);
655 
656  if (!there_is_obs || hardware_error)
657  { // Error
658  m_state = CGenericSensor::ssError;
659  THROW_EXCEPTION("Error grabbing image from Kinect camera.");
660  }
661  else capture_ok = true;
662  }
663  else if (m_cap_openni2)
664  {
665  obs3D = CObservation3DRangeScan::Create();
666  // Specially at start-up, there may be a delay grabbing so a few calls return false: add a timeout.
668  double max_timeout = 3.0; // seconds
669  bool there_is_obs, hardware_error;
670  do
671  {
672  m_cap_openni2->getNextObservation(*obs3D, there_is_obs, hardware_error);
673  if (!there_is_obs) mrpt::system::sleep(1);
674  } while (!there_is_obs && mrpt::system::timeDifference(t0,mrpt::system::now())<max_timeout);
675 
676  if (!there_is_obs || hardware_error)
677  { // Error
678  m_state = CGenericSensor::ssError;
679  THROW_EXCEPTION("Error grabbing image from OpenNI2 camera.");
680  }
681  else capture_ok = true;
682  }
683  else if (m_cap_bumblebee_dc1394)
684  {
685  stObs = CObservationStereoImages::Create();
686  if (!m_cap_bumblebee_dc1394->getStereoObservation( *stObs )) {
687  m_state = CGenericSensor::ssError;
688  THROW_EXCEPTION("Error grabbing stereo images");
689  }
690  else {
691  capture_ok = true;
692  }
693  }
694  else if (m_cap_svs)
695  {
696  stObs = CObservationStereoImages::Create();
697 
698  if(!m_cap_svs->getStereoObservation(*stObs))
699  {
700  // Error
701  m_state = CGenericSensor::ssError;
702  THROW_EXCEPTION("Error grabbing disparity images");
703 
704  }
705  else capture_ok = true;
706 
707  }
708  else if (m_cap_ffmpeg)
709  {
710  obs = CObservationImage::Create();
711 
712  if (!m_cap_ffmpeg->retrieveFrame( obs->image ))
713  { // Error
714  m_state = CGenericSensor::ssError;
715  THROW_EXCEPTION("Error grabbing image");
716  }
717  else capture_ok = true;
718  }
719  else if (m_cap_image_dir)
720  {
721  if( m_img_dir_counter > m_img_dir_end_index )
722  {
723  m_state = CGenericSensor::ssError;
724  THROW_EXCEPTION("Reached end index.");
725  }
726 
727  std::string auxL = format( "%s/%s", m_img_dir_url.c_str(), m_img_dir_left_format.c_str() );
728  if( m_img_dir_is_stereo )
729  {
730 
731  stObs = CObservationStereoImages::Create();
732  if( !stObs->imageLeft.loadFromFile( format(auxL.c_str(), m_img_dir_counter) ) )
733  {
734  m_state = CGenericSensor::ssError;
735  THROW_EXCEPTION("Error reading images from directory");
736  }
737  std::string auxR = format( "%s/%s", m_img_dir_url.c_str(), m_img_dir_right_format.c_str() );
738  if( !stObs->imageRight.loadFromFile( format(auxR.c_str(), m_img_dir_counter++)) )
739  {
740  m_state = CGenericSensor::ssError;
741  THROW_EXCEPTION("Error reading images from directory");
742  }
743  else capture_ok = true;
744  }
745  else
746  {
747  // use only left image prefix
748  obs = CObservationImage::Create();
749  if( !obs->image.loadFromFile( format(auxL.c_str(), m_img_dir_counter++) ) )
750  {
751  m_state = CGenericSensor::ssError;
752  THROW_EXCEPTION("Error reading images from directory");
753  }
754  else capture_ok = true;
755  }
756  }
757  else if (m_cap_rawlog)
758  {
759  // Read in a loop until we found at least one image:
760  // Assign to: obs && stObs
761  CSerializablePtr newObs;
762  while (!obs.present() && !stObs.present() && !obs3D.present())
763  {
764  *m_cap_rawlog >> newObs;
765  if (IS_DERIVED( newObs, CObservation) )
766  {
767  CObservationPtr o = CObservationPtr(newObs);
768  if (!m_rawlog_camera_sensor_label.empty() && m_rawlog_camera_sensor_label!=o->sensorLabel)
769  continue;
770 
772  obs = CObservationImagePtr(o);
774  stObs = CObservationStereoImagesPtr(o);
775  else if (IS_CLASS(o,CObservation3DRangeScan))
776  obs3D = CObservation3DRangeScanPtr(o);
777  }
778  else if (IS_CLASS( newObs, CSensoryFrame) )
779  {
780  CSensoryFramePtr sf = CSensoryFramePtr(newObs);
781 
782  for (CSensoryFrame::iterator i=sf->begin();i!=sf->end();++i)
783  {
784  CObservationPtr &o = *i;
785 
786  if (!m_rawlog_camera_sensor_label.empty() && m_rawlog_camera_sensor_label!=o->sensorLabel)
787  continue;
788 
790  {
791  obs = CObservationImagePtr(o);
792  break;
793  }
795  {
796  stObs = CObservationStereoImagesPtr(o);
797  break;
798  }
799  else if (IS_CLASS(o,CObservation3DRangeScan))
800  {
801  obs3D = CObservation3DRangeScanPtr(o);
802  break;
803  }
804  }
805  }
806  if (obs || stObs || obs3D)
807  {
808  // We must convert externally stored images into "normal in-memory" images.
809  const std::string old_dir = CImage::IMAGES_PATH_BASE; // Save current
810  CImage::IMAGES_PATH_BASE = m_rawlog_detected_images_dir;
811 
812  if (obs && obs->image.isExternallyStored())
813  obs->image.loadFromFile( obs->image.getExternalStorageFileAbsolutePath() );
814 
815  if (obs3D && obs3D->hasIntensityImage && obs3D->intensityImage.isExternallyStored())
816  obs3D->intensityImage.loadFromFile( obs3D->intensityImage.getExternalStorageFileAbsolutePath() );
817 
818  if (stObs && stObs->imageLeft.isExternallyStored())
819  stObs->imageLeft.loadFromFile( stObs->imageLeft.getExternalStorageFileAbsolutePath() );
820 
821  if (stObs && stObs->hasImageRight && stObs->imageRight.isExternallyStored())
822  stObs->imageRight.loadFromFile( stObs->imageRight.getExternalStorageFileAbsolutePath() );
823 
824  if (stObs && stObs->hasImageDisparity && stObs->imageDisparity.isExternallyStored())
825  stObs->imageDisparity.loadFromFile( stObs->imageDisparity.getExternalStorageFileAbsolutePath() );
826 
827  CImage::IMAGES_PATH_BASE = old_dir; // Restore
828  }
829  else continue; // Keep reading
830  }
831  capture_ok = true;
832  }
833  else if (m_cap_flycap)
834  {
835  bool ok;
836  if (!m_cap_flycap->isStereo()) // Mono image
837  {
838  obs = CObservationImage::Create();
839  ok = m_cap_flycap->getObservation(*obs);
840  }
841  else // Stereo camera connected
842  {
843  stObs = CObservationStereoImages::Create();
844  ok = m_cap_flycap->getObservation(*stObs);
845  }
846 
847  if (!ok)
848  { // Error
849  m_state = CGenericSensor::ssError;
850  THROW_EXCEPTION("Error grabbing image");
851  }
852  else capture_ok = true;
853  }
854  else if (m_cap_flycap_stereo_l && m_cap_flycap_stereo_r)
855  {
856  stObs = CObservationStereoImages::Create();
857 
858  CObservationImage obsL,obsR;
859 
860  bool ok1, ok2=false;
861 
862  ok1 = m_cap_flycap_stereo_r->getObservation(obsL);
863  if (ok1)
864  ok2 = m_cap_flycap_stereo_l->getObservation(obsR);
865 
866  if (!ok1 || !ok2)
867  {
868  // Error
869  m_state = CGenericSensor::ssError;
870  THROW_EXCEPTION("Error grabbing disparity images");
871  }
872  else
873  {
874  // Joint the two images as one stereo:
875  const double At = mrpt::system::timeDifference(obsL.timestamp,obsR.timestamp);
876  if (std::abs(At)>0.1) {
877  cout << "[CCamera, flycap_stereo] Warning: Too large delay between left & right images: " << At << " sec.\n";
878  }
879 
880  // It seems that the timestamp is not always filled in from FlyCap driver?
881  stObs->timestamp = (obsL.timestamp!=0) ? obsL.timestamp : mrpt::system::now();
882  stObs->imageLeft.copyFastFrom(obsL.image);
883  stObs->imageRight.copyFastFrom(obsR.image);
884  capture_ok = true;
885  }
886  }
887  else if( m_cap_duo3d )
888  {
889  stObs = CObservationStereoImages::Create();
890  obsIMU = CObservationIMU::Create();
891 
892  bool thereIsIMG, thereIsIMU;
893  m_cap_duo3d->getObservations(*stObs,*obsIMU,thereIsIMG,thereIsIMU);
894  if( !thereIsIMG )
895  {
896  m_state = CGenericSensor::ssError;
897  THROW_EXCEPTION("Error getting observations from DUO3D camera.");
898  }
899  else if( m_cap_duo3d->captureIMUIsSet() && !thereIsIMU )
900  {
901  cout << "[CCamera, duo3d] Warning: There are no IMU data from the device. Only images are being grabbed.";
902  }
903  capture_ok = true;
904  }
905  else
906  {
907  THROW_EXCEPTION("There is no initialized camera driver: has 'initialize()' been called?")
908  }
909 
910  ASSERT_(capture_ok)
911 
912  // Are we supposed to do a decimation??
913  m_camera_grab_decimator_counter++;
914  if (m_camera_grab_decimator_counter<m_camera_grab_decimator)
915  {
916  // Done here:
917  out_obs.push_back( CObservationPtr() );
918  return;
919  }
920  // Continue as normal:
921  m_camera_grab_decimator_counter = 0;
922 
923  ASSERT_(obs || stObs || obs3D || obsIMU)
924  // If we grabbed an image: prepare it and add it to the internal queue:
925  if (obs) {
926  obs->sensorLabel = m_sensorLabel;
927  obs->setSensorPose( m_sensorPose );
928  }
929  else if (stObs) {
930  stObs->sensorLabel = (m_cap_duo3d && m_cap_duo3d->captureIMUIsSet()) ? m_sensorLabel + "_IMG" : m_sensorLabel;
931  stObs->setSensorPose( m_sensorPose );
932  }
933  else if (obs3D) {
934  obs3D->sensorLabel = m_sensorLabel;
935  obs3D->setSensorPose( m_sensorPose );
936  }
937  if (obsIMU) {
938  obsIMU->sensorLabel = m_sensorLabel + "_IMU";
939  obsIMU->setSensorPose( m_sensorPose );
940  }
941 
942  // Convert to grayscale if the user wants so and the driver did ignored us:
943  if (m_capture_grayscale)
944  {
945  if (obs)
946  {
947  if (obs->image.isColor()) obs->image.grayscaleInPlace();
948  }
949  else if (stObs)
950  {
951  if (stObs->imageLeft.isColor()) stObs->imageLeft.grayscaleInPlace();
952  if (stObs->hasImageRight && stObs->imageRight.isColor()) stObs->imageRight.grayscaleInPlace();
953  if (stObs->hasImageDisparity && stObs->imageDisparity.isColor()) stObs->imageDisparity.grayscaleInPlace(); // Shouldn't happen, but...
954  }
955  else if (obs3D)
956  {
957  if (obs3D->hasIntensityImage && obs3D->intensityImage.isColor()) obs3D->intensityImage.grayscaleInPlace();
958  }
959  }
960  // External storage?
961  bool delayed_insertion_in_obs_queue = false; // If true, we'll return nothing, but the observation will be inserted from the thread.
962 
963  if (!m_path_for_external_images.empty())
964  {
965  if( stObs ) // If we have grabbed an stereo observation ...
966  { // Stereo obs -------
967  if (m_external_images_own_thread)
968  {
969  m_csToSaveList.enter();
970 
971  // Select the "m_toSaveList" with the shortest pending queue:
972  size_t idx_min = 0;
973  for (size_t i=0;i<m_toSaveList.size();++i)
974  if (m_toSaveList[i].size()<m_toSaveList[idx_min].size())
975  idx_min = i;
976  // Insert:
977  m_toSaveList[idx_min].insert( TListObsPair( stObs->timestamp, stObs ) );
978 
979  m_csToSaveList.leave();
980 
981  delayed_insertion_in_obs_queue = true;
982  }
983  else
984  {
985  const string filNameL = fileNameStripInvalidChars( trim(m_sensorLabel) ) + format( "_L_%f.%s", (double)timestampTotime_t( stObs->timestamp ), m_external_images_format.c_str() );
986  const string filNameR = fileNameStripInvalidChars( trim(m_sensorLabel) ) + format( "_R_%f.%s", (double)timestampTotime_t( stObs->timestamp ), m_external_images_format.c_str() );
987  const string filNameD = fileNameStripInvalidChars( trim(m_sensorLabel) ) + format( "_D_%f.%s", (double)timestampTotime_t( stObs->timestamp ), m_external_images_format.c_str() );
988  //cout << "[CCameraSensor] Saving " << filName << endl;
989  stObs->imageLeft.saveToFile( m_path_for_external_images + string("/") + filNameL, m_external_images_jpeg_quality );
990  stObs->imageLeft.setExternalStorage( filNameL );
991 
992  if (stObs->hasImageRight) {
993  stObs->imageRight.saveToFile( m_path_for_external_images + string("/") + filNameR, m_external_images_jpeg_quality );
994  stObs->imageRight.setExternalStorage( filNameR );
995  }
996  if (stObs->hasImageDisparity) {
997  stObs->imageDisparity.saveToFile( m_path_for_external_images + string("/") + filNameD, m_external_images_jpeg_quality );
998  stObs->imageDisparity.setExternalStorage( filNameD );
999  }
1000  }
1001  }
1002  else if (obs)
1003  { // Monocular image obs -------
1004  if (m_external_images_own_thread)
1005  {
1006  m_csToSaveList.enter();
1007 
1008  // Select the "m_toSaveList" with the shortest pending queue:
1009  size_t idx_min = 0;
1010  for (size_t i=0;i<m_toSaveList.size();++i)
1011  if (m_toSaveList[i].size()<m_toSaveList[idx_min].size())
1012  idx_min = i;
1013 
1014  // Insert:
1015  m_toSaveList[idx_min].insert( TListObsPair( obs->timestamp, obs ) );
1016 
1017  m_csToSaveList.leave();
1018  delayed_insertion_in_obs_queue = true;
1019  }
1020  else
1021  {
1022  string filName = fileNameStripInvalidChars( trim(m_sensorLabel) ) + format( "_%f.%s", (double)timestampTotime_t( obs->timestamp ), m_external_images_format.c_str() );
1023  //cout << "[CCameraSensor] Saving " << filName << endl;
1024  obs->image.saveToFile( m_path_for_external_images + string("/") +filName, m_external_images_jpeg_quality );
1025  obs->image.setExternalStorage( filName );
1026  }
1027  } // end else
1028  }
1029 
1030  // Show preview??
1031  if (m_preview_decimation>0)
1032  { // Yes
1033  if (++m_preview_counter > m_preview_decimation)
1034  {
1035  m_preview_counter = 0;
1036 
1037  // Create window the first time:
1038  if (!m_preview_win1)
1039  {
1040  string caption = string("Preview of ")+m_sensorLabel;
1041  if (stObs) caption+="-LEFT";
1042  if (m_preview_decimation>1)
1043  caption += format(" (decimation: %i)",m_preview_decimation);
1044  m_preview_win1 = mrpt::gui::CDisplayWindow::Create(caption);
1045  }
1046  if (stObs && !m_preview_win2)
1047  {
1048  string caption = string("Preview of ")+m_sensorLabel;
1049  if (stObs) caption+="-RIGHT";
1050  if (m_preview_decimation>1)
1051  caption += format(" (decimation: %i)",m_preview_decimation);
1052  m_preview_win2 = mrpt::gui::CDisplayWindow::Create(caption);
1053  }
1054  // Monocular image or Left from a stereo pair:
1055  if (m_preview_win1->isOpen())
1056  {
1057  CImage *img;
1058  if (stObs)
1059  img = &stObs->imageLeft;
1060  else if (obs)
1061  img = &obs->image;
1062  else
1063  img = &obs3D->intensityImage;
1064 
1065  // Apply image reduction?
1066  if (m_preview_reduction>=2)
1067  {
1068  unsigned int w = img->getWidth();
1069  unsigned int h = img->getHeight();
1070  CImage auxImg;
1071  img->scaleImage(auxImg, w/m_preview_reduction, h/m_preview_reduction, IMG_INTERP_NN);
1072  m_preview_win1->showImage(auxImg);
1073  }
1074  else
1075  m_preview_win1->showImage(*img);
1076  }
1077 
1078  // Right from a stereo pair:
1079  if (m_preview_win2 && m_preview_win2->isOpen() && stObs && stObs->hasImageRight)
1080  {
1081  // Apply image reduction?
1082  if (m_preview_reduction>=2)
1083  {
1084  unsigned int w = stObs->imageRight.getWidth();
1085  unsigned int h = stObs->imageRight.getHeight();
1086  CImage auxImg;
1087  stObs->imageRight.scaleImage(auxImg, w/m_preview_reduction, h/m_preview_reduction, IMG_INTERP_NN);
1088  m_preview_win2->showImage(auxImg);
1089  }
1090  else
1091  m_preview_win2->showImage(stObs->imageRight);
1092  }
1093 
1094  // Disparity from a stereo pair:
1095  if (m_preview_win2 && m_preview_win2->isOpen() && stObs && stObs->hasImageDisparity)
1096  {
1097  // Apply image reduction?
1098  if (m_preview_reduction>=2)
1099  {
1100  unsigned int w = stObs->imageDisparity.getWidth();
1101  unsigned int h = stObs->imageDisparity.getHeight();
1102  CImage auxImg;
1103  stObs->imageDisparity.scaleImage(auxImg, w/m_preview_reduction, h/m_preview_reduction, IMG_INTERP_NN);
1104  m_preview_win2->showImage(auxImg);
1105  }
1106  else
1107  m_preview_win2->showImage(stObs->imageDisparity);
1108  }
1109  }
1110  } // end show preview
1111 
1112  if (delayed_insertion_in_obs_queue)
1113  {
1114  if( m_cap_duo3d && m_cap_duo3d->captureIMUIsSet() && obsIMU )
1115  out_obs.push_back( CObservationPtr(obsIMU) );
1116  }
1117  else
1118  {
1119  if( stObs ) out_obs.push_back( CObservationPtr(stObs) );
1120  if( obs ) out_obs.push_back( CObservationPtr(obs) );
1121  if( obs3D ) out_obs.push_back( CObservationPtr(obs3D) );
1122  }
1123  return;
1124 }
1125 
1126 /* -----------------------------------------------------
1127  doProcess
1128 ----------------------------------------------------- */
1130 {
1131  vector<CSerializablePtr> out_obs;
1132  getNextFrame(out_obs);
1133  appendObservations(out_obs);
1134 }
1135 
1136 /* -----------------------------------------------------
1137  setSoftwareTriggerLevel
1138 ----------------------------------------------------- */
1140 {
1141  if (m_cap_dc1394)
1142  {
1143  if (!m_cap_dc1394->setSoftwareTriggerLevel(level))
1144  { // Error
1145  m_state = CGenericSensor::ssError;
1146  THROW_EXCEPTION("Error setting Trigger level by software");
1147  }
1148  }
1149  else
1150  {
1151  THROW_EXCEPTION("Software trigger is not implemented for this camera type")
1152  }
1153 }
1154 
1155 /* -----------------------------------------------------
1156  setPathForExternalImages
1157 ----------------------------------------------------- */
1159 {
1160  if (!mrpt::system::createDirectory( directory ))
1161  {
1162  THROW_EXCEPTION_FMT("Error: Cannot create the directory for externally saved images: %s",directory.c_str() )
1163  }
1164  m_path_for_external_images = directory;
1165 }
1166 
1167 /* ------------------------------------------------------------------------
1168  prepareVideoSourceFromUserSelection
1169  ------------------------------------------------------------------------ */
1171 {
1172 #if MRPT_HAS_WXWIDGETS
1173  // Create the main wxThread, if it doesn't exist yet:
1175  {
1176  std::cerr << "[mrpt::hwdrivers::prepareVideoSourceFromUserSelection] Error initiating Wx subsystem." << std::endl;
1177  return CCameraSensorPtr(); // Error!
1178  }
1179 
1180  mrpt::synch::CSemaphore semDlg(0,10);
1182 
1183  // Create window:
1185  REQ->OPCODE = 700;
1186  REQ->sourceCameraSelectDialog = true;
1187  REQ->voidPtr = reinterpret_cast<void*>(&semDlg);
1188  REQ->voidPtr2 = reinterpret_cast<void*>(&dlgSelection);
1189  WxSubsystem::pushPendingWxRequest( REQ );
1190 
1191  // Wait for the window to realize and signal it's alive:
1192  if (!WxSubsystem::isConsoleApp)
1193  {
1194  mrpt::system::sleep(20); // Force at least 1-2 timer ticks for processing the event:
1195  wxApp::GetInstance()->Yield(true);
1196  }
1197 
1198  // wait for window construction:
1199  int maxTimeout =
1200 #ifdef _DEBUG
1201  30000;
1202 #else
1203  6000;
1204 #endif
1205  // If we have an "MRPT_WXSUBSYS_TIMEOUT_MS" environment variable, use that timeout instead:
1206  const char *envVal = getenv("MRPT_WXSUBSYS_TIMEOUT_MS");
1207  if (envVal) maxTimeout = atoi(envVal);
1208 
1209  if(!semDlg.waitForSignal(maxTimeout))
1210  {
1211  cerr << "[prepareVideoSourceFromUserSelection] Timeout waiting window creation." << endl;
1212  return CCameraSensorPtr();
1213  }
1214 
1215  // wait for user selection:
1216  if(!semDlg.waitForSignal())
1217  {
1218  cerr << "[prepareVideoSourceFromUserSelection] Timeout waiting user selection." << endl;
1219  return CCameraSensorPtr();
1220  }
1221 
1222  // If the user didn't accept the dialog, return now:
1223  if (!dlgSelection.accepted_by_user)
1224  return CCameraSensorPtr();
1225 
1227  cam->loadConfig(dlgSelection.selectedConfig,"CONFIG");
1228  cam->initialize(); // This will raise an exception if neccesary
1229 
1230  return cam;
1231 #else
1232  THROW_EXCEPTION("MRPT compiled without wxWidgets")
1233 #endif // MRPT_HAS_WXWIDGETS
1234 }
1235 
1236 /* ------------------------------------------------------------------------
1237  prepareVideoSourceFromPanel
1238  ------------------------------------------------------------------------ */
1240 {
1241 #if MRPT_HAS_WXWIDGETS
1242 
1243  try
1244  {
1245  CConfigFileMemory cfg;
1246  writeConfigFromVideoSourcePanel(_panel,"CONFIG",&cfg);
1247 
1248  // Try to open the camera:
1250  video->loadConfig(cfg,"CONFIG");
1251 
1252  // This will raise an exception if neccesary
1253  video->initialize();
1254 
1255  return video;
1256  }
1257  catch(std::exception &e)
1258  {
1259  cerr << endl << e.what() << endl;
1260  wxMessageBox(_("Couldn't open video source"),_("Error"));
1261  return CCameraSensorPtr();
1262  }
1263 #else
1264  THROW_EXCEPTION("MRPT compiled without wxWidgets")
1265 #endif // MRPT_HAS_WXWIDGETS
1266 }
1267 
1268 /* ------------------------------------------------------------------------
1269  writeConfigFromVideoSourcePanel
1270  ------------------------------------------------------------------------ */
1272  void *_panel,
1273  const std::string &sect,
1275  )
1276 {
1277  MRPT_START
1278 #if MRPT_HAS_WXWIDGETS
1279  ASSERT_(_panel)
1280  mrpt::gui::CPanelCameraSelection *panel = reinterpret_cast<mrpt::gui::CPanelCameraSelection *>(_panel);
1281  ASSERTMSG_(panel,"panel must be of type mrpt::gui::CPanelCameraSelection *")
1282 
1283  panel->writeConfigFromVideoSourcePanel(sect,cfg);
1284 
1285 #else
1286  THROW_EXCEPTION("MRPT compiled without wxWidgets")
1287 #endif // MRPT_HAS_WXWIDGETS
1288  MRPT_END
1289 }
1290 
1291 /* ------------------------------------------------------------------------
1292  readConfigIntoVideoSourcePanel
1293  ------------------------------------------------------------------------ */
1295  void *_panel,
1296  const std::string &sect,
1297  const mrpt::utils::CConfigFileBase *cfg
1298  )
1299 {
1300  MRPT_START
1301 #if MRPT_HAS_WXWIDGETS
1302  ASSERT_(_panel)
1303  mrpt::gui::CPanelCameraSelection *panel = reinterpret_cast<mrpt::gui::CPanelCameraSelection *>(_panel);
1304  ASSERTMSG_(panel,"panel must be of type mrpt::gui::CPanelCameraSelection *")
1305 
1306  panel->readConfigIntoVideoSourcePanel(sect,cfg);
1307 
1308 #else
1309  THROW_EXCEPTION("MRPT compiled without wxWidgets")
1310 #endif // MRPT_HAS_WXWIDGETS
1311  MRPT_END
1312 }
1313 
1314 
1315 /* -----------------------------------------------------
1316  THREAD: Saver of external images
1317  ----------------------------------------------------- */
1318 void CCameraSensor::thread_save_images(unsigned int my_working_thread_index)
1319 {
1320  while (!m_threadImagesSaverShouldEnd)
1321  {
1322  TListObservations newObs;
1323 
1324  // is there any new image?
1325  m_csToSaveList.enter();
1326  m_toSaveList[my_working_thread_index].swap(newObs);
1327  m_csToSaveList.leave();
1328 
1329  for (TListObservations::const_iterator i=newObs.begin();i!=newObs.end();++i)
1330  {
1331  // Optional user-code hook:
1332  if (m_hook_pre_save)
1333  {
1334  if (IS_DERIVED(i->second, CObservation))
1335  {
1336  mrpt::obs::CObservationPtr obs = mrpt::obs::CObservationPtr(i->second);
1337  (*m_hook_pre_save)(obs,m_hook_pre_save_param);
1338  }
1339  }
1340 
1341  if (IS_CLASS(i->second, CObservationImage))
1342  {
1343  CObservationImagePtr obs = CObservationImagePtr(i->second);
1344 
1345  string filName = fileNameStripInvalidChars( trim(m_sensorLabel) ) + format( "_%f.%s", (double)timestampTotime_t( obs->timestamp ), m_external_images_format.c_str() );
1346 
1347  obs->image.saveToFile( m_path_for_external_images + string("/") +filName, m_external_images_jpeg_quality );
1348  obs->image.setExternalStorage( filName );
1349  }
1350  else if (IS_CLASS(i->second, CObservationStereoImages))
1351  {
1352  CObservationStereoImagesPtr stObs = CObservationStereoImagesPtr(i->second);
1353 
1354  const string filNameL = fileNameStripInvalidChars( trim(m_sensorLabel) ) + format( "_L_%f.%s", (double)timestampTotime_t( stObs->timestamp ), m_external_images_format.c_str() );
1355  const string filNameR = fileNameStripInvalidChars( trim(m_sensorLabel) ) + format( "_R_%f.%s", (double)timestampTotime_t( stObs->timestamp ), m_external_images_format.c_str() );
1356  const string filNameD = fileNameStripInvalidChars( trim(m_sensorLabel) ) + format( "_D_%f.%s", (double)timestampTotime_t( stObs->timestamp ), m_external_images_format.c_str() );
1357 
1358  stObs->imageLeft.saveToFile( m_path_for_external_images + string("/") + filNameL, m_external_images_jpeg_quality );
1359  stObs->imageLeft.setExternalStorage( filNameL );
1360 
1361  if (stObs->hasImageRight) {
1362  stObs->imageRight.saveToFile( m_path_for_external_images + string("/") + filNameR, m_external_images_jpeg_quality );
1363  stObs->imageRight.setExternalStorage( filNameR );
1364  }
1365  if (stObs->hasImageDisparity) {
1366  stObs->imageDisparity.saveToFile( m_path_for_external_images + string("/") + filNameD, m_external_images_jpeg_quality );
1367  stObs->imageDisparity.setExternalStorage( filNameD );
1368  }
1369  }
1370 
1371  // Append now:
1372  appendObservation(i->second);
1373  }
1374 
1376  }
1377 }
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 * voidPtr
Parameters, depending on OPCODE.
Definition: WxSubsystem.h:214
#define ADD_COLOR_MAP(c)
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
A class for grabing stereo images from a STOC camera of Videre Design NOTE:
bool BASE_IMPEXP createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:154
TCameraType
These capture types are like their OpenCV equivalents.
std::multimap< mrpt::system::TTimeStamp, mrpt::utils::CSerializablePtr > TListObservations
The data structure for each inter-thread request:
Definition: WxSubsystem.h:182
This "software driver" implements the communication protocol for interfacing a DUO3D Stereo Camera...
Definition: CDUO3DCamera.h:125
A wrapper for Point Gray Research (PGR) FlyCapture2 API for capturing images from Firewire...
void doProcess()
This method will be invoked at a minimum rate of "process_rate" (Hz) method must throw an exception w...
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
#define THROW_EXCEPTION(msg)
A class for grabing "range images" from a MESA imaging SwissRanger 3D cameras (SR-2, SR-3000, SR-4k).
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
Scalar * iterator
Definition: eigen_plugins.h:23
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
unsigned int BASE_IMPEXP getNumberOfProcessors()
Return the number of processors ("cores"), or 1 if it cannot be determined.
Definition: threads.cpp:486
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
A generic class which process a video file or other kind of input stream (http, rtsp) and allows the ...
mrpt::obs::CObservationPtr getNextFrame()
Retrieves the next frame from the video source, raising an exception on any error.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
int checkTypeIndex(const std::string &type) const
Returns the index of a given type within the list of all possible types, or -1 if the given string is...
int OPCODE
Valid codes are: For CDisplayWindow:
Definition: WxSubsystem.h:259
A class for grabing "range images", intensity images (either RGB or IR) and other information from an...
This class allows loading and storing values and vectors of different types from a configuration text...
CCameraSensorPtr HWDRIVERS_IMPEXP prepareVideoSourceFromPanel(void *panel)
Used only from MRPT apps: Use with caution since "panel" MUST be a "mrpt::gui::CPanelCameraSelection ...
This class implements a config file-like interface over a memory-stored string list.
void thread_save_images(unsigned int my_working_thread_index)
Thread to save images to files.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
GLint GLvoid * img
Definition: glew.h:1290
#define IS_DERIVED(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is an i...
Definition: CObject.h:106
A class for grabing "range images", intensity images (either RGB or IR) and other information from an...
Definition: CKinect.h:207
void close()
Close the camera (if open).
void delete_safe(T *&ptr)
Calls "delete" to free an object only if the pointer is not NULL, then set the pointer to NULL...
Definition: bits.h:196
The central class for camera grabbers in MRPT, implementing the "generic sensor" interface.
std::string BASE_IMPEXP lowerCase(const std::string &str)
Returns an lower-case version of a string.
#define MRPT_END
virtual void setPathForExternalImages(const std::string &directory)
Set the path where to save off-rawlog image files (this class DOES take into account this path)...
void BASE_IMPEXP sleep(int time_ms) MRPT_NO_THROWS
An OS-independent method for sending the current thread to "sleep" for a given period of time...
Definition: threads.cpp:57
std::deque< CObservationPtr >::iterator iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
A class for grabing images from a IEEE1394 (Firewire) camera using the libdc1394-2 library...
Grabs from a "Bumblebee" or "Bumblebee2" stereo camera using raw access to the libdc1394 library...
GLint GLint GLsizei GLsizei GLsizei GLint GLenum format
Definition: glew.h:1168
Transparently opens a compressed "gz" file and reads uncompressed data from it.
void scaleImage(unsigned int width, unsigned int height, TInterpolationMethod interp=IMG_INTERP_CUBIC)
Scales this image to a new size, interpolating as needed.
Definition: CImage.cpp:2224
stlplus::smart_ptr< CCameraSensor > CCameraSensorPtr
A smart pointer to a CCameraSensor.
std::string read_string_first_word(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
Reads a configuration parameter of type "string", and keeps only the first word (this can be used to ...
Observation class for either a pair of left+right or left+disparity images from a stereo camera...
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
bool sourceCameraSelectDialog
Only one of source* can be non-NULL, indicating the class that generated the request.
Definition: WxSubsystem.h:206
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
#define DEG2RAD
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
void loadConfig_sensorSpecific(const mrpt::utils::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
TThreadHandle createThreadFromObjectMethod(CLASS *obj, void(CLASS::*func)(PARAM), PARAM param)
Creates a new thread running a non-static method (so it will have access to "this") from another meth...
Definition: threads.h:216
void HWDRIVERS_IMPEXP readConfigIntoVideoSourcePanel(void *panel, const std::string &in_cfgfile_section_name, const mrpt::utils::CConfigFileBase *in_cfgfile)
Parse the given section of the given configuration file and set accordingly the controls of the wxWid...
std::string BASE_IMPEXP fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars= '_')
Replace invalid filename chars by underscores ('_') or any other user-given char. ...
Definition: filesystem.cpp:315
static void startSyncCapture(int numCameras, const CImageGrabber_FlyCapture2 **cameras_array)
Starts a synchronous capture of several cameras, which must have been already opened.
GLsizeiptr size
Definition: glew.h:1586
#define MRPT_START
unsigned __int64 uint64_t
Definition: rptypes.h:52
static CDisplayWindowPtr Create()
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
CCameraSensorPtr HWDRIVERS_IMPEXP prepareVideoSourceFromUserSelection()
Show to the user a list of possible camera drivers and creates and open the selected camera...
GLint level
Definition: glew.h:1166
GLsizei const GLcharARB ** string
Definition: glew.h:3293
#define MRPT_LOAD_HERE_CONFIG_VAR(variableName, variableType, targetVariable, configFileObject, sectionNameStr)
virtual void initialize()
Tries to open the camera, after setting all the parameters with a call to loadConfig.
Declares a class that represents any robot's observation.
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
Definition: CObject.h:103
std::string BASE_IMPEXP upperCase(const std::string &str)
Returns a upper-case version of a string.
bool waitForSignal(unsigned int timeout_ms=0)
Blocks until the count of the semaphore to be non-zero.
A class for grabing images from a "OpenCV"-compatible camera, or from an AVI video file...
This class represents a std::string derived class which is also CSerializable.
Definition: CTypeSelector.h:25
void setSoftwareTriggerLevel(bool level)
Set Software trigger level value (ON or OFF) for cameras with this function available.
#define ASSERT_(f)
mrpt::utils::CImage image
The image captured by the camera, that is, the main piece of information of this observation.
A panel to select the camera input from all the formats supported by MRPT.
Definition: WxUtils.h:163
std::string BASE_IMPEXP trim(const std::string &str)
Removes leading and trailing spaces.
mrpt::utils::CConfigFileMemory selectedConfig
Definition: WxUtils.h:293
void HWDRIVERS_IMPEXP writeConfigFromVideoSourcePanel(void *panel, const std::string &in_cfgfile_section_name, mrpt::utils::CConfigFileBase *out_cfgfile)
Parse the user options in the wxWidgets "panel" and write the configuration into the given section of...
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
double BASE_IMPEXP timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds...
Definition: datetime.cpp:205
#define ASSERTMSG_(f, __ERROR_MSG)
A semaphore for inter-thread synchronization.
Definition: CSemaphore.h:28
size_t getWidth() const MRPT_OVERRIDE
Returns the width of the image in pixels.
Definition: CImage.cpp:855
static bool createOneInstanceMainThread()
Thread-safe method to create one single instance of the main wxWidgets thread: it will create the thr...
double BASE_IMPEXP timestampTotime_t(const mrpt::system::TTimeStamp t)
Transform from TTimeStamp to standard "time_t" (actually a double number, it can contain fractions of...
Definition: datetime.cpp:53
size_t getHeight() const MRPT_OVERRIDE
Returns the height of the image in pixels.
Definition: CImage.cpp:884
virtual ~CCameraSensor()
Destructor.
void BASE_IMPEXP joinThread(const TThreadHandle &threadHandle)
Waits until the given thread ends.
Definition: threads.cpp:190



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