39 using namespace std::literals;
49 m_grabber_type("opencv"),
60 m_img_dir_left_format("imL_%04d.jpg"),
61 m_img_dir_right_format("imR_%04d.jpg"),
63 m_external_image_saver_count(std::thread::hardware_concurrency()),
65 m_hook_pre_save(
nullptr)
68 m_sensorLabel =
"CAMERA";
77 cout <<
"[CCameraSensor::initialize] Opening camera..." << endl;
84 if (m_grabber_type ==
"opencv")
93 catch (std::exception&)
99 "[CCameraSensor::initialize] opencv camera, index: %i type: "
101 int(m_cv_camera_index), (
int)ct);
102 m_cap_cv = std::make_unique<CImageGrabber_OpenCV>(
103 m_cv_camera_index, ct, m_cv_options);
105 if (!m_cap_cv->isOpen())
109 "[CCameraSensor::initialize] ERROR: Couldn't open OpenCV "
113 else if (m_grabber_type ==
"dc1394")
117 "[CCameraSensor::initialize] dc1394 camera, GUID: 0x%lX "
119 long(m_dc1394_camera_guid), m_dc1394_camera_unit);
120 m_cap_dc1394 = std::make_unique<CImageGrabber_dc1394>(
121 m_dc1394_camera_guid, m_dc1394_camera_unit, m_dc1394_options,
124 if (!m_cap_dc1394->isOpen())
128 "[CCameraSensor::initialize] ERROR: Couldn't open dc1394 "
132 else if (m_grabber_type ==
"bumblebee_dc1394")
135 "[CCameraSensor::initialize] bumblebee_libdc1394 camera: "
136 "GUID:0x%08X Index:%i FPS:%f...\n",
137 (
unsigned int)(m_bumblebee_dc1394_camera_guid),
138 m_bumblebee_dc1394_camera_unit, m_bumblebee_dc1394_framerate);
139 m_cap_bumblebee_dc1394 =
140 std::make_unique<CStereoGrabber_Bumblebee_libdc1394>(
141 m_bumblebee_dc1394_camera_guid, m_bumblebee_dc1394_camera_unit,
142 m_bumblebee_dc1394_framerate);
144 else if (m_grabber_type ==
"svs")
147 "[CCameraSensor::initialize] SVS camera: %u...\n",
148 (
unsigned int)(m_svs_camera_index));
149 m_cap_svs = std::make_unique<CStereoGrabber_SVS>(
150 m_svs_camera_index, m_svs_options);
152 else if (m_grabber_type ==
"ffmpeg")
156 "[CCameraSensor::initialize] FFmpeg stream: %s...\n",
157 m_ffmpeg_url.c_str());
158 m_cap_ffmpeg = std::make_unique<CFFMPEG_InputStream>();
160 if (!m_cap_ffmpeg->openURL(m_ffmpeg_url, m_capture_grayscale))
164 "Error opening FFmpeg stream: %s", m_ffmpeg_url.c_str());
167 else if (m_grabber_type ==
"swissranger")
169 cout <<
"[CCameraSensor::initialize] SwissRanger camera...\n";
170 m_cap_swissranger = std::make_unique<CSwissRanger3DCamera>();
172 m_cap_swissranger->setOpenFromUSB(m_sr_open_from_usb);
173 m_cap_swissranger->setOpenIPAddress(m_sr_ip_address);
175 m_cap_swissranger->setSave3D(m_sr_save_3d);
176 m_cap_swissranger->setSaveRangeImage(m_sr_save_range_img);
177 m_cap_swissranger->setSaveIntensityImage(m_sr_save_intensity_img);
178 m_cap_swissranger->setSaveConfidenceImage(m_sr_save_confidence);
180 if (!m_path_for_external_images.empty())
181 m_cap_swissranger->setPathForExternalImages(
182 m_path_for_external_images);
190 catch (std::exception&)
196 else if (m_grabber_type ==
"kinect")
198 cout <<
"[CCameraSensor::initialize] Kinect camera...\n";
199 m_cap_kinect = std::make_unique<CKinect>();
200 m_cap_kinect->enableGrab3DPoints(m_kinect_save_3d);
201 m_cap_kinect->enableGrabDepth(m_kinect_save_range_img);
202 m_cap_kinect->enableGrabRGB(m_kinect_save_intensity_img);
203 m_cap_kinect->setVideoChannel(
207 if (!m_path_for_external_images.empty())
208 m_cap_kinect->setPathForExternalImages(m_path_for_external_images);
216 catch (std::exception&)
222 else if (m_grabber_type ==
"openni2")
224 cout <<
"[CCameraSensor::initialize] OpenNI2 sensor...\n";
225 m_cap_openni2 = std::make_unique<COpenNI2Sensor>();
226 m_cap_openni2->enableGrab3DPoints(m_kinect_save_3d);
230 m_cap_openni2->enableGrabDepth(m_kinect_save_range_img);
231 m_cap_openni2->enableGrabRGB(m_kinect_save_intensity_img);
233 if (!m_path_for_external_images.empty())
234 m_cap_openni2->setPathForExternalImages(m_path_for_external_images);
242 catch (
const std::exception& e)
248 else if (m_grabber_type ==
"image_dir")
252 "[CCameraSensor::initialize] Image dir: %s...\n",
253 m_img_dir_url.c_str());
254 m_cap_image_dir = std::make_unique<std::string>();
256 else if (m_grabber_type ==
"rawlog")
260 "[CCameraSensor::initialize] Rawlog stream: %s...\n",
261 m_rawlog_file.c_str());
262 m_cap_rawlog = std::make_unique<CFileGZInputStream>();
264 if (!m_cap_rawlog->open(m_rawlog_file))
268 "Error opening rawlog file: %s", m_rawlog_file.c_str());
272 m_rawlog_detected_images_dir =
273 CRawlog::detectImagesDirectory(m_rawlog_file);
275 else if (m_grabber_type ==
"flycap")
277 cout <<
"[CCameraSensor::initialize] PGR FlyCapture2 camera...\n";
282 std::make_unique<CImageGrabber_FlyCapture2>(m_flycap_options);
284 catch (std::exception&)
290 else if (m_grabber_type ==
"flycap_stereo")
293 <<
"[CCameraSensor::initialize] PGR FlyCapture2 stereo camera...\n";
297 m_cap_flycap_stereo_l =
298 std::make_unique<CImageGrabber_FlyCapture2>();
299 m_cap_flycap_stereo_r =
300 std::make_unique<CImageGrabber_FlyCapture2>();
302 cout <<
"[CCameraSensor::initialize] PGR FlyCapture2 stereo "
303 "camera: Opening LEFT camera...\n";
304 m_cap_flycap_stereo_l->open(
305 m_flycap_stereo_options[0],
false );
307 cout <<
"[CCameraSensor::initialize] PGR FlyCapture2 stereo "
308 "camera: Opening RIGHT camera...\n";
309 m_cap_flycap_stereo_r->open(
310 m_flycap_stereo_options[1],
false );
313 if (m_fcs_start_synch_capture)
316 m_cap_flycap_stereo_l.get(), m_cap_flycap_stereo_r.get()};
321 m_cap_flycap_stereo_l->startCapture();
322 m_cap_flycap_stereo_r->startCapture();
325 catch (std::exception&)
331 else if (m_grabber_type ==
"duo3d")
334 cout <<
"[CCameraSensor::initialize] DUO3D stereo camera ...\n";
339 m_cap_duo3d = std::make_unique<CDUO3DCamera>(m_duo3d_options);
341 catch (
const std::exception& e)
347 else if (m_grabber_type ==
"myntd")
349 cout <<
"[CCameraSensor::initialize] MYNTEYE-D camera ...\n";
354 m_myntd = std::make_unique<CMyntEyeCamera>(m_myntd_options);
356 catch (
const std::exception& e)
364 "Unknown 'grabber_type' found: %s", m_grabber_type.c_str());
367 cout <<
"[CCameraSensor::initialize] Done!" << endl;
371 if (m_external_images_own_thread)
373 m_threadImagesSaverShouldEnd =
false;
375 m_threadImagesSaver.clear();
376 m_threadImagesSaver.resize(m_external_image_saver_count);
378 m_toSaveList.clear();
379 m_toSaveList.resize(m_external_image_saver_count);
381 for (
unsigned int i = 0; i < m_external_image_saver_count; ++i)
382 m_threadImagesSaver[i] =
393 m_cap_dc1394.reset();
394 m_cap_flycap.reset();
395 m_cap_flycap_stereo_l.reset();
396 m_cap_flycap_stereo_r.reset();
397 m_cap_bumblebee_dc1394.reset();
398 m_cap_ffmpeg.reset();
399 m_cap_rawlog.reset();
400 m_cap_swissranger.reset();
401 m_cap_kinect.reset();
403 m_cap_image_dir.reset();
409 if (!m_threadImagesSaver.empty())
411 m_threadImagesSaverShouldEnd =
true;
412 for (
auto& i : m_threadImagesSaver) i.join();
421 const std::string& iniSection)
428 if (m_grab_decimation > 0)
430 m_camera_grab_decimator = m_grab_decimation;
431 m_camera_grab_decimator_counter = 0;
433 m_grab_decimation = 0;
436 m_camera_grab_decimator = m_camera_grab_decimator_counter = 0;
439 iniSection,
"grabber_type", m_grabber_type);
441 preview_decimation,
int, m_preview_decimation, configSource, iniSection)
443 preview_reduction,
int, m_preview_reduction, configSource, iniSection)
447 iniSection,
"cv_camera_type", m_cv_camera_type);
449 configSource.
read_int(iniSection,
"cv_camera_index", m_cv_camera_index);
451 m_cv_options.frame_width = configSource.
read_int(
452 iniSection,
"cv_frame_width", m_cv_options.frame_width);
453 m_cv_options.frame_height = configSource.
read_int(
454 iniSection,
"cv_frame_height", m_cv_options.frame_height);
456 configSource.
read_double(iniSection,
"cv_gain", m_cv_options.gain);
457 m_cv_options.ieee1394_fps = configSource.
read_double(
458 iniSection,
"cv_fps", m_cv_options.ieee1394_fps);
460 m_capture_grayscale =
461 configSource.
read_bool(iniSection,
"capture_grayscale",
false);
463 m_cv_options.ieee1394_grayscale = m_capture_grayscale;
467 dc1394_camera_guid, uint64_t, m_dc1394_camera_guid, configSource,
470 dc1394_camera_unit,
int, m_dc1394_camera_unit, configSource, iniSection)
473 dc1394_frame_width,
int, m_dc1394_options.frame_width, configSource,
476 dc1394_frame_height,
int, m_dc1394_options.frame_height, configSource,
480 dc1394_mode7,
int, m_dc1394_options.mode7, configSource, iniSection)
483 dc1394_shutter,
int, m_dc1394_options.shutter, configSource, iniSection)
485 dc1394_gain,
int, m_dc1394_options.gain, configSource, iniSection)
487 dc1394_gamma,
int, m_dc1394_options.gamma, configSource, iniSection)
489 dc1394_brightness,
int, m_dc1394_options.brightness, configSource,
492 dc1394_exposure,
int, m_dc1394_options.exposure, configSource,
495 dc1394_sharpness,
int, m_dc1394_options.sharpness, configSource,
498 dc1394_white_balance,
int, m_dc1394_options.white_balance, configSource,
502 dc1394_shutter_mode,
int, m_dc1394_options.shutter_mode, configSource,
505 dc1394_gain_mode,
int, m_dc1394_options.gain_mode, configSource,
508 dc1394_gamma_mode,
int, m_dc1394_options.gamma_mode, configSource,
511 dc1394_brightness_mode,
int, m_dc1394_options.brightness_mode,
512 configSource, iniSection)
514 dc1394_exposure_mode,
int, m_dc1394_options.exposure_mode, configSource,
517 dc1394_sharpness_mode,
int, m_dc1394_options.sharpness_mode,
518 configSource, iniSection)
520 dc1394_white_balance_mode,
int, m_dc1394_options.white_balance_mode,
521 configSource, iniSection)
524 dc1394_trigger_power,
int, m_dc1394_options.trigger_power, configSource,
527 dc1394_trigger_mode,
int, m_dc1394_options.trigger_mode, configSource,
530 dc1394_trigger_source,
int, m_dc1394_options.trigger_source,
531 configSource, iniSection)
533 dc1394_trigger_polarity,
int, m_dc1394_options.trigger_polarity,
534 configSource, iniSection)
536 dc1394_ring_buffer_size,
int, m_dc1394_options.ring_buffer_size,
537 configSource, iniSection)
541 bumblebee_dc1394_camera_guid, uint64_t, m_bumblebee_dc1394_camera_guid,
542 configSource, iniSection)
544 bumblebee_dc1394_camera_unit,
int, m_bumblebee_dc1394_camera_unit,
545 configSource, iniSection)
547 bumblebee_dc1394_framerate,
double, m_bumblebee_dc1394_framerate,
548 configSource, iniSection)
551 m_svs_camera_index = configSource.
read_int(
552 iniSection,
"svs_camera_index", m_svs_camera_index);
553 m_svs_options.frame_width = configSource.
read_int(
554 iniSection,
"svs_frame_width", m_svs_options.frame_width);
555 m_svs_options.frame_height = configSource.
read_int(
556 iniSection,
"svs_frame_height", m_svs_options.frame_height);
557 m_svs_options.framerate = configSource.
read_double(
558 iniSection,
"svs_framerate", m_svs_options.framerate);
559 m_svs_options.m_NDisp =
560 configSource.
read_int(iniSection,
"svs_NDisp", m_svs_options.m_NDisp);
561 m_svs_options.m_Corrsize = configSource.
read_int(
562 iniSection,
"svs_Corrsize", m_svs_options.m_Corrsize);
564 configSource.
read_int(iniSection,
"svs_LR", m_svs_options.m_LR);
565 m_svs_options.m_Thresh =
566 configSource.
read_int(iniSection,
"svs_Thresh", m_svs_options.m_Thresh);
567 m_svs_options.m_Unique =
568 configSource.
read_int(iniSection,
"svs_Unique", m_svs_options.m_Unique);
569 m_svs_options.m_Horopter = configSource.
read_int(
570 iniSection,
"svs_Horopter", m_svs_options.m_Horopter);
571 m_svs_options.m_SpeckleSize = configSource.
read_int(
572 iniSection,
"svs_SpeckleSize", m_svs_options.m_SpeckleSize);
573 m_svs_options.m_procesOnChip = configSource.
read_bool(
574 iniSection,
"svs_procesOnChip", m_svs_options.m_procesOnChip);
575 m_svs_options.m_calDisparity = configSource.
read_bool(
576 iniSection,
"svs_calDisparity", m_svs_options.m_calDisparity);
580 configSource.
read_string(iniSection,
"ffmpeg_url", m_ffmpeg_url));
584 configSource.
read_string(iniSection,
"rawlog_file", m_rawlog_file));
586 iniSection,
"rawlog_camera_sensor_label",
587 m_rawlog_camera_sensor_label));
591 configSource.
read_string(iniSection,
"image_dir_url", m_img_dir_url));
593 iniSection,
"left_format", m_img_dir_left_format));
595 configSource.
read_string(iniSection,
"right_format",
""));
596 m_img_dir_start_index =
597 configSource.
read_int(iniSection,
"start_index", m_img_dir_start_index);
599 m_img_dir_end_index =
600 configSource.
read_int(iniSection,
"end_index", m_img_dir_end_index);
602 m_img_dir_is_stereo = !m_img_dir_right_format.empty();
603 m_img_dir_counter = m_img_dir_start_index;
606 m_duo3d_options.loadOptionsFrom(configSource,
"DUO3DOptions");
610 configSource.
read_bool(iniSection,
"sr_use_usb", m_sr_open_from_usb);
612 configSource.
read_string(iniSection,
"sr_IP", m_sr_ip_address);
615 configSource.
read_bool(iniSection,
"sr_grab_3d", m_sr_save_3d);
616 m_sr_save_intensity_img = configSource.
read_bool(
617 iniSection,
"sr_grab_grayscale", m_sr_save_intensity_img);
618 m_sr_save_range_img = configSource.
read_bool(
619 iniSection,
"sr_grab_range", m_sr_save_range_img);
620 m_sr_save_confidence = configSource.
read_bool(
621 iniSection,
"sr_grab_confidence", m_sr_save_confidence);
624 configSource.
read_bool(iniSection,
"kinect_grab_3d", m_kinect_save_3d);
625 m_kinect_save_intensity_img = configSource.
read_bool(
626 iniSection,
"kinect_grab_intensity", m_kinect_save_intensity_img);
627 m_kinect_save_range_img = configSource.
read_bool(
628 iniSection,
"kinect_grab_range", m_kinect_save_range_img);
629 m_kinect_video_rgb = configSource.
read_bool(
630 iniSection,
"kinect_video_rgb", m_kinect_video_rgb);
633 m_flycap_options.loadOptionsFrom(configSource, iniSection,
"flycap_");
638 m_myntd_options.loadFromConfigFile(c, iniSection);
642 m_fcs_start_synch_capture = configSource.
read_bool(
643 iniSection,
"fcs_start_synch_capture", m_fcs_start_synch_capture);
644 m_flycap_stereo_options[0].loadOptionsFrom(
645 configSource, iniSection,
"fcs_LEFT_");
646 m_flycap_stereo_options[1].loadOptionsFrom(
647 configSource, iniSection,
"fcs_RIGHT_");
650 map<double, grabber_dc1394_framerate_t> map_fps;
651 map<double, grabber_dc1394_framerate_t>::iterator it_fps;
663 configSource.
read_double(iniSection,
"dc1394_framerate", 15.0);
664 it_fps = map_fps.find(the_fps);
665 if (it_fps == map_fps.end())
667 "ERROR: DC1394 framerate seems to be not a valid number: %f",
670 m_dc1394_options.framerate = it_fps->second;
673 map<string, grabber_dc1394_color_coding_t> map_color;
674 map<string, grabber_dc1394_color_coding_t>::iterator it_color;
675 #define ADD_COLOR_MAP(c) map_color[#c] = c;
683 string the_color_coding =
685 iniSection,
"dc1394_color_coding",
"COLOR_CODING_YUV422"));
686 it_color = map_color.find(the_color_coding);
687 if (it_color == map_color.end())
689 "ERROR: Color coding seems not to be valid : '%s'",
690 the_color_coding.c_str());
691 m_dc1394_options.color_coding = it_color->second;
694 iniSection,
"external_images_format", m_external_images_format));
695 m_external_images_jpeg_quality = configSource.
read_int(
696 iniSection,
"external_images_jpeg_quality",
697 m_external_images_jpeg_quality);
698 m_external_images_own_thread = configSource.
read_bool(
699 iniSection,
"external_images_own_thread", m_external_images_own_thread);
700 m_external_image_saver_count = configSource.
read_int(
701 iniSection,
"external_images_own_thread_count",
702 m_external_image_saver_count);
705 m_sensorPose.setFromValues(
706 configSource.
read_float(iniSection,
"pose_x", 0),
707 configSource.
read_float(iniSection,
"pose_y", 0),
708 configSource.
read_float(iniSection,
"pose_z", 0),
721 m_preview_win1.reset();
722 m_preview_win2.reset();
729 vector<CSerializable::Ptr> out_obs;
730 getNextFrame(out_obs);
731 return std::dynamic_pointer_cast<CObservation>(out_obs[0]);
745 bool capture_ok =
false;
749 obs = std::make_shared<CObservationImage>();
750 if (!m_cap_cv->getObservation(*obs))
758 else if (m_cap_dc1394)
760 obs = std::make_shared<CObservationImage>();
761 if (!m_cap_dc1394->getObservation(*obs))
769 else if (m_cap_swissranger)
771 obs3D = std::make_shared<CObservation3DRangeScan>();
773 bool there_is_obs, hardware_error;
774 m_cap_swissranger->getNextObservation(
775 *obs3D, there_is_obs, hardware_error);
777 if (!there_is_obs || hardware_error)
785 else if (m_cap_kinect)
787 obs3D = std::make_shared<CObservation3DRangeScan>();
792 double max_timeout = 3.0;
796 const char* envVal = getenv(
"MRPT_CCAMERA_KINECT_TIMEOUT_MS");
797 if (envVal) max_timeout = atoi(envVal) * 0.001;
799 bool there_is_obs, hardware_error;
802 m_cap_kinect->getNextObservation(
803 *obs3D, there_is_obs, hardware_error);
804 if (!there_is_obs) std::this_thread::sleep_for(1ms);
808 if (!there_is_obs || hardware_error)
816 else if (m_cap_openni2)
818 obs3D = std::make_shared<CObservation3DRangeScan>();
822 double max_timeout = 3.0;
823 bool there_is_obs, hardware_error;
826 m_cap_openni2->getNextObservation(
827 *obs3D, there_is_obs, hardware_error);
828 if (!there_is_obs) std::this_thread::sleep_for(1ms);
832 if (!there_is_obs || hardware_error)
840 else if (m_cap_bumblebee_dc1394)
842 stObs = std::make_shared<CObservationStereoImages>();
843 if (!m_cap_bumblebee_dc1394->getStereoObservation(*stObs))
855 stObs = std::make_shared<CObservationStereoImages>();
857 if (!m_cap_svs->getStereoObservation(*stObs))
866 else if (m_cap_ffmpeg)
868 static bool anyGood =
false;
870 if (!m_cap_ffmpeg->retrieveFrame(im))
881 "ffmpeg capture driver: Failed to get frame (temporary "
888 obs = std::make_shared<CObservationImage>();
889 obs->image = std::move(im);
895 else if (m_cap_image_dir)
897 if (m_img_dir_counter > m_img_dir_end_index)
903 std::string auxL =
format(
904 "%s/%s", m_img_dir_url.c_str(), m_img_dir_left_format.c_str());
905 if (m_img_dir_is_stereo)
907 stObs = std::make_shared<CObservationStereoImages>();
908 if (!stObs->imageLeft.loadFromFile(
909 format(auxL.c_str(), m_img_dir_counter)))
914 std::string auxR =
format(
915 "%s/%s", m_img_dir_url.c_str(), m_img_dir_right_format.c_str());
916 if (!stObs->imageRight.loadFromFile(
917 format(auxR.c_str(), m_img_dir_counter++)))
928 obs = std::make_shared<CObservationImage>();
929 if (!obs->image.loadFromFile(
930 format(auxL.c_str(), m_img_dir_counter++)))
939 else if (m_cap_rawlog)
944 while (!obs && !stObs && !obs3D)
950 std::dynamic_pointer_cast<CObservation>(newObs);
951 if (!m_rawlog_camera_sensor_label.empty() &&
952 m_rawlog_camera_sensor_label != o->sensorLabel)
956 obs = std::dynamic_pointer_cast<CObservationImage>(o);
959 std::dynamic_pointer_cast<CObservationStereoImages>(o);
962 std::dynamic_pointer_cast<CObservation3DRangeScan>(o);
967 std::dynamic_pointer_cast<CSensoryFrame>(newObs);
971 if (!m_rawlog_camera_sensor_label.empty() &&
972 m_rawlog_camera_sensor_label != o->sensorLabel)
977 obs = std::dynamic_pointer_cast<CObservationImage>(o);
983 std::dynamic_pointer_cast<CObservationStereoImages>(
990 std::dynamic_pointer_cast<CObservation3DRangeScan>(
996 if (obs || stObs || obs3D)
1000 const std::string old_dir =
1001 CImage::getImagesPathBase();
1002 CImage::setImagesPathBase(m_rawlog_detected_images_dir);
1004 if (obs && obs->image.isExternallyStored())
1005 obs->image.loadFromFile(
1006 obs->image.getExternalStorageFileAbsolutePath());
1008 if (obs3D && obs3D->hasIntensityImage &&
1009 obs3D->intensityImage.isExternallyStored())
1010 obs3D->intensityImage.loadFromFile(
1011 obs3D->intensityImage
1012 .getExternalStorageFileAbsolutePath());
1014 if (stObs && stObs->imageLeft.isExternallyStored())
1015 stObs->imageLeft.loadFromFile(
1016 stObs->imageLeft.getExternalStorageFileAbsolutePath());
1018 if (stObs && stObs->hasImageRight &&
1019 stObs->imageRight.isExternallyStored())
1020 stObs->imageRight.loadFromFile(
1021 stObs->imageRight.getExternalStorageFileAbsolutePath());
1023 if (stObs && stObs->hasImageDisparity &&
1024 stObs->imageDisparity.isExternallyStored())
1025 stObs->imageDisparity.loadFromFile(
1026 stObs->imageDisparity
1027 .getExternalStorageFileAbsolutePath());
1029 CImage::setImagesPathBase(old_dir);
1036 else if (m_cap_flycap)
1039 if (!m_cap_flycap->isStereo())
1041 obs = std::make_shared<CObservationImage>();
1042 ok = m_cap_flycap->getObservation(*obs);
1046 stObs = std::make_shared<CObservationStereoImages>();
1047 ok = m_cap_flycap->getObservation(*stObs);
1058 else if (m_cap_flycap_stereo_l && m_cap_flycap_stereo_r)
1060 stObs = std::make_shared<CObservationStereoImages>();
1064 bool ok1, ok2 =
false;
1066 ok1 = m_cap_flycap_stereo_r->getObservation(obsL);
1067 if (ok1) ok2 = m_cap_flycap_stereo_l->getObservation(obsR);
1080 if (std::abs(At) > 0.1)
1082 cout <<
"[CCamera, flycap_stereo] Warning: Too large delay "
1083 "between left & right images: "
1092 stObs->imageLeft = std::move(obsL.
image);
1093 stObs->imageRight = std::move(obsR.
image);
1097 else if (m_cap_duo3d)
1099 stObs = std::make_shared<CObservationStereoImages>();
1100 obsIMU = std::make_shared<CObservationIMU>();
1102 bool thereIsIMG, thereIsIMU;
1103 m_cap_duo3d->getObservations(*stObs, *obsIMU, thereIsIMG, thereIsIMU);
1109 else if (m_cap_duo3d->captureIMUIsSet() && !thereIsIMU)
1111 cout <<
"[CCamera, duo3d] Warning: There are no IMU data from the "
1112 "device. Only images are being grabbed.";
1118 obs3D = std::make_shared<CObservation3DRangeScan>();
1120 bool thereIsObs = m_myntd->getObservation(*obs3D);
1121 static int noObsCnt = 0;
1126 if (noObsCnt++ > 100)
1130 "Error getting observations from MYNTEYE-D camera.");
1142 "There is no initialized camera driver: has 'initialize()' been "
1149 m_camera_grab_decimator_counter++;
1150 if (m_camera_grab_decimator_counter < m_camera_grab_decimator)
1157 m_camera_grab_decimator_counter = 0;
1165 obs->sensorLabel = m_sensorLabel;
1166 obs->setSensorPose(m_sensorPose);
1170 stObs->sensorLabel = (m_cap_duo3d && m_cap_duo3d->captureIMUIsSet())
1171 ? m_sensorLabel +
"_IMG"
1173 stObs->setSensorPose(m_sensorPose);
1177 obs3D->sensorLabel = m_sensorLabel;
1178 obs3D->setSensorPose(m_sensorPose);
1182 obsIMU->sensorLabel = m_sensorLabel +
"_IMU";
1183 obsIMU->setSensorPose(m_sensorPose);
1187 if (m_capture_grayscale)
1191 if (obs->image.isColor()) obs->image = obs->image.grayscale();
1195 if (stObs->imageLeft.isColor())
1196 stObs->imageLeft = stObs->imageLeft.grayscale();
1197 if (stObs->hasImageRight && stObs->imageRight.isColor())
1198 stObs->imageRight = stObs->imageRight.grayscale();
1199 if (stObs->hasImageDisparity && stObs->imageDisparity.isColor())
1200 stObs->imageDisparity = stObs->imageDisparity.grayscale();
1204 if (obs3D->hasIntensityImage && obs3D->intensityImage.isColor())
1205 obs3D->intensityImage = obs3D->intensityImage.grayscale();
1210 CImage img4gui, img4guiR;
1211 if (m_preview_win1 && m_preview_win1->isOpen())
1227 bool delayed_insertion_in_obs_queue =
false;
1228 if (!m_path_for_external_images.empty())
1232 if (m_external_images_own_thread)
1234 m_csToSaveList.lock();
1238 for (
size_t i = 0; i < m_toSaveList.size(); ++i)
1239 if (m_toSaveList[i].
size() < m_toSaveList[idx_min].size())
1242 m_toSaveList[idx_min].insert(
1245 m_csToSaveList.unlock();
1247 delayed_insertion_in_obs_queue =
true;
1251 const string filNameL =
1255 m_external_images_format.c_str());
1256 const string filNameR =
1260 m_external_images_format.c_str());
1261 const string filNameD =
1265 m_external_images_format.c_str());
1267 stObs->imageLeft.saveToFile(
1268 m_path_for_external_images +
string(
"/") + filNameL,
1269 m_external_images_jpeg_quality);
1270 stObs->imageLeft.setExternalStorage(filNameL);
1272 if (stObs->hasImageRight)
1274 stObs->imageRight.saveToFile(
1275 m_path_for_external_images +
string(
"/") + filNameR,
1276 m_external_images_jpeg_quality);
1277 stObs->imageRight.setExternalStorage(filNameR);
1279 if (stObs->hasImageDisparity)
1281 stObs->imageDisparity.saveToFile(
1282 m_path_for_external_images +
string(
"/") + filNameD,
1283 m_external_images_jpeg_quality);
1284 stObs->imageDisparity.setExternalStorage(filNameD);
1290 if (m_external_images_own_thread)
1292 m_csToSaveList.lock();
1296 for (
size_t i = 0; i < m_toSaveList.size(); ++i)
1297 if (m_toSaveList[i].
size() < m_toSaveList[idx_min].
size())
1301 m_toSaveList[idx_min].insert(
TListObsPair(obs->timestamp, obs));
1303 m_csToSaveList.unlock();
1304 delayed_insertion_in_obs_queue =
true;
1312 m_external_images_format.c_str());
1314 obs->image.saveToFile(
1315 m_path_for_external_images +
string(
"/") + filName,
1316 m_external_images_jpeg_quality);
1317 obs->image.setExternalStorage(filName);
1323 if (m_preview_decimation > 0)
1325 if (++m_preview_counter > m_preview_decimation)
1327 m_preview_counter = 0;
1330 if (!m_preview_win1)
1332 string caption = string(
"Preview of ") + m_sensorLabel;
1333 if (stObs) caption +=
"-LEFT";
1334 if (m_preview_decimation > 1)
1336 format(
" (decimation: %i)", m_preview_decimation);
1339 if (stObs && !m_preview_win2)
1341 string caption = string(
"Preview of ") + m_sensorLabel;
1342 if (stObs) caption +=
"-RIGHT";
1343 if (m_preview_decimation > 1)
1345 format(
" (decimation: %i)", m_preview_decimation);
1349 if (m_preview_win1->isOpen() && img4gui.
getWidth() > 0)
1352 if (m_preview_reduction >= 2)
1354 unsigned int w = img4gui.
getWidth();
1358 auxImg, w / m_preview_reduction,
1360 m_preview_win1->showImage(auxImg);
1363 m_preview_win1->showImage(img4gui);
1367 if (m_preview_win2 && m_preview_win2->isOpen() && stObs &&
1368 stObs->hasImageRight && img4gui.
getWidth() > 0)
1371 if (m_preview_reduction >= 2)
1373 unsigned int w = img4guiR.
getWidth();
1377 auxImg, w / m_preview_reduction,
1379 m_preview_win2->showImage(auxImg);
1382 m_preview_win2->showImage(img4guiR);
1386 if (m_preview_win2 && m_preview_win2->isOpen() && stObs &&
1387 stObs->hasImageDisparity)
1390 if (m_preview_reduction >= 2)
1392 unsigned int w = stObs->imageDisparity.getWidth();
1393 unsigned int h = stObs->imageDisparity.getHeight();
1396 auxImg, w / m_preview_reduction,
1398 m_preview_win2->showImage(auxImg);
1401 m_preview_win2->showImage(stObs->imageDisparity);
1406 if (delayed_insertion_in_obs_queue)
1408 if (m_cap_duo3d && m_cap_duo3d->captureIMUIsSet() && obsIMU)
1425 vector<CSerializable::Ptr> out_obs;
1426 getNextFrame(out_obs);
1427 appendObservations(out_obs);
1437 if (!m_cap_dc1394->setSoftwareTriggerLevel(level))
1446 "Software trigger is not implemented for this camera type");
1458 "Cannot create the directory for externally saved images: `%s`",
1461 m_path_for_external_images = directory;
1469 #if MRPT_HAS_WXWIDGETS
1473 std::cerr <<
"[mrpt::hwdrivers::prepareVideoSourceFromUserSelection] "
1474 "Error initiating Wx subsystem."
1479 std::promise<void> semDlg;
1480 std::promise<mrpt::gui::detail::TReturnAskUserOpenCamera> dlgSelection;
1485 REQ->sourceCameraSelectDialog =
true;
1486 REQ->voidPtr =
reinterpret_cast<void*
>(&semDlg);
1487 REQ->voidPtr2 =
reinterpret_cast<void*
>(&dlgSelection);
1488 WxSubsystem::pushPendingWxRequest(REQ);
1491 if (!WxSubsystem::isConsoleApp())
1493 std::this_thread::sleep_for(
1495 wxApp::GetInstance()->Yield(
true);
1507 const char* envVal = getenv(
"MRPT_WXSUBSYS_TIMEOUT_MS");
1508 if (envVal) maxTimeout = atoi(envVal);
1510 if (semDlg.get_future().wait_for(std::chrono::milliseconds(maxTimeout)) ==
1511 std::future_status::timeout)
1513 cerr <<
"[prepareVideoSourceFromUserSelection] Timeout waiting window "
1520 auto future = dlgSelection.get_future();
1522 const auto& ret = future.get();
1530 cam->loadConfig(selectedConfig,
"CONFIG");
1536 #endif // MRPT_HAS_WXWIDGETS
1544 #if MRPT_HAS_WXWIDGETS
1553 video->loadConfig(cfg,
"CONFIG");
1556 video->initialize();
1560 catch (
const std::exception& e)
1562 cerr << endl << e.what() << endl;
1563 wxMessageBox(_(
"Couldn't open video source"), _(
"Error"));
1568 #endif // MRPT_HAS_WXWIDGETS
1578 #if MRPT_HAS_WXWIDGETS
1582 panel,
"panel must be of type mrpt::gui::CPanelCameraSelection *");
1583 panel->writeConfigFromVideoSourcePanel(
sect, cfg);
1587 #endif // MRPT_HAS_WXWIDGETS
1595 void* _panel,
const std::string&
sect,
1599 #if MRPT_HAS_WXWIDGETS
1603 panel,
"panel must be of type mrpt::gui::CPanelCameraSelection *");
1605 panel->readConfigIntoVideoSourcePanel(
sect, cfg);
1609 #endif // MRPT_HAS_WXWIDGETS
1618 while (!m_threadImagesSaverShouldEnd)
1623 m_csToSaveList.lock();
1624 m_toSaveList[my_working_thread_index].swap(newObs);
1625 m_csToSaveList.unlock();
1627 for (
auto i = newObs.begin(); i != newObs.end(); ++i)
1630 if (m_hook_pre_save)
1635 std::dynamic_pointer_cast<mrpt::obs::CObservation>(
1637 m_hook_pre_save(obs, m_hook_pre_save_param);
1644 std::dynamic_pointer_cast<CObservationImage>(i->second);
1650 m_external_images_format.c_str());
1652 obs->image.saveToFile(
1653 m_path_for_external_images +
string(
"/") + filName,
1654 m_external_images_jpeg_quality);
1655 obs->image.setExternalStorage(filName);
1660 std::dynamic_pointer_cast<CObservationStereoImages>(
1663 const string filNameL =
1667 m_external_images_format.c_str());
1668 const string filNameR =
1672 m_external_images_format.c_str());
1673 const string filNameD =
1677 m_external_images_format.c_str());
1679 stObs->imageLeft.saveToFile(
1680 m_path_for_external_images +
string(
"/") + filNameL,
1681 m_external_images_jpeg_quality);
1682 stObs->imageLeft.setExternalStorage(filNameL);
1684 if (stObs->hasImageRight)
1686 stObs->imageRight.saveToFile(
1687 m_path_for_external_images +
string(
"/") + filNameR,
1688 m_external_images_jpeg_quality);
1689 stObs->imageRight.setExternalStorage(filNameR);
1691 if (stObs->hasImageDisparity)
1693 stObs->imageDisparity.saveToFile(
1694 m_path_for_external_images +
string(
"/") + filNameD,
1695 m_external_images_jpeg_quality);
1696 stObs->imageDisparity.setExternalStorage(filNameD);
1701 appendObservation(i->second);
1704 std::this_thread::sleep_for(2ms);