43 m_grabber_type("opencv"),
44 m_capture_grayscale(false),
49 m_dc1394_camera_guid(0),
50 m_dc1394_camera_unit(0),
52 m_preview_decimation(0),
53 m_preview_reduction(1),
55 m_bumblebee_dc1394_camera_guid(0),
56 m_bumblebee_dc1394_camera_unit(0),
57 m_bumblebee_dc1394_framerate(15),
59 m_svs_camera_index(0),
62 m_sr_open_from_usb(true),
64 m_sr_save_range_img(true),
65 m_sr_save_intensity_img(true),
66 m_sr_save_confidence(true),
70 m_kinect_save_range_img(true),
71 m_kinect_save_intensity_img(true),
72 m_kinect_video_rgb(true),
74 m_fcs_start_synch_capture(false),
77 m_img_dir_left_format("imL_%04d.jpg"),
78 m_img_dir_right_format("imR_%04d.jpg"),
79 m_img_dir_start_index(0),
80 m_img_dir_end_index(100),
81 m_img_dir_is_stereo(true),
84 m_external_images_own_thread(false),
85 m_camera_grab_decimator(0),
86 m_camera_grab_decimator_counter(0),
88 m_external_image_saver_count(
std::thread::hardware_concurrency()),
89 m_threadImagesSaverShouldEnd(false),
90 m_hook_pre_save(
nullptr),
91 m_hook_pre_save_param(
nullptr)
93 m_sensorLabel =
"CAMERA";
102 cout <<
"[CCameraSensor::initialize] Opening camera..." << endl;
109 if (m_grabber_type ==
"opencv")
113 "CAMERA_CV_AUTODETECT,CAMERA_CV_DC1394,CAMERA_CV_VFL,CAMERA_CV_VFW," 115 "CAMERA_CV_AUTODETECT");
122 "Invalid value of :'%s'", m_cv_camera_type.c_str())
126 "[CCameraSensor::initialize] opencv camera, index: %i type: " 128 int(m_cv_camera_index), idx);
131 m_cv_camera_index,
TCameraType(idx), m_cv_options));
133 if (!m_cap_cv->isOpen())
137 "[CCameraSensor::initialize] ERROR: Couldn't open OpenCV " 141 else if (m_grabber_type ==
"dc1394")
145 "[CCameraSensor::initialize] dc1394 camera, GUID: 0x%lX " 147 long(m_dc1394_camera_guid), m_dc1394_camera_unit);
150 m_dc1394_camera_guid, m_dc1394_camera_unit, m_dc1394_options,
153 if (!m_cap_dc1394->isOpen())
157 "[CCameraSensor::initialize] ERROR: Couldn't open dc1394 " 161 else if (m_grabber_type ==
"bumblebee_dc1394")
164 "[CCameraSensor::initialize] bumblebee_libdc1394 camera: " 165 "GUID:0x%08X Index:%i FPS:%f...\n",
166 (
unsigned int)(m_bumblebee_dc1394_camera_guid),
167 m_bumblebee_dc1394_camera_unit, m_bumblebee_dc1394_framerate);
168 m_cap_bumblebee_dc1394.reset(
170 m_bumblebee_dc1394_camera_guid, m_bumblebee_dc1394_camera_unit,
171 m_bumblebee_dc1394_framerate));
173 else if (m_grabber_type ==
"svs")
176 "[CCameraSensor::initialize] SVS camera: %u...\n",
177 (
unsigned int)(m_svs_camera_index));
181 else if (m_grabber_type ==
"ffmpeg")
185 "[CCameraSensor::initialize] FFmpeg stream: %s...\n",
186 m_ffmpeg_url.c_str());
189 if (!m_cap_ffmpeg->openURL(m_ffmpeg_url, m_capture_grayscale))
193 "Error opening FFmpeg stream: %s", m_ffmpeg_url.c_str())
196 else if (m_grabber_type ==
"swissranger")
198 cout <<
"[CCameraSensor::initialize] SwissRanger camera...\n";
201 m_cap_swissranger->setOpenFromUSB(m_sr_open_from_usb);
202 m_cap_swissranger->setOpenIPAddress(m_sr_ip_address);
204 m_cap_swissranger->setSave3D(m_sr_save_3d);
205 m_cap_swissranger->setSaveRangeImage(m_sr_save_range_img);
206 m_cap_swissranger->setSaveIntensityImage(m_sr_save_intensity_img);
207 m_cap_swissranger->setSaveConfidenceImage(m_sr_save_confidence);
209 if (!m_path_for_external_images.empty())
210 m_cap_swissranger->setPathForExternalImages(
211 m_path_for_external_images);
219 catch (std::exception&)
225 else if (m_grabber_type ==
"kinect")
227 cout <<
"[CCameraSensor::initialize] Kinect camera...\n";
228 m_cap_kinect.reset(
new CKinect());
229 m_cap_kinect->enableGrab3DPoints(m_kinect_save_3d);
230 m_cap_kinect->enableGrabDepth(m_kinect_save_range_img);
231 m_cap_kinect->enableGrabRGB(m_kinect_save_intensity_img);
232 m_cap_kinect->setVideoChannel(
236 if (!m_path_for_external_images.empty())
237 m_cap_kinect->setPathForExternalImages(m_path_for_external_images);
245 catch (std::exception&)
251 else if (m_grabber_type ==
"openni2")
253 cout <<
"[CCameraSensor::initialize] OpenNI2 sensor...\n";
255 m_cap_openni2->enableGrab3DPoints(m_kinect_save_3d);
259 m_cap_openni2->enableGrabDepth(m_kinect_save_range_img);
260 m_cap_openni2->enableGrabRGB(m_kinect_save_intensity_img);
262 if (!m_path_for_external_images.empty())
263 m_cap_openni2->setPathForExternalImages(m_path_for_external_images);
271 catch (std::exception& e)
277 else if (m_grabber_type ==
"image_dir")
281 "[CCameraSensor::initialize] Image dir: %s...\n",
282 m_img_dir_url.c_str());
285 else if (m_grabber_type ==
"rawlog")
289 "[CCameraSensor::initialize] Rawlog stream: %s...\n",
290 m_rawlog_file.c_str());
293 if (!m_cap_rawlog->open(m_rawlog_file))
297 "Error opening rawlog file: %s", m_rawlog_file.c_str())
301 m_rawlog_detected_images_dir =
302 CRawlog::detectImagesDirectory(m_rawlog_file);
304 else if (m_grabber_type ==
"flycap")
306 cout <<
"[CCameraSensor::initialize] PGR FlyCapture2 camera...\n";
312 catch (std::exception&)
318 else if (m_grabber_type ==
"flycap_stereo")
321 <<
"[CCameraSensor::initialize] PGR FlyCapture2 stereo camera...\n";
328 cout <<
"[CCameraSensor::initialize] PGR FlyCapture2 stereo " 329 "camera: Opening LEFT camera...\n";
330 m_cap_flycap_stereo_l->open(
331 m_flycap_stereo_options[0],
false );
333 cout <<
"[CCameraSensor::initialize] PGR FlyCapture2 stereo " 334 "camera: Opening RIGHT camera...\n";
335 m_cap_flycap_stereo_r->open(
336 m_flycap_stereo_options[1],
false );
339 if (m_fcs_start_synch_capture)
342 m_cap_flycap_stereo_l.get(), m_cap_flycap_stereo_r.get()};
347 m_cap_flycap_stereo_l->startCapture();
348 m_cap_flycap_stereo_r->startCapture();
351 catch (std::exception&)
357 else if (m_grabber_type ==
"duo3d")
360 cout <<
format(
"[CCameraSensor::initialize] DUO3D stereo camera ...\n");
367 catch (std::exception& e)
375 "Unknown 'grabber_type' found: %s", m_grabber_type.c_str())
378 cout <<
"[CCameraSensor::initialize] Done!" << endl;
382 if (m_external_images_own_thread)
384 m_threadImagesSaverShouldEnd =
false;
386 m_threadImagesSaver.clear();
387 m_threadImagesSaver.resize(m_external_image_saver_count);
389 m_toSaveList.clear();
390 m_toSaveList.resize(m_external_image_saver_count);
392 for (
unsigned int i = 0; i < m_external_image_saver_count; ++i)
393 m_threadImagesSaver[i] =
404 m_cap_dc1394.reset();
405 m_cap_flycap.reset();
406 m_cap_flycap_stereo_l.reset();
407 m_cap_flycap_stereo_r.reset();
408 m_cap_bumblebee_dc1394.reset();
409 m_cap_ffmpeg.reset();
410 m_cap_rawlog.reset();
411 m_cap_swissranger.reset();
412 m_cap_kinect.reset();
414 m_cap_image_dir.reset();
420 if (!m_threadImagesSaver.empty())
422 m_threadImagesSaverShouldEnd =
true;
423 for (
size_t i = 0; i < m_threadImagesSaver.size(); i++)
424 m_threadImagesSaver[i].join();
440 if (m_grab_decimation > 0)
442 m_camera_grab_decimator = m_grab_decimation;
443 m_camera_grab_decimator_counter = 0;
445 m_grab_decimation = 0;
448 m_camera_grab_decimator = m_camera_grab_decimator_counter = 0;
451 iniSection,
"grabber_type", m_grabber_type);
453 preview_decimation,
int, m_preview_decimation, configSource, iniSection)
455 preview_reduction,
int, m_preview_reduction, configSource, iniSection)
459 iniSection,
"cv_camera_type", m_cv_camera_type);
461 configSource.
read_int(iniSection,
"cv_camera_index", m_cv_camera_index);
463 m_cv_options.frame_width = configSource.
read_int(
464 iniSection,
"cv_frame_width", m_cv_options.frame_width);
465 m_cv_options.frame_height = configSource.
read_int(
466 iniSection,
"cv_frame_height", m_cv_options.frame_height);
468 configSource.
read_double(iniSection,
"cv_gain", m_cv_options.gain);
469 m_cv_options.ieee1394_fps = configSource.
read_double(
470 iniSection,
"cv_fps", m_cv_options.ieee1394_fps);
472 m_capture_grayscale =
473 configSource.
read_bool(iniSection,
"capture_grayscale",
false);
475 m_cv_options.ieee1394_grayscale = m_capture_grayscale;
479 dc1394_camera_guid,
uint64_t, m_dc1394_camera_guid, configSource,
482 dc1394_camera_unit,
int, m_dc1394_camera_unit, configSource, iniSection)
485 dc1394_frame_width,
int, m_dc1394_options.frame_width, configSource,
488 dc1394_frame_height,
int, m_dc1394_options.frame_height, configSource,
492 dc1394_mode7,
int, m_dc1394_options.mode7, configSource, iniSection)
495 dc1394_shutter,
int, m_dc1394_options.shutter, configSource, iniSection)
497 dc1394_gain,
int, m_dc1394_options.gain, configSource, iniSection)
499 dc1394_gamma,
int, m_dc1394_options.gamma, configSource, iniSection)
501 dc1394_brightness,
int, m_dc1394_options.brightness, configSource,
504 dc1394_exposure,
int, m_dc1394_options.exposure, configSource,
507 dc1394_sharpness,
int, m_dc1394_options.sharpness, configSource,
510 dc1394_white_balance,
int, m_dc1394_options.white_balance, configSource,
514 dc1394_shutter_mode,
int, m_dc1394_options.shutter_mode, configSource,
517 dc1394_gain_mode,
int, m_dc1394_options.gain_mode, configSource,
520 dc1394_gamma_mode,
int, m_dc1394_options.gamma_mode, configSource,
523 dc1394_brightness_mode,
int, m_dc1394_options.brightness_mode,
524 configSource, iniSection)
526 dc1394_exposure_mode,
int, m_dc1394_options.exposure_mode, configSource,
529 dc1394_sharpness_mode,
int, m_dc1394_options.sharpness_mode,
530 configSource, iniSection)
532 dc1394_white_balance_mode,
int, m_dc1394_options.white_balance_mode,
533 configSource, iniSection)
536 dc1394_trigger_power,
int, m_dc1394_options.trigger_power, configSource,
539 dc1394_trigger_mode,
int, m_dc1394_options.trigger_mode, configSource,
542 dc1394_trigger_source,
int, m_dc1394_options.trigger_source,
543 configSource, iniSection)
545 dc1394_trigger_polarity,
int, m_dc1394_options.trigger_polarity,
546 configSource, iniSection)
548 dc1394_ring_buffer_size,
int, m_dc1394_options.ring_buffer_size,
549 configSource, iniSection)
553 bumblebee_dc1394_camera_guid,
uint64_t, m_bumblebee_dc1394_camera_guid,
554 configSource, iniSection)
556 bumblebee_dc1394_camera_unit,
int, m_bumblebee_dc1394_camera_unit,
557 configSource, iniSection)
559 bumblebee_dc1394_framerate,
double, m_bumblebee_dc1394_framerate,
560 configSource, iniSection)
563 m_svs_camera_index = configSource.
read_int(
564 iniSection,
"svs_camera_index", m_svs_camera_index);
565 m_svs_options.frame_width = configSource.
read_int(
566 iniSection,
"svs_frame_width", m_svs_options.frame_width);
567 m_svs_options.frame_height = configSource.
read_int(
568 iniSection,
"svs_frame_height", m_svs_options.frame_height);
569 m_svs_options.framerate = configSource.
read_double(
570 iniSection,
"svs_framerate", m_svs_options.framerate);
571 m_svs_options.m_NDisp =
572 configSource.
read_int(iniSection,
"svs_NDisp", m_svs_options.m_NDisp);
573 m_svs_options.m_Corrsize = configSource.
read_int(
574 iniSection,
"svs_Corrsize", m_svs_options.m_Corrsize);
576 configSource.
read_int(iniSection,
"svs_LR", m_svs_options.m_LR);
577 m_svs_options.m_Thresh =
578 configSource.
read_int(iniSection,
"svs_Thresh", m_svs_options.m_Thresh);
579 m_svs_options.m_Unique =
580 configSource.
read_int(iniSection,
"svs_Unique", m_svs_options.m_Unique);
581 m_svs_options.m_Horopter = configSource.
read_int(
582 iniSection,
"svs_Horopter", m_svs_options.m_Horopter);
583 m_svs_options.m_SpeckleSize = configSource.
read_int(
584 iniSection,
"svs_SpeckleSize", m_svs_options.m_SpeckleSize);
585 m_svs_options.m_procesOnChip = configSource.
read_bool(
586 iniSection,
"svs_procesOnChip", m_svs_options.m_procesOnChip);
587 m_svs_options.m_calDisparity = configSource.
read_bool(
588 iniSection,
"svs_calDisparity", m_svs_options.m_calDisparity);
592 configSource.
read_string(iniSection,
"ffmpeg_url", m_ffmpeg_url));
596 configSource.
read_string(iniSection,
"rawlog_file", m_rawlog_file));
599 iniSection,
"rawlog_camera_sensor_label",
600 m_rawlog_camera_sensor_label));
604 configSource.
read_string(iniSection,
"image_dir_url", m_img_dir_url));
607 iniSection,
"left_format", m_img_dir_left_format));
609 configSource.
read_string(iniSection,
"right_format",
""));
610 m_img_dir_start_index =
611 configSource.
read_int(iniSection,
"start_index", m_img_dir_start_index);
613 m_img_dir_end_index =
614 configSource.
read_int(iniSection,
"end_index", m_img_dir_end_index);
616 m_img_dir_is_stereo = !m_img_dir_right_format.empty();
617 m_img_dir_counter = m_img_dir_start_index;
620 m_duo3d_options.loadOptionsFrom(configSource,
"DUO3DOptions");
624 configSource.
read_bool(iniSection,
"sr_use_usb", m_sr_open_from_usb);
626 configSource.
read_string(iniSection,
"sr_IP", m_sr_ip_address);
629 configSource.
read_bool(iniSection,
"sr_grab_3d", m_sr_save_3d);
630 m_sr_save_intensity_img = configSource.
read_bool(
631 iniSection,
"sr_grab_grayscale", m_sr_save_intensity_img);
632 m_sr_save_range_img = configSource.
read_bool(
633 iniSection,
"sr_grab_range", m_sr_save_range_img);
634 m_sr_save_confidence = configSource.
read_bool(
635 iniSection,
"sr_grab_confidence", m_sr_save_confidence);
638 configSource.
read_bool(iniSection,
"kinect_grab_3d", m_kinect_save_3d);
639 m_kinect_save_intensity_img = configSource.
read_bool(
640 iniSection,
"kinect_grab_intensity", m_kinect_save_intensity_img);
641 m_kinect_save_range_img = configSource.
read_bool(
642 iniSection,
"kinect_grab_range", m_kinect_save_range_img);
643 m_kinect_video_rgb = configSource.
read_bool(
644 iniSection,
"kinect_video_rgb", m_kinect_video_rgb);
647 m_flycap_options.loadOptionsFrom(configSource, iniSection,
"flycap_");
650 m_fcs_start_synch_capture = configSource.
read_bool(
651 iniSection,
"fcs_start_synch_capture", m_fcs_start_synch_capture);
652 m_flycap_stereo_options[0].loadOptionsFrom(
653 configSource, iniSection,
"fcs_LEFT_");
654 m_flycap_stereo_options[1].loadOptionsFrom(
655 configSource, iniSection,
"fcs_RIGHT_");
658 map<double, grabber_dc1394_framerate_t> map_fps;
671 configSource.
read_double(iniSection,
"dc1394_framerate", 15.0);
672 it_fps = map_fps.find(the_fps);
673 if (it_fps == map_fps.end())
675 "ERROR: DC1394 framerate seems to be not a valid number: %f",
678 m_dc1394_options.framerate = it_fps->second;
681 map<string, grabber_dc1394_color_coding_t> map_color;
683 #define ADD_COLOR_MAP(c) map_color[#c] = c; 693 iniSection,
"dc1394_color_coding",
"COLOR_CODING_YUV422"));
694 it_color = map_color.find(the_color_coding);
695 if (it_color == map_color.end())
697 "ERROR: Color coding seems not to be valid : '%s'",
698 the_color_coding.c_str());
699 m_dc1394_options.color_coding = it_color->second;
703 iniSection,
"external_images_format", m_external_images_format));
704 m_external_images_jpeg_quality = configSource.
read_int(
705 iniSection,
"external_images_jpeg_quality",
706 m_external_images_jpeg_quality);
707 m_external_images_own_thread = configSource.
read_bool(
708 iniSection,
"external_images_own_thread", m_external_images_own_thread);
709 m_external_image_saver_count = configSource.
read_int(
710 iniSection,
"external_images_own_thread_count",
711 m_external_image_saver_count);
714 m_sensorPose.setFromValues(
715 configSource.
read_float(iniSection,
"pose_x", 0),
716 configSource.
read_float(iniSection,
"pose_y", 0),
717 configSource.
read_float(iniSection,
"pose_z", 0),
730 m_preview_win1.reset();
731 m_preview_win2.reset();
738 vector<CSerializable::Ptr> out_obs;
739 getNextFrame(out_obs);
740 return std::dynamic_pointer_cast<
CObservation>(out_obs[0]);
754 bool capture_ok =
false;
758 obs = mrpt::make_aligned_shared<CObservationImage>();
759 if (!m_cap_cv->getObservation(*obs))
767 else if (m_cap_dc1394)
769 obs = mrpt::make_aligned_shared<CObservationImage>();
770 if (!m_cap_dc1394->getObservation(*obs))
778 else if (m_cap_swissranger)
780 obs3D = mrpt::make_aligned_shared<CObservation3DRangeScan>();
782 bool there_is_obs, hardware_error;
783 m_cap_swissranger->getNextObservation(
784 *obs3D, there_is_obs, hardware_error);
786 if (!there_is_obs || hardware_error)
794 else if (m_cap_kinect)
796 obs3D = mrpt::make_aligned_shared<CObservation3DRangeScan>();
801 double max_timeout = 3.0;
805 const char* envVal = getenv(
"MRPT_CCAMERA_KINECT_TIMEOUT_MS");
806 if (envVal) max_timeout = atoi(envVal) * 0.001;
808 bool there_is_obs, hardware_error;
811 m_cap_kinect->getNextObservation(
812 *obs3D, there_is_obs, hardware_error);
813 if (!there_is_obs) std::this_thread::sleep_for(1ms);
814 }
while (!there_is_obs &&
818 if (!there_is_obs || hardware_error)
826 else if (m_cap_openni2)
828 obs3D = mrpt::make_aligned_shared<CObservation3DRangeScan>();
832 double max_timeout = 3.0;
833 bool there_is_obs, hardware_error;
836 m_cap_openni2->getNextObservation(
837 *obs3D, there_is_obs, hardware_error);
838 if (!there_is_obs) std::this_thread::sleep_for(1ms);
839 }
while (!there_is_obs &&
843 if (!there_is_obs || hardware_error)
851 else if (m_cap_bumblebee_dc1394)
853 stObs = mrpt::make_aligned_shared<CObservationStereoImages>();
854 if (!m_cap_bumblebee_dc1394->getStereoObservation(*stObs))
866 stObs = mrpt::make_aligned_shared<CObservationStereoImages>();
868 if (!m_cap_svs->getStereoObservation(*stObs))
877 else if (m_cap_ffmpeg)
879 obs = mrpt::make_aligned_shared<CObservationImage>();
881 if (!m_cap_ffmpeg->retrieveFrame(obs->image))
889 else if (m_cap_image_dir)
891 if (m_img_dir_counter > m_img_dir_end_index)
898 "%s/%s", m_img_dir_url.c_str(), m_img_dir_left_format.c_str());
899 if (m_img_dir_is_stereo)
901 stObs = mrpt::make_aligned_shared<CObservationStereoImages>();
902 if (!stObs->imageLeft.loadFromFile(
903 format(auxL.c_str(), m_img_dir_counter)))
909 "%s/%s", m_img_dir_url.c_str(), m_img_dir_right_format.c_str());
910 if (!stObs->imageRight.loadFromFile(
911 format(auxR.c_str(), m_img_dir_counter++)))
922 obs = mrpt::make_aligned_shared<CObservationImage>();
923 if (!obs->image.loadFromFile(
924 format(auxL.c_str(), m_img_dir_counter++)))
933 else if (m_cap_rawlog)
938 while (!obs && !stObs && !obs3D)
940 *m_cap_rawlog >> newObs;
945 if (!m_rawlog_camera_sensor_label.empty() &&
946 m_rawlog_camera_sensor_label != o->sensorLabel)
950 obs = std::dynamic_pointer_cast<CObservationImage>(o);
953 std::dynamic_pointer_cast<CObservationStereoImages>(o);
956 std::dynamic_pointer_cast<CObservation3DRangeScan>(o);
968 if (!m_rawlog_camera_sensor_label.empty() &&
969 m_rawlog_camera_sensor_label != o->sensorLabel)
993 if (obs || stObs || obs3D)
998 CImage::getImagesPathBase();
999 CImage::setImagesPathBase(m_rawlog_detected_images_dir);
1001 if (obs && obs->image.isExternallyStored())
1002 obs->image.loadFromFile(
1003 obs->image.getExternalStorageFileAbsolutePath());
1005 if (obs3D && obs3D->hasIntensityImage &&
1006 obs3D->intensityImage.isExternallyStored())
1007 obs3D->intensityImage.loadFromFile(
1008 obs3D->intensityImage
1009 .getExternalStorageFileAbsolutePath());
1011 if (stObs && stObs->imageLeft.isExternallyStored())
1012 stObs->imageLeft.loadFromFile(
1013 stObs->imageLeft.getExternalStorageFileAbsolutePath());
1015 if (stObs && stObs->hasImageRight &&
1016 stObs->imageRight.isExternallyStored())
1017 stObs->imageRight.loadFromFile(
1018 stObs->imageRight.getExternalStorageFileAbsolutePath());
1020 if (stObs && stObs->hasImageDisparity &&
1021 stObs->imageDisparity.isExternallyStored())
1022 stObs->imageDisparity.loadFromFile(
1023 stObs->imageDisparity
1024 .getExternalStorageFileAbsolutePath());
1026 CImage::setImagesPathBase(old_dir);
1033 else if (m_cap_flycap)
1036 if (!m_cap_flycap->isStereo())
1038 obs = mrpt::make_aligned_shared<CObservationImage>();
1039 ok = m_cap_flycap->getObservation(*obs);
1043 stObs = mrpt::make_aligned_shared<CObservationStereoImages>();
1044 ok = m_cap_flycap->getObservation(*stObs);
1055 else if (m_cap_flycap_stereo_l && m_cap_flycap_stereo_r)
1057 stObs = mrpt::make_aligned_shared<CObservationStereoImages>();
1061 bool ok1, ok2 =
false;
1063 ok1 = m_cap_flycap_stereo_r->getObservation(obsL);
1064 if (ok1) ok2 = m_cap_flycap_stereo_l->getObservation(obsR);
1077 if (std::abs(At) > 0.1)
1079 cout <<
"[CCamera, flycap_stereo] Warning: Too large delay " 1080 "between left & right images: " 1088 stObs->imageLeft.copyFastFrom(obsL.image);
1089 stObs->imageRight.copyFastFrom(obsR.image);
1093 else if (m_cap_duo3d)
1095 stObs = mrpt::make_aligned_shared<CObservationStereoImages>();
1096 obsIMU = mrpt::make_aligned_shared<CObservationIMU>();
1098 bool thereIsIMG, thereIsIMU;
1099 m_cap_duo3d->getObservations(*stObs, *obsIMU, thereIsIMG, thereIsIMU);
1105 else if (m_cap_duo3d->captureIMUIsSet() && !thereIsIMU)
1107 cout <<
"[CCamera, duo3d] Warning: There are no IMU data from the " 1108 "device. Only images are being grabbed.";
1115 "There is no initialized camera driver: has 'initialize()' been " 1122 m_camera_grab_decimator_counter++;
1123 if (m_camera_grab_decimator_counter < m_camera_grab_decimator)
1130 m_camera_grab_decimator_counter = 0;
1132 ASSERT_(obs || stObs || obs3D || obsIMU)
1136 obs->sensorLabel = m_sensorLabel;
1137 obs->setSensorPose(m_sensorPose);
1141 stObs->sensorLabel = (m_cap_duo3d && m_cap_duo3d->captureIMUIsSet())
1142 ? m_sensorLabel +
"_IMG" 1144 stObs->setSensorPose(m_sensorPose);
1148 obs3D->sensorLabel = m_sensorLabel;
1149 obs3D->setSensorPose(m_sensorPose);
1153 obsIMU->sensorLabel = m_sensorLabel +
"_IMU";
1154 obsIMU->setSensorPose(m_sensorPose);
1158 if (m_capture_grayscale)
1162 if (obs->image.isColor()) obs->image.grayscaleInPlace();
1166 if (stObs->imageLeft.isColor()) stObs->imageLeft.grayscaleInPlace();
1167 if (stObs->hasImageRight && stObs->imageRight.isColor())
1168 stObs->imageRight.grayscaleInPlace();
1169 if (stObs->hasImageDisparity && stObs->imageDisparity.isColor())
1170 stObs->imageDisparity
1171 .grayscaleInPlace();
1175 if (obs3D->hasIntensityImage && obs3D->intensityImage.isColor())
1176 obs3D->intensityImage.grayscaleInPlace();
1180 bool delayed_insertion_in_obs_queue =
1184 if (!m_path_for_external_images.empty())
1188 if (m_external_images_own_thread)
1190 m_csToSaveList.lock();
1194 for (
size_t i = 0; i < m_toSaveList.size(); ++i)
1195 if (m_toSaveList[i].
size() < m_toSaveList[idx_min].size())
1198 m_toSaveList[idx_min].insert(
1201 m_csToSaveList.unlock();
1203 delayed_insertion_in_obs_queue =
true;
1207 const string filNameL =
1211 m_external_images_format.c_str());
1212 const string filNameR =
1216 m_external_images_format.c_str());
1217 const string filNameD =
1221 m_external_images_format.c_str());
1223 stObs->imageLeft.saveToFile(
1224 m_path_for_external_images +
string(
"/") + filNameL,
1225 m_external_images_jpeg_quality);
1226 stObs->imageLeft.setExternalStorage(filNameL);
1228 if (stObs->hasImageRight)
1230 stObs->imageRight.saveToFile(
1231 m_path_for_external_images +
string(
"/") + filNameR,
1232 m_external_images_jpeg_quality);
1233 stObs->imageRight.setExternalStorage(filNameR);
1235 if (stObs->hasImageDisparity)
1237 stObs->imageDisparity.saveToFile(
1238 m_path_for_external_images +
string(
"/") + filNameD,
1239 m_external_images_jpeg_quality);
1240 stObs->imageDisparity.setExternalStorage(filNameD);
1246 if (m_external_images_own_thread)
1248 m_csToSaveList.lock();
1252 for (
size_t i = 0; i < m_toSaveList.size(); ++i)
1253 if (m_toSaveList[i].
size() < m_toSaveList[idx_min].size())
1257 m_toSaveList[idx_min].insert(
TListObsPair(obs->timestamp, obs));
1259 m_csToSaveList.unlock();
1260 delayed_insertion_in_obs_queue =
true;
1268 m_external_images_format.c_str());
1270 obs->image.saveToFile(
1271 m_path_for_external_images +
string(
"/") + filName,
1272 m_external_images_jpeg_quality);
1273 obs->image.setExternalStorage(filName);
1279 if (m_preview_decimation > 0)
1281 if (++m_preview_counter > m_preview_decimation)
1283 m_preview_counter = 0;
1286 if (!m_preview_win1)
1288 string caption =
string(
"Preview of ") + m_sensorLabel;
1289 if (stObs) caption +=
"-LEFT";
1290 if (m_preview_decimation > 1)
1292 format(
" (decimation: %i)", m_preview_decimation);
1294 mrpt::make_aligned_shared<mrpt::gui::CDisplayWindow>(
1297 if (stObs && !m_preview_win2)
1299 string caption =
string(
"Preview of ") + m_sensorLabel;
1300 if (stObs) caption +=
"-RIGHT";
1301 if (m_preview_decimation > 1)
1303 format(
" (decimation: %i)", m_preview_decimation);
1305 mrpt::make_aligned_shared<mrpt::gui::CDisplayWindow>(
1309 if (m_preview_win1->isOpen())
1313 img = &stObs->imageLeft;
1317 img = &obs3D->intensityImage;
1320 if (m_preview_reduction >= 2)
1322 unsigned int w =
img->getWidth();
1323 unsigned int h =
img->getHeight();
1326 auxImg,
w / m_preview_reduction,
1328 m_preview_win1->showImage(auxImg);
1331 m_preview_win1->showImage(*
img);
1335 if (m_preview_win2 && m_preview_win2->isOpen() && stObs &&
1336 stObs->hasImageRight)
1339 if (m_preview_reduction >= 2)
1341 unsigned int w = stObs->imageRight.getWidth();
1342 unsigned int h = stObs->imageRight.getHeight();
1345 auxImg,
w / m_preview_reduction,
1347 m_preview_win2->showImage(auxImg);
1350 m_preview_win2->showImage(stObs->imageRight);
1354 if (m_preview_win2 && m_preview_win2->isOpen() && stObs &&
1355 stObs->hasImageDisparity)
1358 if (m_preview_reduction >= 2)
1360 unsigned int w = stObs->imageDisparity.getWidth();
1361 unsigned int h = stObs->imageDisparity.getHeight();
1364 auxImg,
w / m_preview_reduction,
1366 m_preview_win2->showImage(auxImg);
1369 m_preview_win2->showImage(stObs->imageDisparity);
1374 if (delayed_insertion_in_obs_queue)
1376 if (m_cap_duo3d && m_cap_duo3d->captureIMUIsSet() && obsIMU)
1393 vector<CSerializable::Ptr> out_obs;
1394 getNextFrame(out_obs);
1395 appendObservations(out_obs);
1405 if (!m_cap_dc1394->setSoftwareTriggerLevel(
level))
1414 "Software trigger is not implemented for this camera type")
1426 "Error: Cannot create the directory for externally saved images: " 1430 m_path_for_external_images = directory;
1438 #if MRPT_HAS_WXWIDGETS 1442 std::cerr <<
"[mrpt::hwdrivers::prepareVideoSourceFromUserSelection] " 1443 "Error initiating Wx subsystem." 1448 std::promise<void> semDlg;
1449 std::promise<mrpt::gui::detail::TReturnAskUserOpenCamera> dlgSelection;
1456 REQ->
voidPtr =
reinterpret_cast<void*
>(&semDlg);
1457 REQ->
voidPtr2 =
reinterpret_cast<void*
>(&dlgSelection);
1458 WxSubsystem::pushPendingWxRequest(REQ);
1461 if (!WxSubsystem::isConsoleApp())
1463 std::this_thread::sleep_for(
1465 wxApp::GetInstance()->Yield(
true);
1477 const char* envVal = getenv(
"MRPT_WXSUBSYS_TIMEOUT_MS");
1478 if (envVal) maxTimeout = atoi(envVal);
1480 if (semDlg.get_future().wait_for(std::chrono::milliseconds(maxTimeout)) ==
1481 std::future_status::timeout)
1483 cerr <<
"[prepareVideoSourceFromUserSelection] Timeout waiting window " 1490 auto future = dlgSelection.get_future();
1497 cam->loadConfig(future.get().selectedConfig,
"CONFIG");
1503 #endif // MRPT_HAS_WXWIDGETS 1511 #if MRPT_HAS_WXWIDGETS 1520 video->loadConfig(cfg,
"CONFIG");
1523 video->initialize();
1527 catch (std::exception& e)
1529 cerr << endl << e.what() << endl;
1530 wxMessageBox(_(
"Couldn't open video source"), _(
"Error"));
1535 #endif // MRPT_HAS_WXWIDGETS 1545 #if MRPT_HAS_WXWIDGETS 1550 panel,
"panel must be of type mrpt::gui::CPanelCameraSelection *")
1552 panel->writeConfigFromVideoSourcePanel(sect, cfg);
1556 #endif // MRPT_HAS_WXWIDGETS 1568 #if MRPT_HAS_WXWIDGETS 1573 panel,
"panel must be of type mrpt::gui::CPanelCameraSelection *")
1575 panel->readConfigIntoVideoSourcePanel(sect, cfg);
1579 #endif // MRPT_HAS_WXWIDGETS 1588 while (!m_threadImagesSaverShouldEnd)
1593 m_csToSaveList.lock();
1594 m_toSaveList[my_working_thread_index].swap(newObs);
1595 m_csToSaveList.unlock();
1598 i != newObs.end(); ++i)
1601 if (m_hook_pre_save)
1608 m_hook_pre_save(obs, m_hook_pre_save_param);
1621 m_external_images_format.c_str());
1623 obs->image.saveToFile(
1624 m_path_for_external_images +
string(
"/") + filName,
1625 m_external_images_jpeg_quality);
1626 obs->image.setExternalStorage(filName);
1634 const string filNameL =
1638 m_external_images_format.c_str());
1639 const string filNameR =
1643 m_external_images_format.c_str());
1644 const string filNameD =
1648 m_external_images_format.c_str());
1650 stObs->imageLeft.saveToFile(
1651 m_path_for_external_images +
string(
"/") + filNameL,
1652 m_external_images_jpeg_quality);
1653 stObs->imageLeft.setExternalStorage(filNameL);
1655 if (stObs->hasImageRight)
1657 stObs->imageRight.saveToFile(
1658 m_path_for_external_images +
string(
"/") + filNameR,
1659 m_external_images_jpeg_quality);
1660 stObs->imageRight.setExternalStorage(filNameR);
1662 if (stObs->hasImageDisparity)
1664 stObs->imageDisparity.saveToFile(
1665 m_path_for_external_images +
string(
"/") + filNameD,
1666 m_external_images_jpeg_quality);
1667 stObs->imageDisparity.setExternalStorage(filNameD);
1672 appendObservation(i->second);
1675 std::this_thread::sleep_for(2ms);
std::shared_ptr< CObservationIMU > Ptr
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
void * voidPtr
Parameters, depending on OPCODE.
std::shared_ptr< CObservationStereoImages > Ptr
Declares a class derived from "CObservation" that encapsules an image from a camera, whose relative pose to robot is also stored.
A class for grabing stereo images from a STOC camera of Videre Design NOTE:
bool createDirectory(const std::string &dirName)
Creates a directory.
bool read_bool(const std::string §ion, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
TCameraType
These capture types are like their OpenCV equivalents.
The data structure for each inter-thread request:
This "software driver" implements the communication protocol for interfacing a DUO3D Stereo Camera...
CCameraSensor::Ptr prepareVideoSourceFromUserSelection()
Show to the user a list of possible camera drivers and creates and open the selected camera...
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
A wrapper for Point Gray Research (PGR) FlyCapture2 API for capturing images from Firewire...
void doProcess()
This method will be invoked at a minimum rate of "process_rate" (Hz)
A class for storing images as grayscale or RGB bitmaps.
#define THROW_EXCEPTION(msg)
A class for grabing "range images" from a MESA imaging SwissRanger 3D cameras (SR-2, SR-3000, SR-4k).
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
std::shared_ptr< CObservation3DRangeScan > Ptr
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Contains classes for various device interfaces.
int checkTypeIndex(const std::string &type) const
Returns the index of a given type within the list of all possible types, or -1 if the given string is...
std::string read_string(const std::string §ion, const std::string &name, const std::string &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. ...
const Scalar * const_iterator
std::multimap< mrpt::system::TTimeStamp, mrpt::utils::CSerializable::Ptr > TListObservations
int OPCODE
Valid codes are: For CDisplayWindow:
GLubyte GLubyte GLubyte GLubyte w
A class for grabing "range images", intensity images (either RGB or IR) and other information from an...
This class allows loading and storing values and vectors of different types from a configuration text...
This class implements a config file-like interface over a memory-stored string list.
void thread_save_images(unsigned int my_working_thread_index)
Thread to save images to files.
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
#define IS_DERIVED(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is an i...
A class for grabing "range images", intensity images (either RGB or IR) and other information from an...
void close()
Close the camera (if open).
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
virtual void setPathForExternalImages(const std::string &directory)
Set the path where to save off-rawlog image files (this class DOES take into account this path)...
A class for grabing images from a IEEE1394 (Firewire) camera using the libdc1394-2 library...
Grabs from a "Bumblebee" or "Bumblebee2" stereo camera using raw access to the libdc1394 library...
void scaleImage(unsigned int width, unsigned int height, TInterpolationMethod interp=IMG_INTERP_CUBIC)
Scales this image to a new size, interpolating as needed.
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...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
std::shared_ptr< CSensoryFrame > Ptr
bool sourceCameraSelectDialog
Only one of source* can be non-nullptr, indicating the class that generated the request.
std::shared_ptr< CObservation > Ptr
GLsizei const GLchar ** string
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
std::shared_ptr< CSerializable > Ptr
void loadConfig_sensorSpecific(const mrpt::utils::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
void readConfigIntoVideoSourcePanel(void *panel, const std::string &in_cfgfile_section_name, const mrpt::utils::CConfigFileBase *in_cfgfile)
Parse the given section of the given configuration file and set accordingly the controls of the wxWid...
static void startSyncCapture(int numCameras, const CImageGrabber_FlyCapture2 **cameras_array)
Starts a synchronous capture of several cameras, which must have been already opened.
unsigned __int64 uint64_t
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
virtual void initialize()
Tries to open the camera, after setting all the parameters with a call to loadConfig.
Declares a class that represents any robot's observation.
double read_double(const std::string §ion, const std::string &name, double defaultValue, bool failIfNotFound=false) const
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
std::shared_ptr< CObservationImage > Ptr
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.
A class for grabing images from a "OpenCV"-compatible camera, or from an AVI video file...
This class represents a std::string derived class which is also CSerializable.
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.
std::string trim(const std::string &str)
Removes leading and trailing spaces.
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 ...
std::deque< CObservation::Ptr >::iterator iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
GLenum GLsizei GLenum format
Classes for creating GUI windows for 2D and 3D visualization.
std::pair< mrpt::system::TTimeStamp, mrpt::utils::CSerializable::Ptr > TListObsPair
void writeConfigFromVideoSourcePanel(void *panel, const std::string &in_cfgfile_section_name, mrpt::utils::CConfigFileBase *out_cfgfile)
Parse the user options in the wxWidgets "panel" and write the configuration into the given section of...
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 ASSERTMSG_(f, __ERROR_MSG)
static bool createOneInstanceMainThread()
Thread-safe method to create one single instance of the main wxWidgets thread: it will create the thr...
double timestampTotime_t(const mrpt::system::TTimeStamp t)
Transform from TTimeStamp to standard "time_t" (actually a double number, it can contain fractions of...
CCameraSensor::Ptr prepareVideoSourceFromPanel(void *panel)
Used only from MRPT apps: Use with caution since "panel" MUST be a "mrpt::gui::CPanelCameraSelection ...
virtual ~CCameraSensor()
Destructor.