40 using namespace std::literals;
50 m_grabber_type("opencv"),
61 m_img_dir_left_format("imL_%04d.jpg"),
62 m_img_dir_right_format("imR_%04d.jpg"),
64 m_external_image_saver_count(std::thread::hardware_concurrency()),
66 m_hook_pre_save(
nullptr)
69 m_sensorLabel =
"CAMERA";
78 cout <<
"[CCameraSensor::initialize] Opening camera..." << endl;
85 if (m_grabber_type ==
"opencv")
94 catch (std::exception&)
100 "[CCameraSensor::initialize] opencv camera, index: %i type: "
102 int(m_cv_camera_index), (
int)ct);
103 m_cap_cv = std::make_unique<CImageGrabber_OpenCV>(
104 m_cv_camera_index, ct, m_cv_options);
106 if (!m_cap_cv->isOpen())
110 "[CCameraSensor::initialize] ERROR: Couldn't open OpenCV "
114 else if (m_grabber_type ==
"dc1394")
118 "[CCameraSensor::initialize] dc1394 camera, GUID: 0x%lX "
120 long(m_dc1394_camera_guid), m_dc1394_camera_unit);
121 m_cap_dc1394 = std::make_unique<CImageGrabber_dc1394>(
122 m_dc1394_camera_guid, m_dc1394_camera_unit, m_dc1394_options,
125 if (!m_cap_dc1394->isOpen())
129 "[CCameraSensor::initialize] ERROR: Couldn't open dc1394 "
133 else if (m_grabber_type ==
"bumblebee_dc1394")
136 "[CCameraSensor::initialize] bumblebee_libdc1394 camera: "
137 "GUID:0x%08X Index:%i FPS:%f...\n",
138 (
unsigned int)(m_bumblebee_dc1394_camera_guid),
139 m_bumblebee_dc1394_camera_unit, m_bumblebee_dc1394_framerate);
140 m_cap_bumblebee_dc1394 =
141 std::make_unique<CStereoGrabber_Bumblebee_libdc1394>(
142 m_bumblebee_dc1394_camera_guid, m_bumblebee_dc1394_camera_unit,
143 m_bumblebee_dc1394_framerate);
145 else if (m_grabber_type ==
"svs")
148 "[CCameraSensor::initialize] SVS camera: %u...\n",
149 (
unsigned int)(m_svs_camera_index));
150 m_cap_svs = std::make_unique<CStereoGrabber_SVS>(
151 m_svs_camera_index, m_svs_options);
153 else if (m_grabber_type ==
"ffmpeg")
157 "[CCameraSensor::initialize] FFmpeg stream: %s...\n",
158 m_ffmpeg_url.c_str());
159 m_cap_ffmpeg = std::make_unique<CFFMPEG_InputStream>();
161 if (!m_cap_ffmpeg->openURL(m_ffmpeg_url, m_capture_grayscale))
165 "Error opening FFmpeg stream: %s", m_ffmpeg_url.c_str());
168 else if (m_grabber_type ==
"swissranger")
170 cout <<
"[CCameraSensor::initialize] SwissRanger camera...\n";
171 m_cap_swissranger = std::make_unique<CSwissRanger3DCamera>();
173 m_cap_swissranger->setOpenFromUSB(m_sr_open_from_usb);
174 m_cap_swissranger->setOpenIPAddress(m_sr_ip_address);
176 m_cap_swissranger->setSave3D(m_sr_save_3d);
177 m_cap_swissranger->setSaveRangeImage(m_sr_save_range_img);
178 m_cap_swissranger->setSaveIntensityImage(m_sr_save_intensity_img);
179 m_cap_swissranger->setSaveConfidenceImage(m_sr_save_confidence);
181 if (!m_path_for_external_images.empty())
182 m_cap_swissranger->setPathForExternalImages(
183 m_path_for_external_images);
191 catch (std::exception&)
197 else if (m_grabber_type ==
"kinect")
199 cout <<
"[CCameraSensor::initialize] Kinect camera...\n";
200 m_cap_kinect = std::make_unique<CKinect>();
201 m_cap_kinect->enableGrab3DPoints(m_kinect_save_3d);
202 m_cap_kinect->enableGrabDepth(m_kinect_save_range_img);
203 m_cap_kinect->enableGrabRGB(m_kinect_save_intensity_img);
204 m_cap_kinect->setVideoChannel(
208 if (!m_path_for_external_images.empty())
209 m_cap_kinect->setPathForExternalImages(m_path_for_external_images);
217 catch (std::exception&)
223 else if (m_grabber_type ==
"openni2")
225 cout <<
"[CCameraSensor::initialize] OpenNI2 sensor...\n";
226 m_cap_openni2 = std::make_unique<COpenNI2Sensor>();
227 m_cap_openni2->enableGrab3DPoints(m_kinect_save_3d);
231 m_cap_openni2->enableGrabDepth(m_kinect_save_range_img);
232 m_cap_openni2->enableGrabRGB(m_kinect_save_intensity_img);
234 if (!m_path_for_external_images.empty())
235 m_cap_openni2->setPathForExternalImages(m_path_for_external_images);
243 catch (
const std::exception& e)
249 else if (m_grabber_type ==
"image_dir")
253 "[CCameraSensor::initialize] Image dir: %s...\n",
254 m_img_dir_url.c_str());
255 m_cap_image_dir = std::make_unique<std::string>();
257 else if (m_grabber_type ==
"rawlog")
261 "[CCameraSensor::initialize] Rawlog stream: %s...\n",
262 m_rawlog_file.c_str());
263 m_cap_rawlog = std::make_unique<CFileGZInputStream>();
265 if (!m_cap_rawlog->open(m_rawlog_file))
269 "Error opening rawlog file: %s", m_rawlog_file.c_str());
273 m_rawlog_detected_images_dir =
274 CRawlog::detectImagesDirectory(m_rawlog_file);
276 else if (m_grabber_type ==
"flycap")
278 cout <<
"[CCameraSensor::initialize] PGR FlyCapture2 camera...\n";
283 std::make_unique<CImageGrabber_FlyCapture2>(m_flycap_options);
285 catch (std::exception&)
291 else if (m_grabber_type ==
"flycap_stereo")
294 <<
"[CCameraSensor::initialize] PGR FlyCapture2 stereo camera...\n";
298 m_cap_flycap_stereo_l =
299 std::make_unique<CImageGrabber_FlyCapture2>();
300 m_cap_flycap_stereo_r =
301 std::make_unique<CImageGrabber_FlyCapture2>();
303 cout <<
"[CCameraSensor::initialize] PGR FlyCapture2 stereo "
304 "camera: Opening LEFT camera...\n";
305 m_cap_flycap_stereo_l->open(
306 m_flycap_stereo_options[0],
false );
308 cout <<
"[CCameraSensor::initialize] PGR FlyCapture2 stereo "
309 "camera: Opening RIGHT camera...\n";
310 m_cap_flycap_stereo_r->open(
311 m_flycap_stereo_options[1],
false );
314 if (m_fcs_start_synch_capture)
317 m_cap_flycap_stereo_l.get(), m_cap_flycap_stereo_r.get()};
322 m_cap_flycap_stereo_l->startCapture();
323 m_cap_flycap_stereo_r->startCapture();
326 catch (std::exception&)
332 else if (m_grabber_type ==
"duo3d")
335 cout <<
"[CCameraSensor::initialize] DUO3D stereo camera ...\n";
340 m_cap_duo3d = std::make_unique<CDUO3DCamera>(m_duo3d_options);
342 catch (
const std::exception& e)
348 else if (m_grabber_type ==
"myntd")
350 cout <<
"[CCameraSensor::initialize] MYNTEYE-D camera ...\n";
355 m_myntd = std::make_unique<CMyntEyeCamera>(m_myntd_options);
357 catch (
const std::exception& e)
365 "Unknown 'grabber_type' found: %s", m_grabber_type.c_str());
368 cout <<
"[CCameraSensor::initialize] Done!" << endl;
372 if (m_external_images_own_thread)
374 m_threadImagesSaverShouldEnd =
false;
376 m_threadImagesSaver.clear();
377 m_threadImagesSaver.resize(m_external_image_saver_count);
379 m_toSaveList.clear();
380 m_toSaveList.resize(m_external_image_saver_count);
382 for (
unsigned int i = 0; i < m_external_image_saver_count; ++i)
384 m_threadImagesSaver[i] =
388 "CameraSaveImages", m_threadImagesSaver[i]);
399 m_cap_dc1394.reset();
400 m_cap_flycap.reset();
401 m_cap_flycap_stereo_l.reset();
402 m_cap_flycap_stereo_r.reset();
403 m_cap_bumblebee_dc1394.reset();
404 m_cap_ffmpeg.reset();
405 m_cap_rawlog.reset();
406 m_cap_swissranger.reset();
407 m_cap_kinect.reset();
409 m_cap_image_dir.reset();
415 if (!m_threadImagesSaver.empty())
417 m_threadImagesSaverShouldEnd =
true;
418 for (
auto& i : m_threadImagesSaver) i.join();
427 const std::string& iniSection)
434 if (m_grab_decimation > 0)
436 m_camera_grab_decimator = m_grab_decimation;
437 m_camera_grab_decimator_counter = 0;
439 m_grab_decimation = 0;
442 m_camera_grab_decimator = m_camera_grab_decimator_counter = 0;
445 iniSection,
"grabber_type", m_grabber_type);
447 preview_decimation,
int, m_preview_decimation, configSource, iniSection)
449 preview_reduction,
int, m_preview_reduction, configSource, iniSection)
453 iniSection,
"cv_camera_type", m_cv_camera_type);
455 configSource.
read_int(iniSection,
"cv_camera_index", m_cv_camera_index);
457 m_cv_options.frame_width = configSource.
read_int(
458 iniSection,
"cv_frame_width", m_cv_options.frame_width);
459 m_cv_options.frame_height = configSource.
read_int(
460 iniSection,
"cv_frame_height", m_cv_options.frame_height);
462 configSource.
read_double(iniSection,
"cv_gain", m_cv_options.gain);
463 m_cv_options.ieee1394_fps = configSource.
read_double(
464 iniSection,
"cv_fps", m_cv_options.ieee1394_fps);
466 m_capture_grayscale =
467 configSource.
read_bool(iniSection,
"capture_grayscale",
false);
469 m_cv_options.ieee1394_grayscale = m_capture_grayscale;
473 dc1394_camera_guid, uint64_t, m_dc1394_camera_guid, configSource,
476 dc1394_camera_unit,
int, m_dc1394_camera_unit, configSource, iniSection)
479 dc1394_frame_width,
int, m_dc1394_options.frame_width, configSource,
482 dc1394_frame_height,
int, m_dc1394_options.frame_height, configSource,
486 dc1394_mode7,
int, m_dc1394_options.mode7, configSource, iniSection)
489 dc1394_shutter,
int, m_dc1394_options.shutter, configSource, iniSection)
491 dc1394_gain,
int, m_dc1394_options.gain, configSource, iniSection)
493 dc1394_gamma,
int, m_dc1394_options.gamma, configSource, iniSection)
495 dc1394_brightness,
int, m_dc1394_options.brightness, configSource,
498 dc1394_exposure,
int, m_dc1394_options.exposure, configSource,
501 dc1394_sharpness,
int, m_dc1394_options.sharpness, configSource,
504 dc1394_white_balance,
int, m_dc1394_options.white_balance, configSource,
508 dc1394_shutter_mode,
int, m_dc1394_options.shutter_mode, configSource,
511 dc1394_gain_mode,
int, m_dc1394_options.gain_mode, configSource,
514 dc1394_gamma_mode,
int, m_dc1394_options.gamma_mode, configSource,
517 dc1394_brightness_mode,
int, m_dc1394_options.brightness_mode,
518 configSource, iniSection)
520 dc1394_exposure_mode,
int, m_dc1394_options.exposure_mode, configSource,
523 dc1394_sharpness_mode,
int, m_dc1394_options.sharpness_mode,
524 configSource, iniSection)
526 dc1394_white_balance_mode,
int, m_dc1394_options.white_balance_mode,
527 configSource, iniSection)
530 dc1394_trigger_power,
int, m_dc1394_options.trigger_power, configSource,
533 dc1394_trigger_mode,
int, m_dc1394_options.trigger_mode, configSource,
536 dc1394_trigger_source,
int, m_dc1394_options.trigger_source,
537 configSource, iniSection)
539 dc1394_trigger_polarity,
int, m_dc1394_options.trigger_polarity,
540 configSource, iniSection)
542 dc1394_ring_buffer_size,
int, m_dc1394_options.ring_buffer_size,
543 configSource, iniSection)
547 bumblebee_dc1394_camera_guid, uint64_t, m_bumblebee_dc1394_camera_guid,
548 configSource, iniSection)
550 bumblebee_dc1394_camera_unit,
int, m_bumblebee_dc1394_camera_unit,
551 configSource, iniSection)
553 bumblebee_dc1394_framerate,
double, m_bumblebee_dc1394_framerate,
554 configSource, iniSection)
557 m_svs_camera_index = configSource.
read_int(
558 iniSection,
"svs_camera_index", m_svs_camera_index);
559 m_svs_options.frame_width = configSource.
read_int(
560 iniSection,
"svs_frame_width", m_svs_options.frame_width);
561 m_svs_options.frame_height = configSource.
read_int(
562 iniSection,
"svs_frame_height", m_svs_options.frame_height);
563 m_svs_options.framerate = configSource.
read_double(
564 iniSection,
"svs_framerate", m_svs_options.framerate);
565 m_svs_options.m_NDisp =
566 configSource.
read_int(iniSection,
"svs_NDisp", m_svs_options.m_NDisp);
567 m_svs_options.m_Corrsize = configSource.
read_int(
568 iniSection,
"svs_Corrsize", m_svs_options.m_Corrsize);
570 configSource.
read_int(iniSection,
"svs_LR", m_svs_options.m_LR);
571 m_svs_options.m_Thresh =
572 configSource.
read_int(iniSection,
"svs_Thresh", m_svs_options.m_Thresh);
573 m_svs_options.m_Unique =
574 configSource.
read_int(iniSection,
"svs_Unique", m_svs_options.m_Unique);
575 m_svs_options.m_Horopter = configSource.
read_int(
576 iniSection,
"svs_Horopter", m_svs_options.m_Horopter);
577 m_svs_options.m_SpeckleSize = configSource.
read_int(
578 iniSection,
"svs_SpeckleSize", m_svs_options.m_SpeckleSize);
579 m_svs_options.m_procesOnChip = configSource.
read_bool(
580 iniSection,
"svs_procesOnChip", m_svs_options.m_procesOnChip);
581 m_svs_options.m_calDisparity = configSource.
read_bool(
582 iniSection,
"svs_calDisparity", m_svs_options.m_calDisparity);
586 configSource.
read_string(iniSection,
"ffmpeg_url", m_ffmpeg_url));
590 configSource.
read_string(iniSection,
"rawlog_file", m_rawlog_file));
592 iniSection,
"rawlog_camera_sensor_label",
593 m_rawlog_camera_sensor_label));
597 configSource.
read_string(iniSection,
"image_dir_url", m_img_dir_url));
599 iniSection,
"left_format", m_img_dir_left_format));
601 configSource.
read_string(iniSection,
"right_format",
""));
602 m_img_dir_start_index =
603 configSource.
read_int(iniSection,
"start_index", m_img_dir_start_index);
605 m_img_dir_end_index =
606 configSource.
read_int(iniSection,
"end_index", m_img_dir_end_index);
608 m_img_dir_is_stereo = !m_img_dir_right_format.empty();
609 m_img_dir_counter = m_img_dir_start_index;
612 m_duo3d_options.loadOptionsFrom(configSource,
"DUO3DOptions");
616 configSource.
read_bool(iniSection,
"sr_use_usb", m_sr_open_from_usb);
618 configSource.
read_string(iniSection,
"sr_IP", m_sr_ip_address);
621 configSource.
read_bool(iniSection,
"sr_grab_3d", m_sr_save_3d);
622 m_sr_save_intensity_img = configSource.
read_bool(
623 iniSection,
"sr_grab_grayscale", m_sr_save_intensity_img);
624 m_sr_save_range_img = configSource.
read_bool(
625 iniSection,
"sr_grab_range", m_sr_save_range_img);
626 m_sr_save_confidence = configSource.
read_bool(
627 iniSection,
"sr_grab_confidence", m_sr_save_confidence);
630 configSource.
read_bool(iniSection,
"kinect_grab_3d", m_kinect_save_3d);
631 m_kinect_save_intensity_img = configSource.
read_bool(
632 iniSection,
"kinect_grab_intensity", m_kinect_save_intensity_img);
633 m_kinect_save_range_img = configSource.
read_bool(
634 iniSection,
"kinect_grab_range", m_kinect_save_range_img);
635 m_kinect_video_rgb = configSource.
read_bool(
636 iniSection,
"kinect_video_rgb", m_kinect_video_rgb);
639 m_flycap_options.loadOptionsFrom(configSource, iniSection,
"flycap_");
644 m_myntd_options.loadFromConfigFile(c, iniSection);
648 m_fcs_start_synch_capture = configSource.
read_bool(
649 iniSection,
"fcs_start_synch_capture", m_fcs_start_synch_capture);
650 m_flycap_stereo_options[0].loadOptionsFrom(
651 configSource, iniSection,
"fcs_LEFT_");
652 m_flycap_stereo_options[1].loadOptionsFrom(
653 configSource, iniSection,
"fcs_RIGHT_");
656 map<double, grabber_dc1394_framerate_t> map_fps;
657 map<double, grabber_dc1394_framerate_t>::iterator it_fps;
669 configSource.
read_double(iniSection,
"dc1394_framerate", 15.0);
670 it_fps = map_fps.find(the_fps);
671 if (it_fps == map_fps.end())
673 "ERROR: DC1394 framerate seems to be not a valid number: %f",
676 m_dc1394_options.framerate = it_fps->second;
679 map<string, grabber_dc1394_color_coding_t> map_color;
680 map<string, grabber_dc1394_color_coding_t>::iterator it_color;
681 #define ADD_COLOR_MAP(c) map_color[#c] = c;
689 string the_color_coding =
691 iniSection,
"dc1394_color_coding",
"COLOR_CODING_YUV422"));
692 it_color = map_color.find(the_color_coding);
693 if (it_color == map_color.end())
695 "ERROR: Color coding seems not to be valid : '%s'",
696 the_color_coding.c_str());
697 m_dc1394_options.color_coding = it_color->second;
700 iniSection,
"external_images_format", m_external_images_format));
701 m_external_images_jpeg_quality = configSource.
read_int(
702 iniSection,
"external_images_jpeg_quality",
703 m_external_images_jpeg_quality);
704 m_external_images_own_thread = configSource.
read_bool(
705 iniSection,
"external_images_own_thread", m_external_images_own_thread);
706 m_external_image_saver_count = configSource.
read_int(
707 iniSection,
"external_images_own_thread_count",
708 m_external_image_saver_count);
711 m_sensorPose.setFromValues(
712 configSource.
read_float(iniSection,
"pose_x", 0),
713 configSource.
read_float(iniSection,
"pose_y", 0),
714 configSource.
read_float(iniSection,
"pose_z", 0),
727 m_preview_win1.reset();
728 m_preview_win2.reset();
735 vector<CSerializable::Ptr> out_obs;
736 getNextFrame(out_obs);
737 return std::dynamic_pointer_cast<CObservation>(out_obs[0]);
751 bool capture_ok =
false;
755 obs = std::make_shared<CObservationImage>();
756 if (!m_cap_cv->getObservation(*obs))
764 else if (m_cap_dc1394)
766 obs = std::make_shared<CObservationImage>();
767 if (!m_cap_dc1394->getObservation(*obs))
775 else if (m_cap_swissranger)
777 obs3D = std::make_shared<CObservation3DRangeScan>();
779 bool there_is_obs, hardware_error;
780 m_cap_swissranger->getNextObservation(
781 *obs3D, there_is_obs, hardware_error);
783 if (!there_is_obs || hardware_error)
791 else if (m_cap_kinect)
793 obs3D = std::make_shared<CObservation3DRangeScan>();
798 double max_timeout = 3.0;
802 const char* envVal = getenv(
"MRPT_CCAMERA_KINECT_TIMEOUT_MS");
803 if (envVal) max_timeout = atoi(envVal) * 0.001;
805 bool there_is_obs, hardware_error;
808 m_cap_kinect->getNextObservation(
809 *obs3D, there_is_obs, hardware_error);
810 if (!there_is_obs) std::this_thread::sleep_for(1ms);
814 if (!there_is_obs || hardware_error)
822 else if (m_cap_openni2)
824 obs3D = std::make_shared<CObservation3DRangeScan>();
828 double max_timeout = 3.0;
829 bool there_is_obs, hardware_error;
832 m_cap_openni2->getNextObservation(
833 *obs3D, there_is_obs, hardware_error);
834 if (!there_is_obs) std::this_thread::sleep_for(1ms);
838 if (!there_is_obs || hardware_error)
846 else if (m_cap_bumblebee_dc1394)
848 stObs = std::make_shared<CObservationStereoImages>();
849 if (!m_cap_bumblebee_dc1394->getStereoObservation(*stObs))
861 stObs = std::make_shared<CObservationStereoImages>();
863 if (!m_cap_svs->getStereoObservation(*stObs))
872 else if (m_cap_ffmpeg)
874 static bool anyGood =
false;
876 if (!m_cap_ffmpeg->retrieveFrame(im))
887 "ffmpeg capture driver: Failed to get frame (temporary "
894 obs = std::make_shared<CObservationImage>();
895 obs->image = std::move(im);
901 else if (m_cap_image_dir)
903 if (m_img_dir_counter > m_img_dir_end_index)
909 std::string auxL =
format(
910 "%s/%s", m_img_dir_url.c_str(), m_img_dir_left_format.c_str());
911 if (m_img_dir_is_stereo)
913 stObs = std::make_shared<CObservationStereoImages>();
914 if (!stObs->imageLeft.loadFromFile(
915 format(auxL.c_str(), m_img_dir_counter)))
920 std::string auxR =
format(
921 "%s/%s", m_img_dir_url.c_str(), m_img_dir_right_format.c_str());
922 if (!stObs->imageRight.loadFromFile(
923 format(auxR.c_str(), m_img_dir_counter++)))
934 obs = std::make_shared<CObservationImage>();
935 if (!obs->image.loadFromFile(
936 format(auxL.c_str(), m_img_dir_counter++)))
945 else if (m_cap_rawlog)
950 while (!obs && !stObs && !obs3D)
956 std::dynamic_pointer_cast<CObservation>(newObs);
957 if (!m_rawlog_camera_sensor_label.empty() &&
958 m_rawlog_camera_sensor_label != o->sensorLabel)
962 obs = std::dynamic_pointer_cast<CObservationImage>(o);
965 std::dynamic_pointer_cast<CObservationStereoImages>(o);
968 std::dynamic_pointer_cast<CObservation3DRangeScan>(o);
973 std::dynamic_pointer_cast<CSensoryFrame>(newObs);
977 if (!m_rawlog_camera_sensor_label.empty() &&
978 m_rawlog_camera_sensor_label != o->sensorLabel)
983 obs = std::dynamic_pointer_cast<CObservationImage>(o);
989 std::dynamic_pointer_cast<CObservationStereoImages>(
996 std::dynamic_pointer_cast<CObservation3DRangeScan>(
1002 if (obs || stObs || obs3D)
1006 const std::string old_dir =
1007 CImage::getImagesPathBase();
1008 CImage::setImagesPathBase(m_rawlog_detected_images_dir);
1010 if (obs && obs->image.isExternallyStored())
1011 obs->image.loadFromFile(
1012 obs->image.getExternalStorageFileAbsolutePath());
1014 if (obs3D && obs3D->hasIntensityImage &&
1015 obs3D->intensityImage.isExternallyStored())
1016 obs3D->intensityImage.loadFromFile(
1017 obs3D->intensityImage
1018 .getExternalStorageFileAbsolutePath());
1020 if (stObs && stObs->imageLeft.isExternallyStored())
1021 stObs->imageLeft.loadFromFile(
1022 stObs->imageLeft.getExternalStorageFileAbsolutePath());
1024 if (stObs && stObs->hasImageRight &&
1025 stObs->imageRight.isExternallyStored())
1026 stObs->imageRight.loadFromFile(
1027 stObs->imageRight.getExternalStorageFileAbsolutePath());
1029 if (stObs && stObs->hasImageDisparity &&
1030 stObs->imageDisparity.isExternallyStored())
1031 stObs->imageDisparity.loadFromFile(
1032 stObs->imageDisparity
1033 .getExternalStorageFileAbsolutePath());
1035 CImage::setImagesPathBase(old_dir);
1042 else if (m_cap_flycap)
1045 if (!m_cap_flycap->isStereo())
1047 obs = std::make_shared<CObservationImage>();
1048 ok = m_cap_flycap->getObservation(*obs);
1052 stObs = std::make_shared<CObservationStereoImages>();
1053 ok = m_cap_flycap->getObservation(*stObs);
1064 else if (m_cap_flycap_stereo_l && m_cap_flycap_stereo_r)
1066 stObs = std::make_shared<CObservationStereoImages>();
1070 bool ok1, ok2 =
false;
1072 ok1 = m_cap_flycap_stereo_r->getObservation(obsL);
1073 if (ok1) ok2 = m_cap_flycap_stereo_l->getObservation(obsR);
1086 if (std::abs(At) > 0.1)
1088 cout <<
"[CCamera, flycap_stereo] Warning: Too large delay "
1089 "between left & right images: "
1098 stObs->imageLeft = std::move(obsL.
image);
1099 stObs->imageRight = std::move(obsR.
image);
1103 else if (m_cap_duo3d)
1105 stObs = std::make_shared<CObservationStereoImages>();
1106 obsIMU = std::make_shared<CObservationIMU>();
1108 bool thereIsIMG, thereIsIMU;
1109 m_cap_duo3d->getObservations(*stObs, *obsIMU, thereIsIMG, thereIsIMU);
1115 else if (m_cap_duo3d->captureIMUIsSet() && !thereIsIMU)
1117 cout <<
"[CCamera, duo3d] Warning: There are no IMU data from the "
1118 "device. Only images are being grabbed.";
1124 obs3D = std::make_shared<CObservation3DRangeScan>();
1126 bool thereIsObs = m_myntd->getObservation(*obs3D);
1127 static int noObsCnt = 0;
1132 if (noObsCnt++ > 100)
1136 "Error getting observations from MYNTEYE-D camera.");
1148 "There is no initialized camera driver: has 'initialize()' been "
1155 m_camera_grab_decimator_counter++;
1156 if (m_camera_grab_decimator_counter < m_camera_grab_decimator)
1163 m_camera_grab_decimator_counter = 0;
1171 obs->sensorLabel = m_sensorLabel;
1172 obs->setSensorPose(m_sensorPose);
1176 stObs->sensorLabel = (m_cap_duo3d && m_cap_duo3d->captureIMUIsSet())
1177 ? m_sensorLabel +
"_IMG"
1179 stObs->setSensorPose(m_sensorPose);
1183 obs3D->sensorLabel = m_sensorLabel;
1184 obs3D->setSensorPose(m_sensorPose);
1188 obsIMU->sensorLabel = m_sensorLabel +
"_IMU";
1189 obsIMU->setSensorPose(m_sensorPose);
1193 if (m_capture_grayscale)
1197 if (obs->image.isColor()) obs->image = obs->image.grayscale();
1201 if (stObs->imageLeft.isColor())
1202 stObs->imageLeft = stObs->imageLeft.grayscale();
1203 if (stObs->hasImageRight && stObs->imageRight.isColor())
1204 stObs->imageRight = stObs->imageRight.grayscale();
1205 if (stObs->hasImageDisparity && stObs->imageDisparity.isColor())
1206 stObs->imageDisparity = stObs->imageDisparity.grayscale();
1210 if (obs3D->hasIntensityImage && obs3D->intensityImage.isColor())
1211 obs3D->intensityImage = obs3D->intensityImage.grayscale();
1216 CImage img4gui, img4guiR;
1217 if (m_preview_win1 && m_preview_win1->isOpen())
1233 bool delayed_insertion_in_obs_queue =
false;
1234 if (!m_path_for_external_images.empty())
1238 if (m_external_images_own_thread)
1240 m_csToSaveList.lock();
1244 for (
size_t i = 0; i < m_toSaveList.size(); ++i)
1245 if (m_toSaveList[i].
size() < m_toSaveList[idx_min].size())
1248 m_toSaveList[idx_min].insert(
1251 m_csToSaveList.unlock();
1253 delayed_insertion_in_obs_queue =
true;
1257 const string filNameL =
1261 m_external_images_format.c_str());
1262 const string filNameR =
1266 m_external_images_format.c_str());
1267 const string filNameD =
1271 m_external_images_format.c_str());
1273 stObs->imageLeft.saveToFile(
1274 m_path_for_external_images +
string(
"/") + filNameL,
1275 m_external_images_jpeg_quality);
1276 stObs->imageLeft.setExternalStorage(filNameL);
1278 if (stObs->hasImageRight)
1280 stObs->imageRight.saveToFile(
1281 m_path_for_external_images +
string(
"/") + filNameR,
1282 m_external_images_jpeg_quality);
1283 stObs->imageRight.setExternalStorage(filNameR);
1285 if (stObs->hasImageDisparity)
1287 stObs->imageDisparity.saveToFile(
1288 m_path_for_external_images +
string(
"/") + filNameD,
1289 m_external_images_jpeg_quality);
1290 stObs->imageDisparity.setExternalStorage(filNameD);
1296 if (m_external_images_own_thread)
1298 m_csToSaveList.lock();
1302 for (
size_t i = 0; i < m_toSaveList.size(); ++i)
1303 if (m_toSaveList[i].
size() < m_toSaveList[idx_min].
size())
1307 m_toSaveList[idx_min].insert(
TListObsPair(obs->timestamp, obs));
1309 m_csToSaveList.unlock();
1310 delayed_insertion_in_obs_queue =
true;
1318 m_external_images_format.c_str());
1320 obs->image.saveToFile(
1321 m_path_for_external_images +
string(
"/") + filName,
1322 m_external_images_jpeg_quality);
1323 obs->image.setExternalStorage(filName);
1329 if (m_preview_decimation > 0)
1331 if (++m_preview_counter > m_preview_decimation)
1333 m_preview_counter = 0;
1336 if (!m_preview_win1)
1338 string caption = string(
"Preview of ") + m_sensorLabel;
1339 if (stObs) caption +=
"-LEFT";
1340 if (m_preview_decimation > 1)
1342 format(
" (decimation: %i)", m_preview_decimation);
1345 if (stObs && !m_preview_win2)
1347 string caption = string(
"Preview of ") + m_sensorLabel;
1348 if (stObs) caption +=
"-RIGHT";
1349 if (m_preview_decimation > 1)
1351 format(
" (decimation: %i)", m_preview_decimation);
1355 if (m_preview_win1->isOpen() && img4gui.
getWidth() > 0)
1358 if (m_preview_reduction >= 2)
1360 unsigned int w = img4gui.
getWidth();
1364 auxImg, w / m_preview_reduction,
1366 m_preview_win1->showImage(auxImg);
1369 m_preview_win1->showImage(img4gui);
1373 if (m_preview_win2 && m_preview_win2->isOpen() && stObs &&
1374 stObs->hasImageRight && img4gui.
getWidth() > 0)
1377 if (m_preview_reduction >= 2)
1379 unsigned int w = img4guiR.
getWidth();
1383 auxImg, w / m_preview_reduction,
1385 m_preview_win2->showImage(auxImg);
1388 m_preview_win2->showImage(img4guiR);
1392 if (m_preview_win2 && m_preview_win2->isOpen() && stObs &&
1393 stObs->hasImageDisparity)
1396 if (m_preview_reduction >= 2)
1398 unsigned int w = stObs->imageDisparity.getWidth();
1399 unsigned int h = stObs->imageDisparity.getHeight();
1402 auxImg, w / m_preview_reduction,
1404 m_preview_win2->showImage(auxImg);
1407 m_preview_win2->showImage(stObs->imageDisparity);
1412 if (delayed_insertion_in_obs_queue)
1414 if (m_cap_duo3d && m_cap_duo3d->captureIMUIsSet() && obsIMU)
1431 vector<CSerializable::Ptr> out_obs;
1432 getNextFrame(out_obs);
1433 appendObservations(out_obs);
1443 if (!m_cap_dc1394->setSoftwareTriggerLevel(level))
1452 "Software trigger is not implemented for this camera type");
1464 "Cannot create the directory for externally saved images: `%s`",
1467 m_path_for_external_images = directory;
1475 #if MRPT_HAS_WXWIDGETS
1479 std::cerr <<
"[mrpt::hwdrivers::prepareVideoSourceFromUserSelection] "
1480 "Error initiating Wx subsystem."
1485 std::promise<void> semDlg;
1486 std::promise<mrpt::gui::detail::TReturnAskUserOpenCamera> dlgSelection;
1491 REQ->sourceCameraSelectDialog =
true;
1492 REQ->voidPtr =
reinterpret_cast<void*
>(&semDlg);
1493 REQ->voidPtr2 =
reinterpret_cast<void*
>(&dlgSelection);
1494 WxSubsystem::pushPendingWxRequest(REQ);
1497 if (!WxSubsystem::isConsoleApp())
1499 std::this_thread::sleep_for(
1501 wxApp::GetInstance()->Yield(
true);
1513 const char* envVal = getenv(
"MRPT_WXSUBSYS_TIMEOUT_MS");
1514 if (envVal) maxTimeout = atoi(envVal);
1516 if (semDlg.get_future().wait_for(std::chrono::milliseconds(maxTimeout)) ==
1517 std::future_status::timeout)
1519 cerr <<
"[prepareVideoSourceFromUserSelection] Timeout waiting window "
1526 auto future = dlgSelection.get_future();
1528 const auto& ret = future.get();
1536 cam->loadConfig(selectedConfig,
"CONFIG");
1542 #endif // MRPT_HAS_WXWIDGETS
1550 #if MRPT_HAS_WXWIDGETS
1559 video->loadConfig(cfg,
"CONFIG");
1562 video->initialize();
1566 catch (
const std::exception& e)
1568 cerr << endl << e.what() << endl;
1569 wxMessageBox(_(
"Couldn't open video source"), _(
"Error"));
1574 #endif // MRPT_HAS_WXWIDGETS
1584 #if MRPT_HAS_WXWIDGETS
1588 panel,
"panel must be of type mrpt::gui::CPanelCameraSelection *");
1589 panel->writeConfigFromVideoSourcePanel(
sect, cfg);
1593 #endif // MRPT_HAS_WXWIDGETS
1601 void* _panel,
const std::string&
sect,
1605 #if MRPT_HAS_WXWIDGETS
1609 panel,
"panel must be of type mrpt::gui::CPanelCameraSelection *");
1611 panel->readConfigIntoVideoSourcePanel(
sect, cfg);
1615 #endif // MRPT_HAS_WXWIDGETS
1624 while (!m_threadImagesSaverShouldEnd)
1629 m_csToSaveList.lock();
1630 m_toSaveList[my_working_thread_index].swap(newObs);
1631 m_csToSaveList.unlock();
1633 for (
auto i = newObs.begin(); i != newObs.end(); ++i)
1636 if (m_hook_pre_save)
1641 std::dynamic_pointer_cast<mrpt::obs::CObservation>(
1643 m_hook_pre_save(obs, m_hook_pre_save_param);
1650 std::dynamic_pointer_cast<CObservationImage>(i->second);
1656 m_external_images_format.c_str());
1658 obs->image.saveToFile(
1659 m_path_for_external_images +
string(
"/") + filName,
1660 m_external_images_jpeg_quality);
1661 obs->image.setExternalStorage(filName);
1666 std::dynamic_pointer_cast<CObservationStereoImages>(
1669 const string filNameL =
1673 m_external_images_format.c_str());
1674 const string filNameR =
1678 m_external_images_format.c_str());
1679 const string filNameD =
1683 m_external_images_format.c_str());
1685 stObs->imageLeft.saveToFile(
1686 m_path_for_external_images +
string(
"/") + filNameL,
1687 m_external_images_jpeg_quality);
1688 stObs->imageLeft.setExternalStorage(filNameL);
1690 if (stObs->hasImageRight)
1692 stObs->imageRight.saveToFile(
1693 m_path_for_external_images +
string(
"/") + filNameR,
1694 m_external_images_jpeg_quality);
1695 stObs->imageRight.setExternalStorage(filNameR);
1697 if (stObs->hasImageDisparity)
1699 stObs->imageDisparity.saveToFile(
1700 m_path_for_external_images +
string(
"/") + filNameD,
1701 m_external_images_jpeg_quality);
1702 stObs->imageDisparity.setExternalStorage(filNameD);
1707 appendObservation(i->second);
1710 std::this_thread::sleep_for(2ms);