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)
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);
977 if (!m_rawlog_camera_sensor_label.empty() &&
978 m_rawlog_camera_sensor_label != o->sensorLabel)
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())
1221 img4gui = stObs->imageLeft.makeDeepCopy();
1222 img4guiR = stObs->imageRight.makeDeepCopy();
1225 img4gui = obs->image.makeDeepCopy();
1227 img4gui = obs3D->intensityImage.makeDeepCopy();
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();
1401 stObs->imageDisparity.scaleImage(
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)
1643 m_hook_pre_save(obs, m_hook_pre_save_param);
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);
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);
This class implements a config file-like interface over a memory-stored string list.
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.
TCameraType
These capture types are like their OpenCV equivalents.
std::string read_string(const std::string §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
The data structure for each inter-thread request:
CCameraSensor::Ptr prepareVideoSourceFromUserSelection()
Show to the user a list of possible camera drivers and creates and open the selected camera...
#define THROW_EXCEPTION(msg)
std::string std::string format(std::string_view fmt, ARGS &&... args)
size_t size(const MATRIXLIKE &m, const int dim)
A wrapper for Point Gray Research (PGR) FlyCapture2 API for capturing images from Firewire...
std::pair< mrpt::system::TTimeStamp, mrpt::serialization::CSerializable::Ptr > TListObsPair
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Contains classes for various device interfaces.
size_t getHeight() const override
Returns the height of the image in pixels.
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 ...
void doProcess() override
This method will be invoked at a minimum rate of "process_rate" (Hz)
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
std::string fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars='_')
Replace invalid filename chars by underscores ('_') or any other user-given char. ...
void thread_name(const std::string &name, std::thread &theThread)
Sets the name of the given thread; useful for debuggers.
~CCameraSensor() override
Destructor.
int OPCODE
Valid codes are: For CDisplayWindow:
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 §ion, 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's CStream, std::istream, std::ostream, std::stringstream.
void thread_save_images(unsigned int my_working_thread_index)
Thread to save images to files.
#define ASSERT_(f)
Defines an assertion mechanism.
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
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.
The central class for camera grabbers in MRPT, implementing the "generic sensor" interface.
std::string lowerCase(const std::string &str)
Returns an lower-case version of a string.
std::shared_ptr< CCameraSensor > Ptr
constexpr double DEG2RAD(const double x)
Degrees to radians.
Versatile class for consistent logging and management of output messages.
double timestampTotime_t(const mrpt::system::TTimeStamp t) noexcept
Transform from TTimeStamp to standard "time_t" (actually a double number, it can contain fractions of...
#define IS_DERIVED(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is an instance of the given ...
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...
static std::string & trim(std::string &s)
std::string read_string_first_word(const std::string §ion, 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...
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
A wrapper for other CConfigFileBase-based objects that prefixes a given token to every key and/or sec...
double read_double(const std::string §ion, const std::string &name, double defaultValue, bool failIfNotFound=false) const
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
static void startSyncCapture(int numCameras, const CImageGrabber_FlyCapture2 **cameras_array)
Starts a synchronous capture of several cameras, which must have been already opened.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void initialize() override
Tries to open the camera, after setting all the parameters with a call to loadConfig.
std::multimap< mrpt::system::TTimeStamp, mrpt::serialization::CSerializable::Ptr > TListObservations
Declares a class that represents any robot's observation.
bool read_bool(const std::string §ion, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
std::string upperCase(const std::string &str)
Returns a upper-case version of a string.
mrpt::obs::CObservation::Ptr getNextFrame()
Retrieves the next frame from the video source, raising an exception on any error.
void setSoftwareTriggerLevel(bool level)
Set Software trigger level value (ON or OFF) for cameras with this function available.
A panel to select the camera input from all the formats supported by MRPT.
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
std::string trim(const std::string &str)
Removes leading and trailing spaces.
Classes for creating GUI windows for 2D and 3D visualization.
static CDisplayWindow::Ptr Create(const std::string &windowCaption, unsigned int initWidth=400, unsigned int initHeight=400)
Class factory returning a smart pointer.
double timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds...
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
void setPathForExternalImages(const std::string &directory) override
Set the path where to save off-rawlog image files (this class DOES take into account this path)...
#define MRPT_LOG_THROTTLE_WARN(_PERIOD_SECONDS, _STRING)
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
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.