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 << format("[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  "Error: Cannot create the directory for externally saved images: "
1395  "%s",
1396  directory.c_str());
1397  }
1398  m_path_for_external_images = directory;
1399 }
1400 
1401 /* ------------------------------------------------------------------------
1402  prepareVideoSourceFromUserSelection
1403  ------------------------------------------------------------------------ */
1405 {
1406 #if MRPT_HAS_WXWIDGETS
1407  // Create the main wxThread, if it doesn't exist yet:
1409  {
1410  std::cerr << "[mrpt::hwdrivers::prepareVideoSourceFromUserSelection] "
1411  "Error initiating Wx subsystem."
1412  << std::endl;
1413  return CCameraSensor::Ptr(); // Error!
1414  }
1415 
1416  std::promise<void> semDlg;
1417  std::promise<mrpt::gui::detail::TReturnAskUserOpenCamera> dlgSelection;
1418 
1419  // Create window:
1420  auto* REQ = new WxSubsystem::TRequestToWxMainThread[1];
1421  REQ->OPCODE = 700;
1422  REQ->sourceCameraSelectDialog = true;
1423  REQ->voidPtr = reinterpret_cast<void*>(&semDlg);
1424  REQ->voidPtr2 = reinterpret_cast<void*>(&dlgSelection);
1425  WxSubsystem::pushPendingWxRequest(REQ);
1426 
1427  // Wait for the window to realize and signal it's alive:
1428  if (!WxSubsystem::isConsoleApp())
1429  {
1430  std::this_thread::sleep_for(
1431  20ms); // Force at least 1-2 timer ticks for processing the event:
1432  wxApp::GetInstance()->Yield(true);
1433  }
1434 
1435  // wait for window construction:
1436  int maxTimeout =
1437 #ifdef _DEBUG
1438  30000;
1439 #else
1440  6000;
1441 #endif
1442  // If we have an "MRPT_WXSUBSYS_TIMEOUT_MS" environment variable, use that
1443  // timeout instead:
1444  const char* envVal = getenv("MRPT_WXSUBSYS_TIMEOUT_MS");
1445  if (envVal) maxTimeout = atoi(envVal);
1446 
1447  if (semDlg.get_future().wait_for(std::chrono::milliseconds(maxTimeout)) ==
1448  std::future_status::timeout)
1449  {
1450  cerr << "[prepareVideoSourceFromUserSelection] Timeout waiting window "
1451  "creation."
1452  << endl;
1453  return CCameraSensor::Ptr();
1454  }
1455 
1456  // wait for user selection:
1457  auto future = dlgSelection.get_future();
1458  future.wait();
1459  const auto& ret = future.get();
1460 
1461  // If the user didn't accept the dialog, return now:
1462  if (!ret.accepted_by_user) return CCameraSensor::Ptr();
1463 
1464  mrpt::config::CConfigFileMemory selectedConfig(ret.selectedConfig);
1465 
1466  CCameraSensor::Ptr cam = std::make_shared<CCameraSensor>();
1467  cam->loadConfig(selectedConfig, "CONFIG");
1468  cam->initialize(); // This will raise an exception if neccesary
1469 
1470  return cam;
1471 #else
1472  THROW_EXCEPTION("MRPT compiled without wxWidgets");
1473 #endif // MRPT_HAS_WXWIDGETS
1474 }
1475 
1476 /* ------------------------------------------------------------------------
1477  prepareVideoSourceFromPanel
1478  ------------------------------------------------------------------------ */
1480 {
1481 #if MRPT_HAS_WXWIDGETS
1482 
1483  try
1484  {
1485  CConfigFileMemory cfg;
1486  writeConfigFromVideoSourcePanel(_panel, "CONFIG", &cfg);
1487 
1488  // Try to open the camera:
1489  CCameraSensor::Ptr video = std::make_shared<CCameraSensor>();
1490  video->loadConfig(cfg, "CONFIG");
1491 
1492  // This will raise an exception if neccesary
1493  video->initialize();
1494 
1495  return video;
1496  }
1497  catch (const std::exception& e)
1498  {
1499  cerr << endl << e.what() << endl;
1500  wxMessageBox(_("Couldn't open video source"), _("Error"));
1501  return CCameraSensor::Ptr();
1502  }
1503 #else
1504  THROW_EXCEPTION("MRPT compiled without wxWidgets");
1505 #endif // MRPT_HAS_WXWIDGETS
1506 }
1507 
1508 /* ------------------------------------------------------------------------
1509  writeConfigFromVideoSourcePanel
1510  ------------------------------------------------------------------------ */
1512  void* _panel, const std::string& sect, mrpt::config::CConfigFileBase* cfg)
1513 {
1514  MRPT_START
1515 #if MRPT_HAS_WXWIDGETS
1516  ASSERT_(_panel);
1517  auto* panel = reinterpret_cast<mrpt::gui::CPanelCameraSelection*>(_panel);
1518  ASSERTMSG_(
1519  panel, "panel must be of type mrpt::gui::CPanelCameraSelection *");
1520  panel->writeConfigFromVideoSourcePanel(sect, cfg);
1521 
1522 #else
1523  THROW_EXCEPTION("MRPT compiled without wxWidgets");
1524 #endif // MRPT_HAS_WXWIDGETS
1525  MRPT_END
1526 }
1527 
1528 /* ------------------------------------------------------------------------
1529  readConfigIntoVideoSourcePanel
1530  ------------------------------------------------------------------------ */
1532  void* _panel, const std::string& sect,
1533  const mrpt::config::CConfigFileBase* cfg)
1534 {
1535  MRPT_START
1536 #if MRPT_HAS_WXWIDGETS
1537  ASSERT_(_panel);
1538  auto* panel = reinterpret_cast<mrpt::gui::CPanelCameraSelection*>(_panel);
1539  ASSERTMSG_(
1540  panel, "panel must be of type mrpt::gui::CPanelCameraSelection *");
1541 
1542  panel->readConfigIntoVideoSourcePanel(sect, cfg);
1543 
1544 #else
1545  THROW_EXCEPTION("MRPT compiled without wxWidgets");
1546 #endif // MRPT_HAS_WXWIDGETS
1547  MRPT_END
1548 }
1549 
1550 /* -----------------------------------------------------
1551  THREAD: Saver of external images
1552  ----------------------------------------------------- */
1553 void CCameraSensor::thread_save_images(unsigned int my_working_thread_index)
1554 {
1555  while (!m_threadImagesSaverShouldEnd)
1556  {
1557  TListObservations newObs;
1558 
1559  // is there any new image?
1560  m_csToSaveList.lock();
1561  m_toSaveList[my_working_thread_index].swap(newObs);
1562  m_csToSaveList.unlock();
1563 
1564  for (auto i = newObs.begin(); i != newObs.end(); ++i)
1565  {
1566  // Optional user-code hook:
1567  if (m_hook_pre_save)
1568  {
1569  if (IS_DERIVED(*i->second, CObservation))
1570  {
1572  std::dynamic_pointer_cast<mrpt::obs::CObservation>(
1573  i->second);
1574  m_hook_pre_save(obs, m_hook_pre_save_param);
1575  }
1576  }
1577 
1578  if (IS_CLASS(*i->second, CObservationImage))
1579  {
1581  std::dynamic_pointer_cast<CObservationImage>(i->second);
1582 
1583  string filName =
1584  fileNameStripInvalidChars(trim(m_sensorLabel)) +
1585  format(
1586  "_%f.%s", (double)timestampTotime_t(obs->timestamp),
1587  m_external_images_format.c_str());
1588 
1589  obs->image.saveToFile(
1590  m_path_for_external_images + string("/") + filName,
1591  m_external_images_jpeg_quality);
1592  obs->image.setExternalStorage(filName);
1593  }
1594  else if (IS_CLASS(*i->second, CObservationStereoImages))
1595  {
1597  std::dynamic_pointer_cast<CObservationStereoImages>(
1598  i->second);
1599 
1600  const string filNameL =
1601  fileNameStripInvalidChars(trim(m_sensorLabel)) +
1602  format(
1603  "_L_%f.%s", (double)timestampTotime_t(stObs->timestamp),
1604  m_external_images_format.c_str());
1605  const string filNameR =
1606  fileNameStripInvalidChars(trim(m_sensorLabel)) +
1607  format(
1608  "_R_%f.%s", (double)timestampTotime_t(stObs->timestamp),
1609  m_external_images_format.c_str());
1610  const string filNameD =
1611  fileNameStripInvalidChars(trim(m_sensorLabel)) +
1612  format(
1613  "_D_%f.%s", (double)timestampTotime_t(stObs->timestamp),
1614  m_external_images_format.c_str());
1615 
1616  stObs->imageLeft.saveToFile(
1617  m_path_for_external_images + string("/") + filNameL,
1618  m_external_images_jpeg_quality);
1619  stObs->imageLeft.setExternalStorage(filNameL);
1620 
1621  if (stObs->hasImageRight)
1622  {
1623  stObs->imageRight.saveToFile(
1624  m_path_for_external_images + string("/") + filNameR,
1625  m_external_images_jpeg_quality);
1626  stObs->imageRight.setExternalStorage(filNameR);
1627  }
1628  if (stObs->hasImageDisparity)
1629  {
1630  stObs->imageDisparity.saveToFile(
1631  m_path_for_external_images + string("/") + filNameD,
1632  m_external_images_jpeg_quality);
1633  stObs->imageDisparity.setExternalStorage(filNameD);
1634  }
1635  }
1636 
1637  // Append now:
1638  appendObservation(i->second);
1639  }
1640 
1641  std::this_thread::sleep_for(2ms);
1642  }
1643 }
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
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:878
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:1682
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:586
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:847
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
#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.
unsigned __int64 uint64_t
Definition: rptypes.h:53
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 format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at dom jul 14 20:00:11 CEST 2019