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



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 1e710274b Thu May 28 15:30:14 2020 +0200 at jue may 28 15:45:10 CEST 2020