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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: abb8b1a1e Fri Oct 18 14:19:12 2019 +0200 at vie oct 18 14:20:13 CEST 2019