Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
12 #include <mrpt/system/os.h>
15 #include <mrpt/system/filesystem.h>
18 #include <mrpt/obs/CSensoryFrame.h>
19 #include <mrpt/obs/CRawlog.h>
22 #include <mrpt/gui/WxUtils.h>
23 #include <mrpt/gui/WxSubsystem.h>
25 
26 using namespace mrpt;
27 using namespace mrpt::hwdrivers;
28 using namespace mrpt::gui;
29 using namespace mrpt::obs;
30 using namespace mrpt::config;
31 using namespace mrpt::system;
32 using namespace mrpt::io;
33 using namespace mrpt::serialization;
34 using namespace mrpt::img;
35 using namespace std;
36 using namespace std::literals;
37 
39 
40 /* -----------------------------------------------------
41  Constructor
42  ----------------------------------------------------- */
44  : mrpt::system::COutputLogger("CCameraSensor"),
45  m_sensorPose(),
46  m_grabber_type("opencv"),
47  m_capture_grayscale(false),
48  m_cv_camera_index(0),
49  m_cv_camera_type("CAMERA_CV_AUTODETECT"),
50  m_cv_options(),
51  // ---
52  m_dc1394_camera_guid(0),
53  m_dc1394_camera_unit(0),
54  m_dc1394_options(),
55  m_preview_decimation(0),
56  m_preview_reduction(1),
57  // ---
58  m_bumblebee_dc1394_camera_guid(0),
59  m_bumblebee_dc1394_camera_unit(0),
60  m_bumblebee_dc1394_framerate(15),
61  // ---
62  m_svs_camera_index(0),
63  m_svs_options(),
64  // ---
65  m_sr_open_from_usb(true),
66  m_sr_save_3d(true),
67  m_sr_save_range_img(true),
68  m_sr_save_intensity_img(true),
69  m_sr_save_confidence(true),
70  // ---
71  m_kinect_save_3d(
72  true), // These options are also used for OpenNI2 grabber
73  m_kinect_save_range_img(true),
74  m_kinect_save_intensity_img(true),
75  m_kinect_video_rgb(true),
76  // ---
77  m_fcs_start_synch_capture(false),
78  // ---
79  m_img_dir_url(""),
80  m_img_dir_left_format("imL_%04d.jpg"),
81  m_img_dir_right_format("imR_%04d.jpg"),
82  m_img_dir_start_index(0),
83  m_img_dir_end_index(100),
84  m_img_dir_is_stereo(true),
85  m_img_dir_counter(0),
86  // ---
87  m_external_images_own_thread(false),
88  m_camera_grab_decimator(0),
89  m_camera_grab_decimator_counter(0),
90  m_preview_counter(0),
91  m_external_image_saver_count(std::thread::hardware_concurrency()),
92  m_threadImagesSaverShouldEnd(false),
93  m_hook_pre_save(nullptr),
94  m_hook_pre_save_param(nullptr)
95 {
96  m_sensorLabel = "CAMERA";
98 }
99 
100 /* -----------------------------------------------------
101  initialize
102  ----------------------------------------------------- */
104 {
105  cout << "[CCameraSensor::initialize] Opening camera..." << endl;
106  close();
107 
108  // Select type of device
109  m_grabber_type = trim(lowerCase(m_grabber_type));
110  m_cv_camera_type = trim(upperCase(m_cv_camera_type));
111 
112  if (m_grabber_type == "opencv")
113  {
114  // OpenCV driver:
115  TCameraType ct;
116  try
117  {
119  m_cv_camera_type);
120  }
121  catch (std::exception&)
122  {
123  m_state = CGenericSensor::ssError;
124  throw;
125  }
126  cout << format(
127  "[CCameraSensor::initialize] opencv camera, index: %i type: "
128  "%i...\n",
129  int(m_cv_camera_index), (int)ct);
130  m_cap_cv.reset(
131  new CImageGrabber_OpenCV(m_cv_camera_index, ct, m_cv_options));
132 
133  if (!m_cap_cv->isOpen())
134  {
135  m_state = CGenericSensor::ssError;
137  "[CCameraSensor::initialize] ERROR: Couldn't open OpenCV "
138  "camera.")
139  }
140  }
141  else if (m_grabber_type == "dc1394")
142  {
143  // m_cap_dc1394
144  cout << format(
145  "[CCameraSensor::initialize] dc1394 camera, GUID: 0x%lX "
146  "UNIT:%d...\n",
147  long(m_dc1394_camera_guid), m_dc1394_camera_unit);
148  m_cap_dc1394.reset(
150  m_dc1394_camera_guid, m_dc1394_camera_unit, m_dc1394_options,
151  true /* verbose */));
152 
153  if (!m_cap_dc1394->isOpen())
154  {
155  m_state = CGenericSensor::ssError;
157  "[CCameraSensor::initialize] ERROR: Couldn't open dc1394 "
158  "camera.")
159  }
160  }
161  else if (m_grabber_type == "bumblebee_dc1394")
162  {
163  cout << format(
164  "[CCameraSensor::initialize] bumblebee_libdc1394 camera: "
165  "GUID:0x%08X Index:%i FPS:%f...\n",
166  (unsigned int)(m_bumblebee_dc1394_camera_guid),
167  m_bumblebee_dc1394_camera_unit, m_bumblebee_dc1394_framerate);
168  m_cap_bumblebee_dc1394.reset(
170  m_bumblebee_dc1394_camera_guid, m_bumblebee_dc1394_camera_unit,
171  m_bumblebee_dc1394_framerate));
172  }
173  else if (m_grabber_type == "svs")
174  {
175  cout << format(
176  "[CCameraSensor::initialize] SVS camera: %u...\n",
177  (unsigned int)(m_svs_camera_index));
178  m_cap_svs.reset(
179  new CStereoGrabber_SVS(m_svs_camera_index, m_svs_options));
180  }
181  else if (m_grabber_type == "ffmpeg")
182  {
183  // m_cap_ffmpeg
184  cout << format(
185  "[CCameraSensor::initialize] FFmpeg stream: %s...\n",
186  m_ffmpeg_url.c_str());
187  m_cap_ffmpeg.reset(new CFFMPEG_InputStream());
188 
189  if (!m_cap_ffmpeg->openURL(m_ffmpeg_url, m_capture_grayscale))
190  {
191  m_state = CGenericSensor::ssError;
193  "Error opening FFmpeg stream: %s", m_ffmpeg_url.c_str())
194  }
195  }
196  else if (m_grabber_type == "swissranger")
197  {
198  cout << "[CCameraSensor::initialize] SwissRanger camera...\n";
199  m_cap_swissranger.reset(new CSwissRanger3DCamera());
200 
201  m_cap_swissranger->setOpenFromUSB(m_sr_open_from_usb);
202  m_cap_swissranger->setOpenIPAddress(m_sr_ip_address);
203 
204  m_cap_swissranger->setSave3D(m_sr_save_3d);
205  m_cap_swissranger->setSaveRangeImage(m_sr_save_range_img);
206  m_cap_swissranger->setSaveIntensityImage(m_sr_save_intensity_img);
207  m_cap_swissranger->setSaveConfidenceImage(m_sr_save_confidence);
208 
209  if (!m_path_for_external_images.empty())
210  m_cap_swissranger->setPathForExternalImages(
211  m_path_for_external_images);
212 
213  // Open it:
214  try
215  {
216  m_cap_swissranger
217  ->initialize(); // This will launch an exception if needed.
218  }
219  catch (std::exception&)
220  {
221  m_state = CGenericSensor::ssError;
222  throw;
223  }
224  }
225  else if (m_grabber_type == "kinect")
226  {
227  cout << "[CCameraSensor::initialize] Kinect camera...\n";
228  m_cap_kinect.reset(new CKinect());
229  m_cap_kinect->enableGrab3DPoints(m_kinect_save_3d);
230  m_cap_kinect->enableGrabDepth(m_kinect_save_range_img);
231  m_cap_kinect->enableGrabRGB(m_kinect_save_intensity_img);
232  m_cap_kinect->setVideoChannel(
233  m_kinect_video_rgb ? CKinect::VIDEO_CHANNEL_RGB
235 
236  if (!m_path_for_external_images.empty())
237  m_cap_kinect->setPathForExternalImages(m_path_for_external_images);
238 
239  // Open it:
240  try
241  {
242  m_cap_kinect
243  ->initialize(); // This will launch an exception if needed.
244  }
245  catch (std::exception&)
246  {
247  m_state = CGenericSensor::ssError;
248  throw;
249  }
250  }
251  else if (m_grabber_type == "openni2")
252  {
253  cout << "[CCameraSensor::initialize] OpenNI2 sensor...\n";
254  m_cap_openni2.reset(new COpenNI2Sensor());
255  m_cap_openni2->enableGrab3DPoints(m_kinect_save_3d); // It uses the
256  // same options as
257  // the Kinect
258  // grabber
259  m_cap_openni2->enableGrabDepth(m_kinect_save_range_img);
260  m_cap_openni2->enableGrabRGB(m_kinect_save_intensity_img);
261 
262  if (!m_path_for_external_images.empty())
263  m_cap_openni2->setPathForExternalImages(m_path_for_external_images);
264 
265  // Open it:
266  try
267  {
268  m_cap_openni2
269  ->initialize(); // This will launch an exception if needed.
270  }
271  catch (std::exception& e)
272  {
273  m_state = CGenericSensor::ssError;
274  throw e;
275  }
276  }
277  else if (m_grabber_type == "image_dir")
278  {
279  // m_cap_image_dir
280  cout << format(
281  "[CCameraSensor::initialize] Image dir: %s...\n",
282  m_img_dir_url.c_str());
283  m_cap_image_dir.reset(new std::string());
284  }
285  else if (m_grabber_type == "rawlog")
286  {
287  // m_cap_rawlog
288  cout << format(
289  "[CCameraSensor::initialize] Rawlog stream: %s...\n",
290  m_rawlog_file.c_str());
291  m_cap_rawlog.reset(new CFileGZInputStream());
292 
293  if (!m_cap_rawlog->open(m_rawlog_file))
294  {
295  m_state = CGenericSensor::ssError;
297  "Error opening rawlog file: %s", m_rawlog_file.c_str())
298  }
299  // File open OK.
300  // Localize the external images directory of this rawlog, if it exists:
301  m_rawlog_detected_images_dir =
302  CRawlog::detectImagesDirectory(m_rawlog_file);
303  }
304  else if (m_grabber_type == "flycap")
305  {
306  cout << "[CCameraSensor::initialize] PGR FlyCapture2 camera...\n";
307  try
308  {
309  // Open camera and start capture:
310  m_cap_flycap.reset(new CImageGrabber_FlyCapture2(m_flycap_options));
311  }
312  catch (std::exception&)
313  {
314  m_state = CGenericSensor::ssError;
315  throw;
316  }
317  }
318  else if (m_grabber_type == "flycap_stereo")
319  {
320  cout
321  << "[CCameraSensor::initialize] PGR FlyCapture2 stereo camera...\n";
322  try
323  {
324  // Open camera and start capture:
325  m_cap_flycap_stereo_l.reset(new CImageGrabber_FlyCapture2());
326  m_cap_flycap_stereo_r.reset(new CImageGrabber_FlyCapture2());
327 
328  cout << "[CCameraSensor::initialize] PGR FlyCapture2 stereo "
329  "camera: Opening LEFT camera...\n";
330  m_cap_flycap_stereo_l->open(
331  m_flycap_stereo_options[0], false /* don't start grabbing */);
332 
333  cout << "[CCameraSensor::initialize] PGR FlyCapture2 stereo "
334  "camera: Opening RIGHT camera...\n";
335  m_cap_flycap_stereo_r->open(
336  m_flycap_stereo_options[1], false /* don't start grabbing */);
337 
338  // Now, start grabbing "simultaneously":
339  if (m_fcs_start_synch_capture)
340  {
341  const CImageGrabber_FlyCapture2* cams[2] = {
342  m_cap_flycap_stereo_l.get(), m_cap_flycap_stereo_r.get()};
344  }
345  else
346  {
347  m_cap_flycap_stereo_l->startCapture();
348  m_cap_flycap_stereo_r->startCapture();
349  }
350  }
351  catch (std::exception&)
352  {
353  m_state = CGenericSensor::ssError;
354  throw;
355  }
356  }
357  else if (m_grabber_type == "duo3d")
358  {
359  // m_cap_duo3D
360  cout << format("[CCameraSensor::initialize] DUO3D stereo camera ...\n");
361 
362  // Open it:
363  try
364  {
365  m_cap_duo3d.reset(new CDUO3DCamera(m_duo3d_options));
366  }
367  catch (std::exception& e)
368  {
369  m_state = CGenericSensor::ssError;
370  throw e;
371  }
372  }
373  else
375  "Unknown 'grabber_type' found: %s", m_grabber_type.c_str())
376 
377  // Change state:
378  cout << "[CCameraSensor::initialize] Done!" << endl;
379  m_state = CGenericSensor::ssWorking;
380 
381  // Launch independent thread?
382  if (m_external_images_own_thread)
383  {
384  m_threadImagesSaverShouldEnd = false;
385 
386  m_threadImagesSaver.clear();
387  m_threadImagesSaver.resize(m_external_image_saver_count);
388 
389  m_toSaveList.clear();
390  m_toSaveList.resize(m_external_image_saver_count);
391 
392  for (unsigned int i = 0; i < m_external_image_saver_count; ++i)
393  m_threadImagesSaver[i] =
394  std::thread(&CCameraSensor::thread_save_images, this, i);
395  }
396 }
397 
398 /* -----------------------------------------------------
399  close
400  ----------------------------------------------------- */
402 {
403  m_cap_cv.reset();
404  m_cap_dc1394.reset();
405  m_cap_flycap.reset();
406  m_cap_flycap_stereo_l.reset();
407  m_cap_flycap_stereo_r.reset();
408  m_cap_bumblebee_dc1394.reset();
409  m_cap_ffmpeg.reset();
410  m_cap_rawlog.reset();
411  m_cap_swissranger.reset();
412  m_cap_kinect.reset();
413  m_cap_svs.reset();
414  m_cap_image_dir.reset();
415  m_cap_duo3d.reset();
416 
418 
419  // Wait for threads:
420  if (!m_threadImagesSaver.empty())
421  {
422  m_threadImagesSaverShouldEnd = true;
423  for (size_t i = 0; i < m_threadImagesSaver.size(); i++)
424  m_threadImagesSaver[i].join();
425  }
426 }
427 
428 /* -----------------------------------------------------
429  loadConfig_sensorSpecific
430  ----------------------------------------------------- */
432  const mrpt::config::CConfigFileBase& configSource,
433  const std::string& iniSection)
434 {
435  // At this point, my parent class CGenericSensor has already loaded its
436  // params:
437  // Since cameras are special, we'll take control over "m_grab_decimation"
438  // so
439  // external image files are not saved just to be discarded later on...
440  if (m_grab_decimation > 0)
441  {
442  m_camera_grab_decimator = m_grab_decimation;
443  m_camera_grab_decimator_counter = 0;
444  // Reset in parent:
445  m_grab_decimation = 0;
446  }
447  else
448  m_camera_grab_decimator = m_camera_grab_decimator_counter = 0;
449 
450  m_grabber_type = configSource.read_string_first_word(
451  iniSection, "grabber_type", m_grabber_type);
453  preview_decimation, int, m_preview_decimation, configSource, iniSection)
455  preview_reduction, int, m_preview_reduction, configSource, iniSection)
456 
457  // OpenCV options:
458  m_cv_camera_type = configSource.read_string_first_word(
459  iniSection, "cv_camera_type", m_cv_camera_type);
460  m_cv_camera_index =
461  configSource.read_int(iniSection, "cv_camera_index", m_cv_camera_index);
462 
463  m_cv_options.frame_width = configSource.read_int(
464  iniSection, "cv_frame_width", m_cv_options.frame_width);
465  m_cv_options.frame_height = configSource.read_int(
466  iniSection, "cv_frame_height", m_cv_options.frame_height);
467  m_cv_options.gain =
468  configSource.read_double(iniSection, "cv_gain", m_cv_options.gain);
469  m_cv_options.ieee1394_fps = configSource.read_double(
470  iniSection, "cv_fps", m_cv_options.ieee1394_fps);
471 
472  m_capture_grayscale =
473  configSource.read_bool(iniSection, "capture_grayscale", false);
474 
475  m_cv_options.ieee1394_grayscale = m_capture_grayscale;
476 
477  // dc1394 options:
479  dc1394_camera_guid, uint64_t, m_dc1394_camera_guid, configSource,
480  iniSection)
482  dc1394_camera_unit, int, m_dc1394_camera_unit, configSource, iniSection)
483 
485  dc1394_frame_width, int, m_dc1394_options.frame_width, configSource,
486  iniSection)
488  dc1394_frame_height, int, m_dc1394_options.frame_height, configSource,
489  iniSection)
490 
492  dc1394_mode7, int, m_dc1394_options.mode7, configSource, iniSection)
493 
495  dc1394_shutter, int, m_dc1394_options.shutter, configSource, iniSection)
497  dc1394_gain, int, m_dc1394_options.gain, configSource, iniSection)
499  dc1394_gamma, int, m_dc1394_options.gamma, configSource, iniSection)
501  dc1394_brightness, int, m_dc1394_options.brightness, configSource,
502  iniSection)
504  dc1394_exposure, int, m_dc1394_options.exposure, configSource,
505  iniSection)
507  dc1394_sharpness, int, m_dc1394_options.sharpness, configSource,
508  iniSection)
510  dc1394_white_balance, int, m_dc1394_options.white_balance, configSource,
511  iniSection)
512 
514  dc1394_shutter_mode, int, m_dc1394_options.shutter_mode, configSource,
515  iniSection)
517  dc1394_gain_mode, int, m_dc1394_options.gain_mode, configSource,
518  iniSection)
520  dc1394_gamma_mode, int, m_dc1394_options.gamma_mode, configSource,
521  iniSection)
523  dc1394_brightness_mode, int, m_dc1394_options.brightness_mode,
524  configSource, iniSection)
526  dc1394_exposure_mode, int, m_dc1394_options.exposure_mode, configSource,
527  iniSection)
529  dc1394_sharpness_mode, int, m_dc1394_options.sharpness_mode,
530  configSource, iniSection)
532  dc1394_white_balance_mode, int, m_dc1394_options.white_balance_mode,
533  configSource, iniSection)
534 
536  dc1394_trigger_power, int, m_dc1394_options.trigger_power, configSource,
537  iniSection)
539  dc1394_trigger_mode, int, m_dc1394_options.trigger_mode, configSource,
540  iniSection)
542  dc1394_trigger_source, int, m_dc1394_options.trigger_source,
543  configSource, iniSection)
545  dc1394_trigger_polarity, int, m_dc1394_options.trigger_polarity,
546  configSource, iniSection)
548  dc1394_ring_buffer_size, int, m_dc1394_options.ring_buffer_size,
549  configSource, iniSection)
550 
551  // Bumblebee_dc1394 options:
553  bumblebee_dc1394_camera_guid, uint64_t, m_bumblebee_dc1394_camera_guid,
554  configSource, iniSection)
556  bumblebee_dc1394_camera_unit, int, m_bumblebee_dc1394_camera_unit,
557  configSource, iniSection)
559  bumblebee_dc1394_framerate, double, m_bumblebee_dc1394_framerate,
560  configSource, iniSection)
561 
562  // SVS options:
563  m_svs_camera_index = configSource.read_int(
564  iniSection, "svs_camera_index", m_svs_camera_index);
565  m_svs_options.frame_width = configSource.read_int(
566  iniSection, "svs_frame_width", m_svs_options.frame_width);
567  m_svs_options.frame_height = configSource.read_int(
568  iniSection, "svs_frame_height", m_svs_options.frame_height);
569  m_svs_options.framerate = configSource.read_double(
570  iniSection, "svs_framerate", m_svs_options.framerate);
571  m_svs_options.m_NDisp =
572  configSource.read_int(iniSection, "svs_NDisp", m_svs_options.m_NDisp);
573  m_svs_options.m_Corrsize = configSource.read_int(
574  iniSection, "svs_Corrsize", m_svs_options.m_Corrsize);
575  m_svs_options.m_LR =
576  configSource.read_int(iniSection, "svs_LR", m_svs_options.m_LR);
577  m_svs_options.m_Thresh =
578  configSource.read_int(iniSection, "svs_Thresh", m_svs_options.m_Thresh);
579  m_svs_options.m_Unique =
580  configSource.read_int(iniSection, "svs_Unique", m_svs_options.m_Unique);
581  m_svs_options.m_Horopter = configSource.read_int(
582  iniSection, "svs_Horopter", m_svs_options.m_Horopter);
583  m_svs_options.m_SpeckleSize = configSource.read_int(
584  iniSection, "svs_SpeckleSize", m_svs_options.m_SpeckleSize);
585  m_svs_options.m_procesOnChip = configSource.read_bool(
586  iniSection, "svs_procesOnChip", m_svs_options.m_procesOnChip);
587  m_svs_options.m_calDisparity = configSource.read_bool(
588  iniSection, "svs_calDisparity", m_svs_options.m_calDisparity);
589 
590  // FFmpeg options:
591  m_ffmpeg_url = mrpt::system::trim(
592  configSource.read_string(iniSection, "ffmpeg_url", m_ffmpeg_url));
593 
594  // Rawlog options:
595  m_rawlog_file = mrpt::system::trim(
596  configSource.read_string(iniSection, "rawlog_file", m_rawlog_file));
597  m_rawlog_camera_sensor_label = mrpt::system::trim(
598  configSource.read_string(
599  iniSection, "rawlog_camera_sensor_label",
600  m_rawlog_camera_sensor_label));
601 
602  // Image directory options:
603  m_img_dir_url = mrpt::system::trim(
604  configSource.read_string(iniSection, "image_dir_url", m_img_dir_url));
605  m_img_dir_left_format = mrpt::system::trim(
606  configSource.read_string(
607  iniSection, "left_format", m_img_dir_left_format));
608  m_img_dir_right_format = mrpt::system::trim(
609  configSource.read_string(iniSection, "right_format", ""));
610  m_img_dir_start_index =
611  configSource.read_int(iniSection, "start_index", m_img_dir_start_index);
612  ;
613  m_img_dir_end_index =
614  configSource.read_int(iniSection, "end_index", m_img_dir_end_index);
615 
616  m_img_dir_is_stereo = !m_img_dir_right_format.empty();
617  m_img_dir_counter = m_img_dir_start_index;
618 
619  // DUO3D Camera options:
620  m_duo3d_options.loadOptionsFrom(configSource, "DUO3DOptions");
621 
622  // SwissRanger options:
623  m_sr_open_from_usb =
624  configSource.read_bool(iniSection, "sr_use_usb", m_sr_open_from_usb);
625  m_sr_ip_address =
626  configSource.read_string(iniSection, "sr_IP", m_sr_ip_address);
627 
628  m_sr_save_3d =
629  configSource.read_bool(iniSection, "sr_grab_3d", m_sr_save_3d);
630  m_sr_save_intensity_img = configSource.read_bool(
631  iniSection, "sr_grab_grayscale", m_sr_save_intensity_img);
632  m_sr_save_range_img = configSource.read_bool(
633  iniSection, "sr_grab_range", m_sr_save_range_img);
634  m_sr_save_confidence = configSource.read_bool(
635  iniSection, "sr_grab_confidence", m_sr_save_confidence);
636 
637  m_kinect_save_3d =
638  configSource.read_bool(iniSection, "kinect_grab_3d", m_kinect_save_3d);
639  m_kinect_save_intensity_img = configSource.read_bool(
640  iniSection, "kinect_grab_intensity", m_kinect_save_intensity_img);
641  m_kinect_save_range_img = configSource.read_bool(
642  iniSection, "kinect_grab_range", m_kinect_save_range_img);
643  m_kinect_video_rgb = configSource.read_bool(
644  iniSection, "kinect_video_rgb", m_kinect_video_rgb);
645 
646  // FlyCap:
647  m_flycap_options.loadOptionsFrom(configSource, iniSection, "flycap_");
648 
649  // FlyCap stereo
650  m_fcs_start_synch_capture = configSource.read_bool(
651  iniSection, "fcs_start_synch_capture", m_fcs_start_synch_capture);
652  m_flycap_stereo_options[0].loadOptionsFrom(
653  configSource, iniSection, "fcs_LEFT_");
654  m_flycap_stereo_options[1].loadOptionsFrom(
655  configSource, iniSection, "fcs_RIGHT_");
656 
657  // Special stuff: FPS
658  map<double, grabber_dc1394_framerate_t> map_fps;
660  map_fps[1.875] = FRAMERATE_1_875;
661  map_fps[3.75] = FRAMERATE_3_75;
662  map_fps[7.5] = FRAMERATE_7_5;
663  map_fps[15] = FRAMERATE_15;
664  map_fps[30] = FRAMERATE_30;
665  map_fps[60] = FRAMERATE_60;
666  map_fps[120] = FRAMERATE_120;
667  map_fps[240] = FRAMERATE_240;
668 
669  // ... for dc1394
670  double the_fps =
671  configSource.read_double(iniSection, "dc1394_framerate", 15.0);
672  it_fps = map_fps.find(the_fps);
673  if (it_fps == map_fps.end())
675  "ERROR: DC1394 framerate seems to be not a valid number: %f",
676  the_fps);
677 
678  m_dc1394_options.framerate = it_fps->second;
679 
680  // Special stuff: color encoding:
681  map<string, grabber_dc1394_color_coding_t> map_color;
683 #define ADD_COLOR_MAP(c) map_color[#c] = c;
690 
691  string the_color_coding = mrpt::system::upperCase(
692  configSource.read_string_first_word(
693  iniSection, "dc1394_color_coding", "COLOR_CODING_YUV422"));
694  it_color = map_color.find(the_color_coding);
695  if (it_color == map_color.end())
697  "ERROR: Color coding seems not to be valid : '%s'",
698  the_color_coding.c_str());
699  m_dc1394_options.color_coding = it_color->second;
700 
701  m_external_images_format = mrpt::system::trim(
702  configSource.read_string(
703  iniSection, "external_images_format", m_external_images_format));
704  m_external_images_jpeg_quality = configSource.read_int(
705  iniSection, "external_images_jpeg_quality",
706  m_external_images_jpeg_quality);
707  m_external_images_own_thread = configSource.read_bool(
708  iniSection, "external_images_own_thread", m_external_images_own_thread);
709  m_external_image_saver_count = configSource.read_int(
710  iniSection, "external_images_own_thread_count",
711  m_external_image_saver_count);
712 
713  // Sensor pose:
714  m_sensorPose.setFromValues(
715  configSource.read_float(iniSection, "pose_x", 0),
716  configSource.read_float(iniSection, "pose_y", 0),
717  configSource.read_float(iniSection, "pose_z", 0),
718  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
719  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
720  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
721 }
722 
723 /* -----------------------------------------------------
724  Destructor
725  ----------------------------------------------------- */
727 {
728  close();
729 
730  m_preview_win1.reset();
731  m_preview_win2.reset();
732 }
733 /* -----------------------------------------------------
734  getNextFrame
735 ----------------------------------------------------- */
737 {
738  vector<CSerializable::Ptr> out_obs;
739  getNextFrame(out_obs);
740  return std::dynamic_pointer_cast<CObservation>(out_obs[0]);
741 }
742 
743 /* -----------------------------------------------------
744  getNextFrame
745 ----------------------------------------------------- */
746 void CCameraSensor::getNextFrame(vector<CSerializable::Ptr>& out_obs)
747 {
751  obs3D; // 3D range image, also with an intensity channel
752  CObservationIMU::Ptr obsIMU; // IMU observation grabbed by DUO3D cameras
753 
754  bool capture_ok = false;
755 
756  if (m_cap_cv)
757  {
758  obs = mrpt::make_aligned_shared<CObservationImage>();
759  if (!m_cap_cv->getObservation(*obs))
760  { // Error
761  m_state = CGenericSensor::ssError;
762  THROW_EXCEPTION("Error grabbing image");
763  }
764  else
765  capture_ok = true;
766  }
767  else if (m_cap_dc1394)
768  {
769  obs = mrpt::make_aligned_shared<CObservationImage>();
770  if (!m_cap_dc1394->getObservation(*obs))
771  { // Error
772  m_state = CGenericSensor::ssError;
773  THROW_EXCEPTION("Error grabbing image");
774  }
775  else
776  capture_ok = true;
777  }
778  else if (m_cap_swissranger)
779  {
780  obs3D = mrpt::make_aligned_shared<CObservation3DRangeScan>();
781 
782  bool there_is_obs, hardware_error;
783  m_cap_swissranger->getNextObservation(
784  *obs3D, there_is_obs, hardware_error);
785 
786  if (!there_is_obs || hardware_error)
787  { // Error
788  m_state = CGenericSensor::ssError;
789  THROW_EXCEPTION("Error grabbing image from SwissRanger camera.");
790  }
791  else
792  capture_ok = true;
793  }
794  else if (m_cap_kinect)
795  {
796  obs3D = mrpt::make_aligned_shared<CObservation3DRangeScan>();
797 
798  // Specially at start-up, there may be a delay grabbing so a few calls
799  // return false: add a timeout.
801  double max_timeout = 3.0; // seconds
802 
803  // If we have an "MRPT_CCAMERA_KINECT_TIMEOUT_MS" environment variable,
804  // use that timeout instead:
805  const char* envVal = getenv("MRPT_CCAMERA_KINECT_TIMEOUT_MS");
806  if (envVal) max_timeout = atoi(envVal) * 0.001;
807 
808  bool there_is_obs, hardware_error;
809  do
810  {
811  m_cap_kinect->getNextObservation(
812  *obs3D, there_is_obs, hardware_error);
813  if (!there_is_obs) std::this_thread::sleep_for(1ms);
814  } while (!there_is_obs &&
816  max_timeout);
817 
818  if (!there_is_obs || hardware_error)
819  { // Error
820  m_state = CGenericSensor::ssError;
821  THROW_EXCEPTION("Error grabbing image from Kinect camera.");
822  }
823  else
824  capture_ok = true;
825  }
826  else if (m_cap_openni2)
827  {
828  obs3D = mrpt::make_aligned_shared<CObservation3DRangeScan>();
829  // Specially at start-up, there may be a delay grabbing so a few calls
830  // return false: add a timeout.
832  double max_timeout = 3.0; // seconds
833  bool there_is_obs, hardware_error;
834  do
835  {
836  m_cap_openni2->getNextObservation(
837  *obs3D, there_is_obs, hardware_error);
838  if (!there_is_obs) std::this_thread::sleep_for(1ms);
839  } while (!there_is_obs &&
841  max_timeout);
842 
843  if (!there_is_obs || hardware_error)
844  { // Error
845  m_state = CGenericSensor::ssError;
846  THROW_EXCEPTION("Error grabbing image from OpenNI2 camera.");
847  }
848  else
849  capture_ok = true;
850  }
851  else if (m_cap_bumblebee_dc1394)
852  {
853  stObs = mrpt::make_aligned_shared<CObservationStereoImages>();
854  if (!m_cap_bumblebee_dc1394->getStereoObservation(*stObs))
855  {
856  m_state = CGenericSensor::ssError;
857  THROW_EXCEPTION("Error grabbing stereo images");
858  }
859  else
860  {
861  capture_ok = true;
862  }
863  }
864  else if (m_cap_svs)
865  {
866  stObs = mrpt::make_aligned_shared<CObservationStereoImages>();
867 
868  if (!m_cap_svs->getStereoObservation(*stObs))
869  {
870  // Error
871  m_state = CGenericSensor::ssError;
872  THROW_EXCEPTION("Error grabbing disparity images");
873  }
874  else
875  capture_ok = true;
876  }
877  else if (m_cap_ffmpeg)
878  {
879  obs = mrpt::make_aligned_shared<CObservationImage>();
880 
881  if (!m_cap_ffmpeg->retrieveFrame(obs->image))
882  { // Error
883  m_state = CGenericSensor::ssError;
884  THROW_EXCEPTION("Error grabbing image");
885  }
886  else
887  capture_ok = true;
888  }
889  else if (m_cap_image_dir)
890  {
891  if (m_img_dir_counter > m_img_dir_end_index)
892  {
893  m_state = CGenericSensor::ssError;
894  THROW_EXCEPTION("Reached end index.");
895  }
896 
897  std::string auxL = format(
898  "%s/%s", m_img_dir_url.c_str(), m_img_dir_left_format.c_str());
899  if (m_img_dir_is_stereo)
900  {
901  stObs = mrpt::make_aligned_shared<CObservationStereoImages>();
902  if (!stObs->imageLeft.loadFromFile(
903  format(auxL.c_str(), m_img_dir_counter)))
904  {
905  m_state = CGenericSensor::ssError;
906  THROW_EXCEPTION("Error reading images from directory");
907  }
908  std::string auxR = format(
909  "%s/%s", m_img_dir_url.c_str(), m_img_dir_right_format.c_str());
910  if (!stObs->imageRight.loadFromFile(
911  format(auxR.c_str(), m_img_dir_counter++)))
912  {
913  m_state = CGenericSensor::ssError;
914  THROW_EXCEPTION("Error reading images from directory");
915  }
916  else
917  capture_ok = true;
918  }
919  else
920  {
921  // use only left image prefix
922  obs = mrpt::make_aligned_shared<CObservationImage>();
923  if (!obs->image.loadFromFile(
924  format(auxL.c_str(), m_img_dir_counter++)))
925  {
926  m_state = CGenericSensor::ssError;
927  THROW_EXCEPTION("Error reading images from directory");
928  }
929  else
930  capture_ok = true;
931  }
932  }
933  else if (m_cap_rawlog)
934  {
935  // Read in a loop until we found at least one image:
936  // Assign to: obs && stObs
937  CSerializable::Ptr newObs;
938  while (!obs && !stObs && !obs3D)
939  {
940  archiveFrom(*m_cap_rawlog) >> newObs;
941  if (IS_DERIVED(newObs, CObservation))
942  {
944  std::dynamic_pointer_cast<CObservation>(newObs);
945  if (!m_rawlog_camera_sensor_label.empty() &&
946  m_rawlog_camera_sensor_label != o->sensorLabel)
947  continue;
948 
949  if (IS_CLASS(o, CObservationImage))
950  obs = std::dynamic_pointer_cast<CObservationImage>(o);
951  else if (IS_CLASS(o, CObservationStereoImages))
952  stObs =
953  std::dynamic_pointer_cast<CObservationStereoImages>(o);
954  else if (IS_CLASS(o, CObservation3DRangeScan))
955  obs3D =
956  std::dynamic_pointer_cast<CObservation3DRangeScan>(o);
957  }
958  else if (IS_CLASS(newObs, CSensoryFrame))
959  {
960  CSensoryFrame::Ptr sf =
961  std::dynamic_pointer_cast<CSensoryFrame>(newObs);
962 
963  for (CSensoryFrame::iterator i = sf->begin(); i != sf->end();
964  ++i)
965  {
966  CObservation::Ptr& o = *i;
967 
968  if (!m_rawlog_camera_sensor_label.empty() &&
969  m_rawlog_camera_sensor_label != o->sensorLabel)
970  continue;
971 
972  if (IS_CLASS(o, CObservationImage))
973  {
974  obs = std::dynamic_pointer_cast<CObservationImage>(o);
975  break;
976  }
977  else if (IS_CLASS(o, CObservationStereoImages))
978  {
979  stObs =
980  std::dynamic_pointer_cast<CObservationStereoImages>(
981  o);
982  break;
983  }
984  else if (IS_CLASS(o, CObservation3DRangeScan))
985  {
986  obs3D =
987  std::dynamic_pointer_cast<CObservation3DRangeScan>(
988  o);
989  break;
990  }
991  }
992  }
993  if (obs || stObs || obs3D)
994  {
995  // We must convert externally stored images into "normal
996  // in-memory" images.
997  const std::string old_dir =
998  CImage::getImagesPathBase(); // Save current
999  CImage::setImagesPathBase(m_rawlog_detected_images_dir);
1000 
1001  if (obs && obs->image.isExternallyStored())
1002  obs->image.loadFromFile(
1003  obs->image.getExternalStorageFileAbsolutePath());
1004 
1005  if (obs3D && obs3D->hasIntensityImage &&
1006  obs3D->intensityImage.isExternallyStored())
1007  obs3D->intensityImage.loadFromFile(
1008  obs3D->intensityImage
1009  .getExternalStorageFileAbsolutePath());
1010 
1011  if (stObs && stObs->imageLeft.isExternallyStored())
1012  stObs->imageLeft.loadFromFile(
1013  stObs->imageLeft.getExternalStorageFileAbsolutePath());
1014 
1015  if (stObs && stObs->hasImageRight &&
1016  stObs->imageRight.isExternallyStored())
1017  stObs->imageRight.loadFromFile(
1018  stObs->imageRight.getExternalStorageFileAbsolutePath());
1019 
1020  if (stObs && stObs->hasImageDisparity &&
1021  stObs->imageDisparity.isExternallyStored())
1022  stObs->imageDisparity.loadFromFile(
1023  stObs->imageDisparity
1024  .getExternalStorageFileAbsolutePath());
1025 
1026  CImage::setImagesPathBase(old_dir); // Restore
1027  }
1028  else
1029  continue; // Keep reading
1030  }
1031  capture_ok = true;
1032  }
1033  else if (m_cap_flycap)
1034  {
1035  bool ok;
1036  if (!m_cap_flycap->isStereo()) // Mono image
1037  {
1038  obs = mrpt::make_aligned_shared<CObservationImage>();
1039  ok = m_cap_flycap->getObservation(*obs);
1040  }
1041  else // Stereo camera connected
1042  {
1043  stObs = mrpt::make_aligned_shared<CObservationStereoImages>();
1044  ok = m_cap_flycap->getObservation(*stObs);
1045  }
1046 
1047  if (!ok)
1048  { // Error
1049  m_state = CGenericSensor::ssError;
1050  THROW_EXCEPTION("Error grabbing image");
1051  }
1052  else
1053  capture_ok = true;
1054  }
1055  else if (m_cap_flycap_stereo_l && m_cap_flycap_stereo_r)
1056  {
1057  stObs = mrpt::make_aligned_shared<CObservationStereoImages>();
1058 
1059  CObservationImage obsL, obsR;
1060 
1061  bool ok1, ok2 = false;
1062 
1063  ok1 = m_cap_flycap_stereo_r->getObservation(obsL);
1064  if (ok1) ok2 = m_cap_flycap_stereo_l->getObservation(obsR);
1065 
1066  if (!ok1 || !ok2)
1067  {
1068  // Error
1069  m_state = CGenericSensor::ssError;
1070  THROW_EXCEPTION("Error grabbing disparity images");
1071  }
1072  else
1073  {
1074  // Joint the two images as one stereo:
1075  const double At =
1076  mrpt::system::timeDifference(obsL.timestamp, obsR.timestamp);
1077  if (std::abs(At) > 0.1)
1078  {
1079  cout << "[CCamera, flycap_stereo] Warning: Too large delay "
1080  "between left & right images: "
1081  << At << " sec.\n";
1082  }
1083 
1084  // It seems that the timestamp is not always filled in from FlyCap
1085  // driver?
1086  stObs->timestamp =
1087  (obsL.timestamp != 0) ? obsL.timestamp : mrpt::system::now();
1088  stObs->imageLeft.copyFastFrom(obsL.image);
1089  stObs->imageRight.copyFastFrom(obsR.image);
1090  capture_ok = true;
1091  }
1092  }
1093  else if (m_cap_duo3d)
1094  {
1095  stObs = mrpt::make_aligned_shared<CObservationStereoImages>();
1096  obsIMU = mrpt::make_aligned_shared<CObservationIMU>();
1097 
1098  bool thereIsIMG, thereIsIMU;
1099  m_cap_duo3d->getObservations(*stObs, *obsIMU, thereIsIMG, thereIsIMU);
1100  if (!thereIsIMG)
1101  {
1102  m_state = CGenericSensor::ssError;
1103  THROW_EXCEPTION("Error getting observations from DUO3D camera.");
1104  }
1105  else if (m_cap_duo3d->captureIMUIsSet() && !thereIsIMU)
1106  {
1107  cout << "[CCamera, duo3d] Warning: There are no IMU data from the "
1108  "device. Only images are being grabbed.";
1109  }
1110  capture_ok = true;
1111  }
1112  else
1113  {
1115  "There is no initialized camera driver: has 'initialize()' been "
1116  "called?")
1117  }
1118 
1119  ASSERT_(capture_ok);
1120 
1121  // Are we supposed to do a decimation??
1122  m_camera_grab_decimator_counter++;
1123  if (m_camera_grab_decimator_counter < m_camera_grab_decimator)
1124  {
1125  // Done here:
1126  out_obs.push_back(CObservation::Ptr());
1127  return;
1128  }
1129  // Continue as normal:
1130  m_camera_grab_decimator_counter = 0;
1131 
1132  ASSERT_(obs || stObs || obs3D || obsIMU);
1133  // If we grabbed an image: prepare it and add it to the internal queue:
1134  if (obs)
1135  {
1136  obs->sensorLabel = m_sensorLabel;
1137  obs->setSensorPose(m_sensorPose);
1138  }
1139  else if (stObs)
1140  {
1141  stObs->sensorLabel = (m_cap_duo3d && m_cap_duo3d->captureIMUIsSet())
1142  ? m_sensorLabel + "_IMG"
1143  : m_sensorLabel;
1144  stObs->setSensorPose(m_sensorPose);
1145  }
1146  else if (obs3D)
1147  {
1148  obs3D->sensorLabel = m_sensorLabel;
1149  obs3D->setSensorPose(m_sensorPose);
1150  }
1151  if (obsIMU)
1152  {
1153  obsIMU->sensorLabel = m_sensorLabel + "_IMU";
1154  obsIMU->setSensorPose(m_sensorPose);
1155  }
1156 
1157  // Convert to grayscale if the user wants so and the driver did ignored us:
1158  if (m_capture_grayscale)
1159  {
1160  if (obs)
1161  {
1162  if (obs->image.isColor()) obs->image.grayscaleInPlace();
1163  }
1164  else if (stObs)
1165  {
1166  if (stObs->imageLeft.isColor()) stObs->imageLeft.grayscaleInPlace();
1167  if (stObs->hasImageRight && stObs->imageRight.isColor())
1168  stObs->imageRight.grayscaleInPlace();
1169  if (stObs->hasImageDisparity && stObs->imageDisparity.isColor())
1170  stObs->imageDisparity
1171  .grayscaleInPlace(); // Shouldn't happen, but...
1172  }
1173  else if (obs3D)
1174  {
1175  if (obs3D->hasIntensityImage && obs3D->intensityImage.isColor())
1176  obs3D->intensityImage.grayscaleInPlace();
1177  }
1178  }
1179  // External storage?
1180  bool delayed_insertion_in_obs_queue =
1181  false; // If true, we'll return nothing, but the observation will be
1182  // inserted from the thread.
1183 
1184  if (!m_path_for_external_images.empty())
1185  {
1186  if (stObs) // If we have grabbed an stereo observation ...
1187  { // Stereo obs -------
1188  if (m_external_images_own_thread)
1189  {
1190  m_csToSaveList.lock();
1191 
1192  // Select the "m_toSaveList" with the shortest pending queue:
1193  size_t idx_min = 0;
1194  for (size_t i = 0; i < m_toSaveList.size(); ++i)
1195  if (m_toSaveList[i].size() < m_toSaveList[idx_min].size())
1196  idx_min = i;
1197  // Insert:
1198  m_toSaveList[idx_min].insert(
1199  TListObsPair(stObs->timestamp, stObs));
1200 
1201  m_csToSaveList.unlock();
1202 
1203  delayed_insertion_in_obs_queue = true;
1204  }
1205  else
1206  {
1207  const string filNameL =
1208  fileNameStripInvalidChars(trim(m_sensorLabel)) +
1209  format(
1210  "_L_%f.%s", (double)timestampTotime_t(stObs->timestamp),
1211  m_external_images_format.c_str());
1212  const string filNameR =
1213  fileNameStripInvalidChars(trim(m_sensorLabel)) +
1214  format(
1215  "_R_%f.%s", (double)timestampTotime_t(stObs->timestamp),
1216  m_external_images_format.c_str());
1217  const string filNameD =
1218  fileNameStripInvalidChars(trim(m_sensorLabel)) +
1219  format(
1220  "_D_%f.%s", (double)timestampTotime_t(stObs->timestamp),
1221  m_external_images_format.c_str());
1222  // cout << "[CCameraSensor] Saving " << filName << endl;
1223  stObs->imageLeft.saveToFile(
1224  m_path_for_external_images + string("/") + filNameL,
1225  m_external_images_jpeg_quality);
1226  stObs->imageLeft.setExternalStorage(filNameL);
1227 
1228  if (stObs->hasImageRight)
1229  {
1230  stObs->imageRight.saveToFile(
1231  m_path_for_external_images + string("/") + filNameR,
1232  m_external_images_jpeg_quality);
1233  stObs->imageRight.setExternalStorage(filNameR);
1234  }
1235  if (stObs->hasImageDisparity)
1236  {
1237  stObs->imageDisparity.saveToFile(
1238  m_path_for_external_images + string("/") + filNameD,
1239  m_external_images_jpeg_quality);
1240  stObs->imageDisparity.setExternalStorage(filNameD);
1241  }
1242  }
1243  }
1244  else if (obs)
1245  { // Monocular image obs -------
1246  if (m_external_images_own_thread)
1247  {
1248  m_csToSaveList.lock();
1249 
1250  // Select the "m_toSaveList" with the shortest pending queue:
1251  size_t idx_min = 0;
1252  for (size_t i = 0; i < m_toSaveList.size(); ++i)
1253  if (m_toSaveList[i].size() < m_toSaveList[idx_min].size())
1254  idx_min = i;
1255 
1256  // Insert:
1257  m_toSaveList[idx_min].insert(TListObsPair(obs->timestamp, obs));
1258 
1259  m_csToSaveList.unlock();
1260  delayed_insertion_in_obs_queue = true;
1261  }
1262  else
1263  {
1264  string filName =
1265  fileNameStripInvalidChars(trim(m_sensorLabel)) +
1266  format(
1267  "_%f.%s", (double)timestampTotime_t(obs->timestamp),
1268  m_external_images_format.c_str());
1269  // cout << "[CCameraSensor] Saving " << filName << endl;
1270  obs->image.saveToFile(
1271  m_path_for_external_images + string("/") + filName,
1272  m_external_images_jpeg_quality);
1273  obs->image.setExternalStorage(filName);
1274  }
1275  } // end else
1276  }
1277 
1278  // Show preview??
1279  if (m_preview_decimation > 0)
1280  { // Yes
1281  if (++m_preview_counter > m_preview_decimation)
1282  {
1283  m_preview_counter = 0;
1284 
1285  // Create window the first time:
1286  if (!m_preview_win1)
1287  {
1288  string caption = string("Preview of ") + m_sensorLabel;
1289  if (stObs) caption += "-LEFT";
1290  if (m_preview_decimation > 1)
1291  caption +=
1292  format(" (decimation: %i)", m_preview_decimation);
1293  m_preview_win1 =
1294  mrpt::make_aligned_shared<mrpt::gui::CDisplayWindow>(
1295  caption);
1296  }
1297  if (stObs && !m_preview_win2)
1298  {
1299  string caption = string("Preview of ") + m_sensorLabel;
1300  if (stObs) caption += "-RIGHT";
1301  if (m_preview_decimation > 1)
1302  caption +=
1303  format(" (decimation: %i)", m_preview_decimation);
1304  m_preview_win2 =
1305  mrpt::make_aligned_shared<mrpt::gui::CDisplayWindow>(
1306  caption);
1307  }
1308  // Monocular image or Left from a stereo pair:
1309  if (m_preview_win1->isOpen())
1310  {
1311  CImage* img;
1312  if (stObs)
1313  img = &stObs->imageLeft;
1314  else if (obs)
1315  img = &obs->image;
1316  else
1317  img = &obs3D->intensityImage;
1318 
1319  // Apply image reduction?
1320  if (m_preview_reduction >= 2)
1321  {
1322  unsigned int w = img->getWidth();
1323  unsigned int h = img->getHeight();
1324  CImage auxImg;
1325  img->scaleImage(
1326  auxImg, w / m_preview_reduction,
1327  h / m_preview_reduction, IMG_INTERP_NN);
1328  m_preview_win1->showImage(auxImg);
1329  }
1330  else
1331  m_preview_win1->showImage(*img);
1332  }
1333 
1334  // Right from a stereo pair:
1335  if (m_preview_win2 && m_preview_win2->isOpen() && stObs &&
1336  stObs->hasImageRight)
1337  {
1338  // Apply image reduction?
1339  if (m_preview_reduction >= 2)
1340  {
1341  unsigned int w = stObs->imageRight.getWidth();
1342  unsigned int h = stObs->imageRight.getHeight();
1343  CImage auxImg;
1344  stObs->imageRight.scaleImage(
1345  auxImg, w / m_preview_reduction,
1346  h / m_preview_reduction, IMG_INTERP_NN);
1347  m_preview_win2->showImage(auxImg);
1348  }
1349  else
1350  m_preview_win2->showImage(stObs->imageRight);
1351  }
1352 
1353  // Disparity from a stereo pair:
1354  if (m_preview_win2 && m_preview_win2->isOpen() && stObs &&
1355  stObs->hasImageDisparity)
1356  {
1357  // Apply image reduction?
1358  if (m_preview_reduction >= 2)
1359  {
1360  unsigned int w = stObs->imageDisparity.getWidth();
1361  unsigned int h = stObs->imageDisparity.getHeight();
1362  CImage auxImg;
1363  stObs->imageDisparity.scaleImage(
1364  auxImg, w / m_preview_reduction,
1365  h / m_preview_reduction, IMG_INTERP_NN);
1366  m_preview_win2->showImage(auxImg);
1367  }
1368  else
1369  m_preview_win2->showImage(stObs->imageDisparity);
1370  }
1371  }
1372  } // end show preview
1373 
1374  if (delayed_insertion_in_obs_queue)
1375  {
1376  if (m_cap_duo3d && m_cap_duo3d->captureIMUIsSet() && obsIMU)
1377  out_obs.push_back(CObservation::Ptr(obsIMU));
1378  }
1379  else
1380  {
1381  if (stObs) out_obs.push_back(CObservation::Ptr(stObs));
1382  if (obs) out_obs.push_back(CObservation::Ptr(obs));
1383  if (obs3D) out_obs.push_back(CObservation::Ptr(obs3D));
1384  }
1385  return;
1386 }
1387 
1388 /* -----------------------------------------------------
1389  doProcess
1390 ----------------------------------------------------- */
1392 {
1393  vector<CSerializable::Ptr> out_obs;
1394  getNextFrame(out_obs);
1395  appendObservations(out_obs);
1396 }
1397 
1398 /* -----------------------------------------------------
1399  setSoftwareTriggerLevel
1400 ----------------------------------------------------- */
1402 {
1403  if (m_cap_dc1394)
1404  {
1405  if (!m_cap_dc1394->setSoftwareTriggerLevel(level))
1406  { // Error
1407  m_state = CGenericSensor::ssError;
1408  THROW_EXCEPTION("Error setting Trigger level by software");
1409  }
1410  }
1411  else
1412  {
1414  "Software trigger is not implemented for this camera type")
1415  }
1416 }
1417 
1418 /* -----------------------------------------------------
1419  setPathForExternalImages
1420 ----------------------------------------------------- */
1422 {
1423  if (!mrpt::system::createDirectory(directory))
1424  {
1426  "Error: Cannot create the directory for externally saved images: "
1427  "%s",
1428  directory.c_str())
1429  }
1430  m_path_for_external_images = directory;
1431 }
1432 
1433 /* ------------------------------------------------------------------------
1434  prepareVideoSourceFromUserSelection
1435  ------------------------------------------------------------------------ */
1437 {
1438 #if MRPT_HAS_WXWIDGETS
1439  // Create the main wxThread, if it doesn't exist yet:
1441  {
1442  std::cerr << "[mrpt::hwdrivers::prepareVideoSourceFromUserSelection] "
1443  "Error initiating Wx subsystem."
1444  << std::endl;
1445  return CCameraSensor::Ptr(); // Error!
1446  }
1447 
1448  std::promise<void> semDlg;
1449  std::promise<mrpt::gui::detail::TReturnAskUserOpenCamera> dlgSelection;
1450 
1451  // Create window:
1454  REQ->OPCODE = 700;
1455  REQ->sourceCameraSelectDialog = true;
1456  REQ->voidPtr = reinterpret_cast<void*>(&semDlg);
1457  REQ->voidPtr2 = reinterpret_cast<void*>(&dlgSelection);
1458  WxSubsystem::pushPendingWxRequest(REQ);
1459 
1460  // Wait for the window to realize and signal it's alive:
1461  if (!WxSubsystem::isConsoleApp())
1462  {
1463  std::this_thread::sleep_for(
1464  20ms); // Force at least 1-2 timer ticks for processing the event:
1465  wxApp::GetInstance()->Yield(true);
1466  }
1467 
1468  // wait for window construction:
1469  int maxTimeout =
1470 #ifdef _DEBUG
1471  30000;
1472 #else
1473  6000;
1474 #endif
1475  // If we have an "MRPT_WXSUBSYS_TIMEOUT_MS" environment variable, use that
1476  // timeout instead:
1477  const char* envVal = getenv("MRPT_WXSUBSYS_TIMEOUT_MS");
1478  if (envVal) maxTimeout = atoi(envVal);
1479 
1480  if (semDlg.get_future().wait_for(std::chrono::milliseconds(maxTimeout)) ==
1481  std::future_status::timeout)
1482  {
1483  cerr << "[prepareVideoSourceFromUserSelection] Timeout waiting window "
1484  "creation."
1485  << endl;
1486  return CCameraSensor::Ptr();
1487  }
1488 
1489  // wait for user selection:
1490  auto future = dlgSelection.get_future();
1491  future.wait();
1492 
1493  // If the user didn't accept the dialog, return now:
1494  if (!future.get().accepted_by_user) return CCameraSensor::Ptr();
1495 
1496  CCameraSensor::Ptr cam = mrpt::make_aligned_shared<CCameraSensor>();
1497  cam->loadConfig(future.get().selectedConfig, "CONFIG");
1498  cam->initialize(); // This will raise an exception if neccesary
1499 
1500  return cam;
1501 #else
1502  THROW_EXCEPTION("MRPT compiled without wxWidgets");
1503 #endif // MRPT_HAS_WXWIDGETS
1504 }
1505 
1506 /* ------------------------------------------------------------------------
1507  prepareVideoSourceFromPanel
1508  ------------------------------------------------------------------------ */
1510 {
1511 #if MRPT_HAS_WXWIDGETS
1512 
1513  try
1514  {
1515  CConfigFileMemory cfg;
1516  writeConfigFromVideoSourcePanel(_panel, "CONFIG", &cfg);
1517 
1518  // Try to open the camera:
1519  CCameraSensor::Ptr video = mrpt::make_aligned_shared<CCameraSensor>();
1520  video->loadConfig(cfg, "CONFIG");
1521 
1522  // This will raise an exception if neccesary
1523  video->initialize();
1524 
1525  return video;
1526  }
1527  catch (std::exception& e)
1528  {
1529  cerr << endl << e.what() << endl;
1530  wxMessageBox(_("Couldn't open video source"), _("Error"));
1531  return CCameraSensor::Ptr();
1532  }
1533 #else
1534  THROW_EXCEPTION("MRPT compiled without wxWidgets");
1535 #endif // MRPT_HAS_WXWIDGETS
1536 }
1537 
1538 /* ------------------------------------------------------------------------
1539  writeConfigFromVideoSourcePanel
1540  ------------------------------------------------------------------------ */
1542  void* _panel, const std::string& sect, mrpt::config::CConfigFileBase* cfg)
1543 {
1544  MRPT_START
1545 #if MRPT_HAS_WXWIDGETS
1546  ASSERT_(_panel);
1548  reinterpret_cast<mrpt::gui::CPanelCameraSelection*>(_panel);
1549  ASSERTMSG_(
1550  panel, "panel must be of type mrpt::gui::CPanelCameraSelection *");
1551  panel->writeConfigFromVideoSourcePanel(sect, cfg);
1552 
1553 #else
1554  THROW_EXCEPTION("MRPT compiled without wxWidgets");
1555 #endif // MRPT_HAS_WXWIDGETS
1556  MRPT_END
1557 }
1558 
1559 /* ------------------------------------------------------------------------
1560  readConfigIntoVideoSourcePanel
1561  ------------------------------------------------------------------------ */
1563  void* _panel, const std::string& sect,
1564  const mrpt::config::CConfigFileBase* cfg)
1565 {
1566  MRPT_START
1567 #if MRPT_HAS_WXWIDGETS
1568  ASSERT_(_panel);
1570  reinterpret_cast<mrpt::gui::CPanelCameraSelection*>(_panel);
1571  ASSERTMSG_(
1572  panel, "panel must be of type mrpt::gui::CPanelCameraSelection *");
1573 
1574  panel->readConfigIntoVideoSourcePanel(sect, cfg);
1575 
1576 #else
1577  THROW_EXCEPTION("MRPT compiled without wxWidgets");
1578 #endif // MRPT_HAS_WXWIDGETS
1579  MRPT_END
1580 }
1581 
1582 /* -----------------------------------------------------
1583  THREAD: Saver of external images
1584  ----------------------------------------------------- */
1585 void CCameraSensor::thread_save_images(unsigned int my_working_thread_index)
1586 {
1587  while (!m_threadImagesSaverShouldEnd)
1588  {
1589  TListObservations newObs;
1590 
1591  // is there any new image?
1592  m_csToSaveList.lock();
1593  m_toSaveList[my_working_thread_index].swap(newObs);
1594  m_csToSaveList.unlock();
1595 
1596  for (TListObservations::const_iterator i = newObs.begin();
1597  i != newObs.end(); ++i)
1598  {
1599  // Optional user-code hook:
1600  if (m_hook_pre_save)
1601  {
1602  if (IS_DERIVED(i->second, CObservation))
1603  {
1605  std::dynamic_pointer_cast<mrpt::obs::CObservation>(
1606  i->second);
1607  m_hook_pre_save(obs, m_hook_pre_save_param);
1608  }
1609  }
1610 
1611  if (IS_CLASS(i->second, CObservationImage))
1612  {
1614  std::dynamic_pointer_cast<CObservationImage>(i->second);
1615 
1616  string filName =
1617  fileNameStripInvalidChars(trim(m_sensorLabel)) +
1618  format(
1619  "_%f.%s", (double)timestampTotime_t(obs->timestamp),
1620  m_external_images_format.c_str());
1621 
1622  obs->image.saveToFile(
1623  m_path_for_external_images + string("/") + filName,
1624  m_external_images_jpeg_quality);
1625  obs->image.setExternalStorage(filName);
1626  }
1627  else if (IS_CLASS(i->second, CObservationStereoImages))
1628  {
1630  std::dynamic_pointer_cast<CObservationStereoImages>(
1631  i->second);
1632 
1633  const string filNameL =
1634  fileNameStripInvalidChars(trim(m_sensorLabel)) +
1635  format(
1636  "_L_%f.%s", (double)timestampTotime_t(stObs->timestamp),
1637  m_external_images_format.c_str());
1638  const string filNameR =
1639  fileNameStripInvalidChars(trim(m_sensorLabel)) +
1640  format(
1641  "_R_%f.%s", (double)timestampTotime_t(stObs->timestamp),
1642  m_external_images_format.c_str());
1643  const string filNameD =
1644  fileNameStripInvalidChars(trim(m_sensorLabel)) +
1645  format(
1646  "_D_%f.%s", (double)timestampTotime_t(stObs->timestamp),
1647  m_external_images_format.c_str());
1648 
1649  stObs->imageLeft.saveToFile(
1650  m_path_for_external_images + string("/") + filNameL,
1651  m_external_images_jpeg_quality);
1652  stObs->imageLeft.setExternalStorage(filNameL);
1653 
1654  if (stObs->hasImageRight)
1655  {
1656  stObs->imageRight.saveToFile(
1657  m_path_for_external_images + string("/") + filNameR,
1658  m_external_images_jpeg_quality);
1659  stObs->imageRight.setExternalStorage(filNameR);
1660  }
1661  if (stObs->hasImageDisparity)
1662  {
1663  stObs->imageDisparity.saveToFile(
1664  m_path_for_external_images + string("/") + filNameD,
1665  m_external_images_jpeg_quality);
1666  stObs->imageDisparity.setExternalStorage(filNameD);
1667  }
1668  }
1669 
1670  // Append now:
1671  appendObservation(i->second);
1672  }
1673 
1674  std::this_thread::sleep_for(2ms);
1675  }
1676 }
mrpt::io::CFileGZInputStream CFileGZInputStream
void * voidPtr
Parameters, depending on OPCODE.
Definition: WxSubsystem.h:229
This class implements a config file-like interface over a memory-stored string list.
Scalar * iterator
Definition: eigen_plugins.h:26
#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.
A class for grabing stereo images from a STOC camera of Videre Design NOTE:
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:158
std::deque< CObservation::Ptr >::iterator iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
#define MRPT_START
Definition: exceptions.h:262
TCameraType
These capture types are like their OpenCV equivalents.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
The data structure for each inter-thread request:
Definition: WxSubsystem.h:192
static ENUMTYPE name2value(const std::string &name)
Gives the numerical name for a given enum text name.
Definition: TEnumType.h:96
This "software driver" implements the communication protocol for interfacing a DUO3D Stereo Camera...
Definition: CDUO3DCamera.h:156
CCameraSensor::Ptr prepareVideoSourceFromUserSelection()
Show to the user a list of possible camera drivers and creates and open the selected camera...
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
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)
std::pair< mrpt::system::TTimeStamp, mrpt::serialization::CSerializable::Ptr > TListObsPair
double DEG2RAD(const double x)
Degrees to radians.
A class for grabing "range images" from a MESA imaging SwissRanger 3D cameras (SR-2, SR-3000, SR-4k).
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:75
Contains classes for various device interfaces.
void readConfigIntoVideoSourcePanel(void *panel, const std::string &in_cfgfile_section_name, const mrpt::config::CConfigFileBase *in_cfgfile)
Parse the given section of the given configuration file and set accordingly the controls of the wxWid...
A generic class which process a video file or other kind of input stream (http, rtsp) and allows the ...
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
STL namespace.
std::string fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars='_')
Replace invalid filename chars by underscores (&#39;_&#39;) or any other user-given char. ...
Definition: filesystem.cpp:328
int OPCODE
Valid codes are: For CDisplayWindow:
Definition: WxSubsystem.h:299
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
A class for grabing "range images", intensity images (either RGB or IR) and other information from an...
void writeConfigFromVideoSourcePanel(void *panel, const std::string &in_cfgfile_section_name, mrpt::config::CConfigFileBase *out_cfgfile)
Parse the user options in the wxWidgets "panel" and write the configuration into the given section of...
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream
Definition: CArchive.h:561
void thread_save_images(unsigned int my_working_thread_index)
Thread to save images to files.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
#define IS_DERIVED(ptrObj, class_name)
Evaluates to true if a pointer to an object (derived from mrpt::rtti::CObject) is an instance of the ...
Definition: CObject.h:109
A class for grabing "range images", intensity images (either RGB or IR) and other information from an...
Definition: CKinect.h:268
This class allows loading and storing values and vectors of different types from a configuration text...
void close()
Close the camera (if open).
The central class for camera grabbers in MRPT, implementing the "generic sensor" interface.
std::string lowerCase(const std::string &str)
Returns an lower-case version of a string.
std::shared_ptr< CCameraSensor > Ptr
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)...
GLint GLvoid * img
Definition: glext.h:3763
Versatile class for consistent logging and management of output messages.
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...
Observation class for either a pair of left+right or left+disparity images from a stereo camera...
This namespace contains representation of robot actions and observations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:54
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
void readConfigIntoVideoSourcePanel(const std::string &sect, const mrpt::config::CConfigFileBase *cfg) const
Definition: WxUtils.cpp:1044
bool sourceCameraSelectDialog
Only one of source* can be non-nullptr, indicating the class that generated the request.
Definition: WxSubsystem.h:221
static std::string & trim(std::string &s)
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 ...
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
void writeConfigFromVideoSourcePanel(const std::string &sect, mrpt::config::CConfigFileBase *cfg) const
Definition: WxUtils.cpp:877
GLsizei const GLchar ** string
Definition: glext.h:4101
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
static void startSyncCapture(int numCameras, const CImageGrabber_FlyCapture2 **cameras_array)
Starts a synchronous capture of several cameras, which must have been already opened.
unsigned __int64 uint64_t
Definition: rptypes.h:50
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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:2217
std::multimap< mrpt::system::TTimeStamp, mrpt::serialization::CSerializable::Ptr > TListObservations
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&#39;s observation.
Definition: CObservation.h:43
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:103
std::string upperCase(const std::string &str)
Returns a upper-case version of a string.
#define MRPT_END
Definition: exceptions.h:266
mrpt::obs::CObservation::Ptr getNextFrame()
Retrieves the next frame from the video source, raising an exception on any error.
A class for grabing images from a "OpenCV"-compatible camera, or from an AVI video file...
GLint level
Definition: glext.h:3600
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:31
void setSoftwareTriggerLevel(bool level)
Set Software trigger level value (ON or OFF) for cameras with this function available.
A panel to select the camera input from all the formats supported by MRPT.
Definition: WxUtils.h:171
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
std::string trim(const std::string &str)
Removes leading and trailing spaces.
GLenum GLsizei GLenum format
Definition: glext.h:3531
Classes for creating GUI windows for 2D and 3D visualization.
Definition: about_box.h:16
GLsizeiptr size
Definition: glext.h:3923
double 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:209
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
const Scalar * const_iterator
Definition: eigen_plugins.h:27
static bool createOneInstanceMainThread()
Thread-safe method to create one single instance of the main wxWidgets thread: it will create the thr...
double 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:56
CCameraSensor::Ptr prepareVideoSourceFromPanel(void *panel)
Used only from MRPT apps: Use with caution since "panel" MUST be a "mrpt::gui::CPanelCameraSelection ...
virtual ~CCameraSensor()
Destructor.
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:130



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019