Debian Codegraph @

libs/hwdrivers/src/CCameraSensor.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 //
12 #include <mrpt/config/CConfigFile.h>
13 #include <mrpt/config/CConfigFileMemory.h>
14 #include <mrpt/config/CConfigFilePrefixer.h>
15 #include <mrpt/gui/WxSubsystem.h>
16 #include <mrpt/gui/WxUtils.h>
17 #include <mrpt/hwdrivers/CCameraSensor.h>
18 #include <mrpt/obs/CObservationImage.h>
19 #include <mrpt/obs/CObservationStereoImages.h>
20 #include <mrpt/obs/CRawlog.h>
21 #include <mrpt/obs/CSensoryFrame.h>
22 #include <mrpt/serialization/CArchive.h>
23 #include <mrpt/system/filesystem.h>
24 #include <mrpt/system/os.h>
25 #include <mrpt/system/string_utils.h>
26 #include <mrpt/system/thread_name.h>
27
28 #include <chrono>
29 #include <memory>
30 #include <thread>
31
32 using namespace mrpt;
33 using namespace mrpt::hwdrivers;
34 using namespace mrpt::gui;
35 using namespace mrpt::obs;
36 using namespace mrpt::config;
37 using namespace mrpt::system;
38 using namespace mrpt::io;
39 using namespace mrpt::serialization;
40 using namespace mrpt::img;
41 using namespace std;
42 using namespace std::literals;
43
44 IMPLEMENTS_GENERIC_SENSOR(CCameraSensor, mrpt::hwdrivers)
45
46 /* -----------------------------------------------------
47 Constructor
48 ----------------------------------------------------- */
49 CCameraSensor::CCameraSensor() :
50 mrpt::system::COutputLogger("CCameraSensor"),
51 m_sensorPose(),
52 m_grabber_type("opencv"),
53
54 m_cv_camera_type("CAMERA_CV_AUTODETECT"),
55 m_cv_options(),
56
57 m_dc1394_options(),
58
59 m_svs_options(),
60
61 // ---
62 m_img_dir_url(""),
63 m_img_dir_left_format("imL_%04d.jpg"),
64 m_img_dir_right_format("imR_%04d.jpg"),
65
66 m_external_image_saver_count(std::thread::hardware_concurrency()),
67
68 m_hook_pre_save(nullptr)
69
70 {
71 m_sensorLabel = "CAMERA";
72 m_state = CGenericSensor::ssInitializing;
73 }
74
75 /* -----------------------------------------------------
76 initialize
77 ----------------------------------------------------- */
78 void CCameraSensor::initialize()
79 {
80 cout << "[CCameraSensor::initialize] Opening camera..." << endl;
81 close();
82
83 // Select type of device
84 m_grabber_type = trim(lowerCase(m_grabber_type));
85 m_cv_camera_type = trim(upperCase(m_cv_camera_type));
86
87 if (m_grabber_type == "opencv")
88 {
89 // OpenCV driver:
90 TCameraType ct;
91 try
92 {
93 ct = mrpt::typemeta::TEnumType<TCameraType>::name2value(m_cv_camera_type);
94 }
95 catch (std::exception&)
96 {
97 m_state = CGenericSensor::ssError;
98 throw;
99 }
100 cout << mrpt::format(
101 "[CCameraSensor::initialize] opencv camera, index: %i type: "
102 "%i...\n",
103 int(m_cv_camera_index), (int)ct);
104 m_cap_cv = std::make_unique<CImageGrabber_OpenCV>(m_cv_camera_index, ct, m_cv_options);
105
106 if (!m_cap_cv->isOpen())
107 {
108 m_state = CGenericSensor::ssError;
109 THROW_EXCEPTION(
110 "[CCameraSensor::initialize] ERROR: Couldn't open OpenCV "
111 "camera.");
112 }
113 }
114 else if (m_grabber_type == "dc1394")
115 {
116 // m_cap_dc1394
117 cout << mrpt::format(
118 "[CCameraSensor::initialize] dc1394 camera, GUID: 0x%lX "
119 "UNIT:%d...\n",
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, true /* verbose */);
123
124 if (!m_cap_dc1394->isOpen())
125 {
126 m_state = CGenericSensor::ssError;
127 THROW_EXCEPTION(
128 "[CCameraSensor::initialize] ERROR: Couldn't open dc1394 "
129 "camera.");
130 }
131 }
132 else if (m_grabber_type == "bumblebee_dc1394")
133 {
134 cout << mrpt::format(
135 "[CCameraSensor::initialize] bumblebee_libdc1394 camera: "
136 "GUID:0x%08X Index:%i FPS:%f...\n",
137 (unsigned int)(m_bumblebee_dc1394_camera_guid), m_bumblebee_dc1394_camera_unit,
138 m_bumblebee_dc1394_framerate);
139 m_cap_bumblebee_dc1394 = std::make_unique<CStereoGrabber_Bumblebee_libdc1394>(
140 m_bumblebee_dc1394_camera_guid, m_bumblebee_dc1394_camera_unit,
141 m_bumblebee_dc1394_framerate);
142 }
143 else if (m_grabber_type == "svs")
144 {
145 cout << mrpt::format(
146 "[CCameraSensor::initialize] SVS camera: %u...\n", (unsigned int)(m_svs_camera_index));
147 m_cap_svs = std::make_unique<CStereoGrabber_SVS>(m_svs_camera_index, m_svs_options);
148 }
149 else if (m_grabber_type == "ffmpeg")
150 {
151 // m_cap_ffmpeg
152 cout << mrpt::format(
153 "[CCameraSensor::initialize] FFmpeg stream: %s...\n", m_ffmpeg_url.c_str());
154 m_cap_ffmpeg = std::make_unique<CFFMPEG_InputStream>();
155
156 if (!m_cap_ffmpeg->openURL(m_ffmpeg_url, m_capture_grayscale))
157 {
158 m_state = CGenericSensor::ssError;
159 THROW_EXCEPTION_FMT("Error opening FFmpeg stream: %s", m_ffmpeg_url.c_str());
160 }
161 }
162 else if (m_grabber_type == "swissranger")
163 {
164 cout << "[CCameraSensor::initialize] SwissRanger camera...\n";
165 m_cap_swissranger = std::make_unique<CSwissRanger3DCamera>();
166
167 m_cap_swissranger->setOpenFromUSB(m_sr_open_from_usb);
168 m_cap_swissranger->setOpenIPAddress(m_sr_ip_address);
169
170 m_cap_swissranger->setSave3D(m_sr_save_3d);
171 m_cap_swissranger->setSaveRangeImage(m_sr_save_range_img);
172 m_cap_swissranger->setSaveIntensityImage(m_sr_save_intensity_img);
173 m_cap_swissranger->setSaveConfidenceImage(m_sr_save_confidence);
174
175 if (!m_path_for_external_images.empty())
176 m_cap_swissranger->setPathForExternalImages(m_path_for_external_images);
177
178 // Open it:
179 try
180 {
181 m_cap_swissranger->initialize(); // This will launch an exception if needed.
182 }
183 catch (std::exception&)
184 {
185 m_state = CGenericSensor::ssError;
186 throw;
187 }
188 }
189 else if (m_grabber_type == "kinect")
190 {
191 cout << "[CCameraSensor::initialize] Kinect camera...\n";
192 m_cap_kinect = std::make_unique<CKinect>();
193 m_cap_kinect->enableGrab3DPoints(m_kinect_save_3d);
194 m_cap_kinect->enableGrabDepth(m_kinect_save_range_img);
195 m_cap_kinect->enableGrabRGB(m_kinect_save_intensity_img);
196 m_cap_kinect->setVideoChannel(
197 m_kinect_video_rgb ? CKinect::VIDEO_CHANNEL_RGB : CKinect::VIDEO_CHANNEL_IR);
198
199 if (!m_path_for_external_images.empty())
200 m_cap_kinect->setPathForExternalImages(m_path_for_external_images);
201
202 // Open it:
203 try
204 {
205 m_cap_kinect->initialize(); // This will launch an exception if needed.
206 }
207 catch (std::exception&)
208 {
209 m_state = CGenericSensor::ssError;
210 throw;
211 }
212 }
213 else if (m_grabber_type == "openni2")
214 {
215 cout << "[CCameraSensor::initialize] OpenNI2 sensor...\n";
216 m_cap_openni2 = std::make_unique<COpenNI2Sensor>();
217 m_cap_openni2->enableGrab3DPoints(m_kinect_save_3d); // It uses the
218 // same options as
219 // the Kinect
220 // grabber
221 m_cap_openni2->enableGrabDepth(m_kinect_save_range_img);
222 m_cap_openni2->enableGrabRGB(m_kinect_save_intensity_img);
223
224 if (!m_path_for_external_images.empty())
225 m_cap_openni2->setPathForExternalImages(m_path_for_external_images);
226
227 // Open it:
228 try
229 {
230 m_cap_openni2->initialize(); // This will launch an exception if needed.
231 }
232 catch (const std::exception& e)
233 {
234 m_state = CGenericSensor::ssError;
235 throw e;
236 }
237 }
238 else if (m_grabber_type == "image_dir")
239 {
240 // m_cap_image_dir
241 cout << mrpt::format("[CCameraSensor::initialize] Image dir: %s...\n", m_img_dir_url.c_str());
242 m_cap_image_dir = std::make_unique<std::string>();
243 }
244 else if (m_grabber_type == "rawlog")
245 {
246 // m_cap_rawlog
247 cout << mrpt::format(
248 "[CCameraSensor::initialize] Rawlog stream: %s...\n", m_rawlog_file.c_str());
249 m_cap_rawlog = std::make_unique<CCompressedInputStream>();
250
251 if (!m_cap_rawlog->open(m_rawlog_file))
252 {
253 m_state = CGenericSensor::ssError;
254 THROW_EXCEPTION_FMT("Error opening rawlog file: %s", m_rawlog_file.c_str());
255 }
256 // File open OK.
257 // Localize the external images directory of this rawlog, if it exists:
258 m_rawlog_detected_images_dir = CRawlog::detectImagesDirectory(m_rawlog_file);
259 }
260 else if (m_grabber_type == "flycap")
261 {
262 cout << "[CCameraSensor::initialize] PGR FlyCapture2 camera...\n";
263 try
264 {
265 // Open camera and start capture:
266 m_cap_flycap = std::make_unique<CImageGrabber_FlyCapture2>(m_flycap_options);
267 }
268 catch (std::exception&)
269 {
270 m_state = CGenericSensor::ssError;
271 throw;
272 }
273 }
274 else if (m_grabber_type == "flycap_stereo")
275 {
276 cout << "[CCameraSensor::initialize] PGR FlyCapture2 stereo camera...\n";
277 try
278 {
279 // Open camera and start capture:
280 m_cap_flycap_stereo_l = std::make_unique<CImageGrabber_FlyCapture2>();
281 m_cap_flycap_stereo_r = std::make_unique<CImageGrabber_FlyCapture2>();
282
283 cout << "[CCameraSensor::initialize] PGR FlyCapture2 stereo "
284 "camera: Opening LEFT camera...\n";
285 m_cap_flycap_stereo_l->open(m_flycap_stereo_options[0], false /* don't start grabbing */);
286
287 cout << "[CCameraSensor::initialize] PGR FlyCapture2 stereo "
288 "camera: Opening RIGHT camera...\n";
289 m_cap_flycap_stereo_r->open(m_flycap_stereo_options[1], false /* don't start grabbing */);
290
291 // Now, start grabbing "simultaneously":
292 if (m_fcs_start_synch_capture)
293 {
294 const CImageGrabber_FlyCapture2* cams[2] = {
295 m_cap_flycap_stereo_l.get(), m_cap_flycap_stereo_r.get()};
296 CImageGrabber_FlyCapture2::startSyncCapture(2, cams);
297 }
298 else
299 {
300 m_cap_flycap_stereo_l->startCapture();
301 m_cap_flycap_stereo_r->startCapture();
302 }
303 }
304 catch (std::exception&)
305 {
306 m_state = CGenericSensor::ssError;
307 throw;
308 }
309 }
310 else if (m_grabber_type == "myntd")
311 {
312 cout << "[CCameraSensor::initialize] MYNTEYE-D camera ...\n";
313
314 // Open it:
315 try
316 {
317 m_myntd = std::make_unique<CMyntEyeCamera>(m_myntd_options);
318 }
319 catch (const std::exception& e)
320 {
321 m_state = CGenericSensor::ssError;
322 throw e;
323 }
324 }
325 else
326 THROW_EXCEPTION_FMT("Unknown 'grabber_type' found: %s", m_grabber_type.c_str());
327
328 // Change state:
329 cout << "[CCameraSensor::initialize] Done!" << endl;
330 m_state = CGenericSensor::ssWorking;
331
332 // Launch independent thread?
333 if (m_external_images_own_thread)
334 {
335 m_threadImagesSaverShouldEnd = false;
336
337 m_threadImagesSaver.clear();
338 m_threadImagesSaver.resize(m_external_image_saver_count);
339
340 m_toSaveList.clear();
341 m_toSaveList.resize(m_external_image_saver_count);
342
343 for (unsigned int i = 0; i < m_external_image_saver_count; ++i)
344 {
345 m_threadImagesSaver[i] = std::thread(&CCameraSensor::thread_save_images, this, i);
346
347 mrpt::system::thread_name("CameraSaveImages", m_threadImagesSaver[i]);
348 }
349 }
350 }
351
352 /* -----------------------------------------------------
353 close
354 ----------------------------------------------------- */
355 void CCameraSensor::close()
356 {
357 m_cap_cv.reset();
358 m_cap_dc1394.reset();
359 m_cap_flycap.reset();
360 m_cap_flycap_stereo_l.reset();
361 m_cap_flycap_stereo_r.reset();
362 m_cap_bumblebee_dc1394.reset();
363 m_cap_ffmpeg.reset();
364 m_cap_rawlog.reset();
365 m_cap_swissranger.reset();
366 m_cap_kinect.reset();
367 m_cap_svs.reset();
368 m_cap_image_dir.reset();
369
370 m_state = CGenericSensor::ssInitializing;
371
372 // Wait for threads:
373 if (!m_threadImagesSaver.empty())
374 {
375 m_threadImagesSaverShouldEnd = true;
376 for (auto& i : m_threadImagesSaver)
377 if (i.joinable()) i.join();
378 }
379 }
380
381 /* -----------------------------------------------------
382 loadConfig_sensorSpecific
383 ----------------------------------------------------- */
384 void CCameraSensor::loadConfig_sensorSpecific(
385 const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
386 {
387 // At this point, my parent class CGenericSensor has already loaded its
388 // params:
389 // Since cameras are special, we'll take control over "m_grab_decimation"
390 // so
391 // external image files are not saved just to be discarded later on...
392 if (m_grab_decimation > 0)
393 {
394 m_camera_grab_decimator = m_grab_decimation;
395 m_camera_grab_decimator_counter = 0;
396 // Reset in parent:
397 m_grab_decimation = 0;
398 }
399 else
400 m_camera_grab_decimator = m_camera_grab_decimator_counter = 0;
401
402 m_grabber_type = configSource.read_string_first_word(iniSection, "grabber_type", m_grabber_type);
403 MRPT_LOAD_HERE_CONFIG_VAR(preview_decimation, int, m_preview_decimation, configSource, iniSection)
404 MRPT_LOAD_HERE_CONFIG_VAR(preview_reduction, int, m_preview_reduction, configSource, iniSection)
405
406 // OpenCV options:
407 m_cv_camera_type =
408 configSource.read_string_first_word(iniSection, "cv_camera_type", m_cv_camera_type);
409 m_cv_camera_index = configSource.read_int(iniSection, "cv_camera_index", m_cv_camera_index);
410
411 m_cv_options.frame_width =
412 configSource.read_int(iniSection, "cv_frame_width", m_cv_options.frame_width);
413 m_cv_options.frame_height =
414 configSource.read_int(iniSection, "cv_frame_height", m_cv_options.frame_height);
415 m_cv_options.gain = configSource.read_double(iniSection, "cv_gain", m_cv_options.gain);
416 m_cv_options.ieee1394_fps =
417 configSource.read_double(iniSection, "cv_fps", m_cv_options.ieee1394_fps);
418
419 m_capture_grayscale = configSource.read_bool(iniSection, "capture_grayscale", false);
420
421 m_cv_options.ieee1394_grayscale = m_capture_grayscale;
422
423 // dc1394 options:
424 MRPT_LOAD_HERE_CONFIG_VAR(
425 dc1394_camera_guid, uint64_t, m_dc1394_camera_guid, configSource, iniSection)
426 MRPT_LOAD_HERE_CONFIG_VAR(dc1394_camera_unit, int, m_dc1394_camera_unit, configSource, iniSection)
427
428 MRPT_LOAD_HERE_CONFIG_VAR(
429 dc1394_frame_width, int, m_dc1394_options.frame_width, configSource, iniSection)
430 MRPT_LOAD_HERE_CONFIG_VAR(
431 dc1394_frame_height, int, m_dc1394_options.frame_height, configSource, iniSection)
432
433 MRPT_LOAD_HERE_CONFIG_VAR(dc1394_mode7, int, m_dc1394_options.mode7, configSource, iniSection)
434
435 MRPT_LOAD_HERE_CONFIG_VAR(dc1394_shutter, int, m_dc1394_options.shutter, configSource, iniSection)
436 MRPT_LOAD_HERE_CONFIG_VAR(dc1394_gain, int, m_dc1394_options.gain, configSource, iniSection)
437 MRPT_LOAD_HERE_CONFIG_VAR(dc1394_gamma, int, m_dc1394_options.gamma, configSource, iniSection)
438 MRPT_LOAD_HERE_CONFIG_VAR(
439 dc1394_brightness, int, m_dc1394_options.brightness, configSource, iniSection)
440 MRPT_LOAD_HERE_CONFIG_VAR(
441 dc1394_exposure, int, m_dc1394_options.exposure, configSource, iniSection)
442 MRPT_LOAD_HERE_CONFIG_VAR(
443 dc1394_sharpness, int, m_dc1394_options.sharpness, configSource, iniSection)
444 MRPT_LOAD_HERE_CONFIG_VAR(
445 dc1394_white_balance, int, m_dc1394_options.white_balance, configSource, iniSection)
446
447 MRPT_LOAD_HERE_CONFIG_VAR(
448 dc1394_shutter_mode, int, m_dc1394_options.shutter_mode, configSource, iniSection)
449 MRPT_LOAD_HERE_CONFIG_VAR(
450 dc1394_gain_mode, int, m_dc1394_options.gain_mode, configSource, iniSection)
451 MRPT_LOAD_HERE_CONFIG_VAR(
452 dc1394_gamma_mode, int, m_dc1394_options.gamma_mode, configSource, iniSection)
453 MRPT_LOAD_HERE_CONFIG_VAR(
454 dc1394_brightness_mode, int, m_dc1394_options.brightness_mode, configSource, iniSection)
455 MRPT_LOAD_HERE_CONFIG_VAR(
456 dc1394_exposure_mode, int, m_dc1394_options.exposure_mode, configSource, iniSection)
457 MRPT_LOAD_HERE_CONFIG_VAR(
458 dc1394_sharpness_mode, int, m_dc1394_options.sharpness_mode, configSource, iniSection)
459 MRPT_LOAD_HERE_CONFIG_VAR(
460 dc1394_white_balance_mode, int, m_dc1394_options.white_balance_mode, configSource, iniSection)
461
462 MRPT_LOAD_HERE_CONFIG_VAR(
463 dc1394_trigger_power, int, m_dc1394_options.trigger_power, configSource, iniSection)
464 MRPT_LOAD_HERE_CONFIG_VAR(
465 dc1394_trigger_mode, int, m_dc1394_options.trigger_mode, configSource, iniSection)
466 MRPT_LOAD_HERE_CONFIG_VAR(
467 dc1394_trigger_source, int, m_dc1394_options.trigger_source, configSource, iniSection)
468 MRPT_LOAD_HERE_CONFIG_VAR(
469 dc1394_trigger_polarity, int, m_dc1394_options.trigger_polarity, configSource, iniSection)
470 MRPT_LOAD_HERE_CONFIG_VAR(
471 dc1394_ring_buffer_size, int, m_dc1394_options.ring_buffer_size, configSource, iniSection)
472
473 // Bumblebee_dc1394 options:
474 MRPT_LOAD_HERE_CONFIG_VAR(
475 bumblebee_dc1394_camera_guid, uint64_t, m_bumblebee_dc1394_camera_guid, configSource,
476 iniSection)
477 MRPT_LOAD_HERE_CONFIG_VAR(
478 bumblebee_dc1394_camera_unit, int, m_bumblebee_dc1394_camera_unit, configSource, iniSection)
479 MRPT_LOAD_HERE_CONFIG_VAR(
480 bumblebee_dc1394_framerate, double, m_bumblebee_dc1394_framerate, configSource, iniSection)
481
482 // SVS options:
483 m_svs_camera_index = configSource.read_int(iniSection, "svs_camera_index", m_svs_camera_index);
484 m_svs_options.frame_width =
485 configSource.read_int(iniSection, "svs_frame_width", m_svs_options.frame_width);
486 m_svs_options.frame_height =
487 configSource.read_int(iniSection, "svs_frame_height", m_svs_options.frame_height);
488 m_svs_options.framerate =
489 configSource.read_double(iniSection, "svs_framerate", m_svs_options.framerate);
490 m_svs_options.m_NDisp = configSource.read_int(iniSection, "svs_NDisp", m_svs_options.m_NDisp);
491 m_svs_options.m_Corrsize =
492 configSource.read_int(iniSection, "svs_Corrsize", m_svs_options.m_Corrsize);
493 m_svs_options.m_LR = configSource.read_int(iniSection, "svs_LR", m_svs_options.m_LR);
494 m_svs_options.m_Thresh = configSource.read_int(iniSection, "svs_Thresh", m_svs_options.m_Thresh);
495 m_svs_options.m_Unique = configSource.read_int(iniSection, "svs_Unique", m_svs_options.m_Unique);
496 m_svs_options.m_Horopter =
497 configSource.read_int(iniSection, "svs_Horopter", m_svs_options.m_Horopter);
498 m_svs_options.m_SpeckleSize =
499 configSource.read_int(iniSection, "svs_SpeckleSize", m_svs_options.m_SpeckleSize);
500 m_svs_options.m_procesOnChip =
501 configSource.read_bool(iniSection, "svs_procesOnChip", m_svs_options.m_procesOnChip);
502 m_svs_options.m_calDisparity =
503 configSource.read_bool(iniSection, "svs_calDisparity", m_svs_options.m_calDisparity);
504
505 // FFmpeg options:
506 m_ffmpeg_url =
507 mrpt::system::trim(configSource.read_string(iniSection, "ffmpeg_url", m_ffmpeg_url));
508
509 // Rawlog options:
510 m_rawlog_file =
511 mrpt::system::trim(configSource.read_string(iniSection, "rawlog_file", m_rawlog_file));
512 m_rawlog_camera_sensor_label = mrpt::system::trim(configSource.read_string(
513 iniSection, "rawlog_camera_sensor_label", m_rawlog_camera_sensor_label));
514
515 // Image directory options:
516 m_img_dir_url =
517 mrpt::system::trim(configSource.read_string(iniSection, "image_dir_url", m_img_dir_url));
518 m_img_dir_left_format = mrpt::system::trim(
519 configSource.read_string(iniSection, "left_format", m_img_dir_left_format));
520 m_img_dir_right_format =
521 mrpt::system::trim(configSource.read_string(iniSection, "right_format", ""));
522 m_img_dir_start_index = configSource.read_int(iniSection, "start_index", m_img_dir_start_index);
523 ;
524 m_img_dir_end_index = configSource.read_int(iniSection, "end_index", m_img_dir_end_index);
525
526 m_img_dir_is_stereo = !m_img_dir_right_format.empty();
527 m_img_dir_counter = m_img_dir_start_index;
528
529 // SwissRanger options:
530 m_sr_open_from_usb = configSource.read_bool(iniSection, "sr_use_usb", m_sr_open_from_usb);
531 m_sr_ip_address = configSource.read_string(iniSection, "sr_IP", m_sr_ip_address);
532
533 m_sr_save_3d = configSource.read_bool(iniSection, "sr_grab_3d", m_sr_save_3d);
534 m_sr_save_intensity_img =
535 configSource.read_bool(iniSection, "sr_grab_grayscale", m_sr_save_intensity_img);
536 m_sr_save_range_img = configSource.read_bool(iniSection, "sr_grab_range", m_sr_save_range_img);
537 m_sr_save_confidence =
538 configSource.read_bool(iniSection, "sr_grab_confidence", m_sr_save_confidence);
539
540 m_kinect_save_3d = configSource.read_bool(iniSection, "kinect_grab_3d", m_kinect_save_3d);
541 m_kinect_save_intensity_img =
542 configSource.read_bool(iniSection, "kinect_grab_intensity", m_kinect_save_intensity_img);
543 m_kinect_save_range_img =
544 configSource.read_bool(iniSection, "kinect_grab_range", m_kinect_save_range_img);
545 m_kinect_video_rgb = configSource.read_bool(iniSection, "kinect_video_rgb", m_kinect_video_rgb);
546
547 // FlyCap:
548 m_flycap_options.loadOptionsFrom(configSource, iniSection, "flycap_");
549
550 // Myntd:
551 {
552 mrpt::config::CConfigFilePrefixer c(configSource, "", "myntd_");
553 m_myntd_options.loadFromConfigFile(c, iniSection);
554 }
555
556 // FlyCap stereo
557 m_fcs_start_synch_capture =
558 configSource.read_bool(iniSection, "fcs_start_synch_capture", m_fcs_start_synch_capture);
559 m_flycap_stereo_options[0].loadOptionsFrom(configSource, iniSection, "fcs_LEFT_");
560 m_flycap_stereo_options[1].loadOptionsFrom(configSource, iniSection, "fcs_RIGHT_");
561
562 // Special stuff: FPS
563 map<double, grabber_dc1394_framerate_t> map_fps;
564 map<double, grabber_dc1394_framerate_t>::iterator it_fps;
565 map_fps[1.875] = FRAMERATE_1_875;
566 map_fps[3.75] = FRAMERATE_3_75;
567 map_fps[7.5] = FRAMERATE_7_5;
568 map_fps[15] = FRAMERATE_15;
569 map_fps[30] = FRAMERATE_30;
570 map_fps[60] = FRAMERATE_60;
571 map_fps[120] = FRAMERATE_120;
572 map_fps[240] = FRAMERATE_240;
573
574 // ... for dc1394
575 double the_fps = configSource.read_double(iniSection, "dc1394_framerate", 15.0);
576 it_fps = map_fps.find(the_fps);
577 if (it_fps == map_fps.end())
578 THROW_EXCEPTION_FMT("ERROR: DC1394 framerate seems to be not a valid number: %f", the_fps);
579
580 m_dc1394_options.framerate = it_fps->second;
581
582 // Special stuff: color encoding:
583 map<string, grabber_dc1394_color_coding_t> map_color;
584 map<string, grabber_dc1394_color_coding_t>::iterator it_color;
585 #define ADD_COLOR_MAP(c) map_color[#c] = c;
586 ADD_COLOR_MAP(COLOR_CODING_MONO8)
587 ADD_COLOR_MAP(COLOR_CODING_YUV411)
588 ADD_COLOR_MAP(COLOR_CODING_YUV422)
589 ADD_COLOR_MAP(COLOR_CODING_YUV444)
590 ADD_COLOR_MAP(COLOR_CODING_RGB8)
591 ADD_COLOR_MAP(COLOR_CODING_MONO16)
592
593 string the_color_coding = mrpt::system::upperCase(configSource.read_string_first_word(
594 iniSection, "dc1394_color_coding", "COLOR_CODING_YUV422"));
595 it_color = map_color.find(the_color_coding);
596 if (it_color == map_color.end())
597 THROW_EXCEPTION_FMT(
598 "ERROR: Color coding seems not to be valid : '%s'", the_color_coding.c_str());
599 m_dc1394_options.color_coding = it_color->second;
600
601 m_external_images_format = mrpt::system::trim(
602 configSource.read_string(iniSection, "external_images_format", m_external_images_format));
603 m_external_images_jpeg_quality = configSource.read_int(
604 iniSection, "external_images_jpeg_quality", m_external_images_jpeg_quality);
605 m_external_images_own_thread = configSource.read_bool(
606 iniSection, "external_images_own_thread", m_external_images_own_thread);
607 m_external_image_saver_count = configSource.read_int(
608 iniSection, "external_images_own_thread_count", m_external_image_saver_count);
609
610 // Sensor pose:
611 m_sensorPose.setFromValues(
612 configSource.read_float(iniSection, "pose_x", 0),
613 configSource.read_float(iniSection, "pose_y", 0),
614 configSource.read_float(iniSection, "pose_z", 0),
615 DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
616 DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
617 DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
618 }
619
620 /* -----------------------------------------------------
621 Destructor
622 ----------------------------------------------------- */
623 CCameraSensor::~CCameraSensor()
624 {
625 close();
626
627 m_preview_win1.reset();
628 m_preview_win2.reset();
629 }
630 /* -----------------------------------------------------
631 getNextFrame
632 ----------------------------------------------------- */
633 CObservation::Ptr CCameraSensor::getNextFrame()
634 {
635 std::vector<CSerializable::Ptr> out_obs;
636 getNextFrame(out_obs);
637 if (out_obs.empty())
638 {
639 return {};
640 }
641
642 return std::dynamic_pointer_cast<CObservation>(out_obs[0]);
643 }
644
645 /* -----------------------------------------------------
646 getNextFrame
647 ----------------------------------------------------- */
648 void CCameraSensor::getNextFrame(vector<CSerializable::Ptr>& out_obs)
649 {
650 CObservationImage::Ptr obs;
651 CObservationStereoImages::Ptr stObs;
652 CObservation3DRangeScan::Ptr obs3D; // 3D range image, also with an intensity channel
653 CObservationIMU::Ptr obsIMU; // IMU observation grabbed by DUO3D cameras
654
655 bool capture_ok = false;
656
657 if (m_cap_cv)
658 {
659 obs = std::make_shared<CObservationImage>();
660 if (!m_cap_cv->getObservation(*obs))
661 { // Error
662 m_state = CGenericSensor::ssError;
663 THROW_EXCEPTION("Error grabbing image");
664 }
665 else
666 capture_ok = true;
667 }
668 else if (m_cap_dc1394)
669 {
670 obs = std::make_shared<CObservationImage>();
671 if (!m_cap_dc1394->getObservation(*obs))
672 { // Error
673 m_state = CGenericSensor::ssError;
674 THROW_EXCEPTION("Error grabbing image");
675 }
676 else
677 capture_ok = true;
678 }
679 else if (m_cap_swissranger)
680 {
681 obs3D = std::make_shared<CObservation3DRangeScan>();
682
683 bool there_is_obs, hardware_error;
684 m_cap_swissranger->getNextObservation(*obs3D, there_is_obs, hardware_error);
685
686 if (!there_is_obs || hardware_error)
687 { // Error
688 m_state = CGenericSensor::ssError;
689 THROW_EXCEPTION("Error grabbing image from SwissRanger camera.");
690 }
691 else
692 capture_ok = true;
693 }
694 else if (m_cap_kinect)
695 {
696 obs3D = std::make_shared<CObservation3DRangeScan>();
697
698 // Specially at start-up, there may be a delay grabbing so a few calls
699 // return false: add a timeout.
700 const mrpt::system::TTimeStamp t0 = mrpt::Clock::now();
701 double max_timeout = 3.0; // seconds
702
703 // If we have an "MRPT_CCAMERA_KINECT_TIMEOUT_MS" environment variable,
704 // use that timeout instead:
705 const char* envVal = getenv("MRPT_CCAMERA_KINECT_TIMEOUT_MS");
706 if (envVal) max_timeout = atoi(envVal) * 0.001;
707
708 bool there_is_obs, hardware_error;
709 do
710 {
711 m_cap_kinect->getNextObservation(*obs3D, there_is_obs, hardware_error);
712 if (!there_is_obs) std::this_thread::sleep_for(1ms);
713 } while (!there_is_obs && mrpt::system::timeDifference(t0, mrpt::Clock::now()) < max_timeout);
714
715 if (!there_is_obs || hardware_error)
716 { // Error
717 m_state = CGenericSensor::ssError;
718 THROW_EXCEPTION("Error grabbing image from Kinect camera.");
719 }
720 else
721 capture_ok = true;
722 }
723 else if (m_cap_openni2)
724 {
725 obs3D = std::make_shared<CObservation3DRangeScan>();
726 // Specially at start-up, there may be a delay grabbing so a few calls
727 // return false: add a timeout.
728 const mrpt::system::TTimeStamp t0 = mrpt::Clock::now();
729 double max_timeout = 3.0; // seconds
730 bool there_is_obs, hardware_error;
731 do
732 {
733 m_cap_openni2->getNextObservation(*obs3D, there_is_obs, hardware_error);
734 if (!there_is_obs) std::this_thread::sleep_for(1ms);
735 } while (!there_is_obs && mrpt::system::timeDifference(t0, mrpt::Clock::now()) < max_timeout);
736
737 if (!there_is_obs || hardware_error)
738 { // Error
739 m_state = CGenericSensor::ssError;
740 THROW_EXCEPTION("Error grabbing image from OpenNI2 camera.");
741 }
742 else
743 capture_ok = true;
744 }
745 else if (m_cap_bumblebee_dc1394)
746 {
747 stObs = std::make_shared<CObservationStereoImages>();
748 if (!m_cap_bumblebee_dc1394->getStereoObservation(*stObs))
749 {
750 m_state = CGenericSensor::ssError;
751 THROW_EXCEPTION("Error grabbing stereo images");
752 }
753 else
754 {
755 capture_ok = true;
756 }
757 }
758 else if (m_cap_svs)
759 {
760 stObs = std::make_shared<CObservationStereoImages>();
761
762 if (!m_cap_svs->getStereoObservation(*stObs))
763 {
764 // Error
765 m_state = CGenericSensor::ssError;
766 THROW_EXCEPTION("Error grabbing disparity images");
767 }
768 else
769 capture_ok = true;
770 }
771 else if (m_cap_ffmpeg)
772 {
773 static bool anyGood = false;
774 mrpt::img::CImage im;
775 if (!m_cap_ffmpeg->retrieveFrame(im))
776 { // Error
777 m_state = CGenericSensor::ssError;
778 if (!anyGood)
779 {
780 THROW_EXCEPTION("Error grabbing image");
781 }
782 else
783 {
784 MRPT_LOG_THROTTLE_WARN(
785 2.0,
786 "ffmpeg capture driver: Failed to get frame (temporary "
787 "error or EOF?)");
788 capture_ok = true;
789 }
790 }
791 else
792 {
793 obs = std::make_shared<CObservationImage>();
794 obs->image = std::move(im);
795
796 anyGood = true;
797 capture_ok = true;
798 }
799 }
800 else if (m_cap_image_dir)
801 {
802 if (m_img_dir_counter > m_img_dir_end_index)
803 {
804 m_state = CGenericSensor::ssError;
805 THROW_EXCEPTION("Reached end index.");
806 }
807
808 std::string auxL = mrpt::format("%s/%s", m_img_dir_url.c_str(), m_img_dir_left_format.c_str());
809 if (m_img_dir_is_stereo)
810 {
811 stObs = std::make_shared<CObservationStereoImages>();
812 if (!stObs->imageLeft.loadFromFile(format(auxL.c_str(), m_img_dir_counter)))
813 {
814 m_state = CGenericSensor::ssError;
815 THROW_EXCEPTION("Error reading images from directory");
816 }
817 std::string auxR =
818 mrpt::format("%s/%s", m_img_dir_url.c_str(), m_img_dir_right_format.c_str());
819 if (!stObs->imageRight.loadFromFile(format(auxR.c_str(), m_img_dir_counter++)))
820 {
821 m_state = CGenericSensor::ssError;
822 THROW_EXCEPTION("Error reading images from directory");
823 }
824 else
825 capture_ok = true;
826 }
827 else
828 {
829 // use only left image prefix
830 obs = std::make_shared<CObservationImage>();
831 if (!obs->image.loadFromFile(format(auxL.c_str(), m_img_dir_counter++)))
832 {
833 m_state = CGenericSensor::ssError;
834 THROW_EXCEPTION("Error reading images from directory");
835 }
836 else
837 capture_ok = true;
838 }
839 }
840 else if (m_cap_rawlog)
841 {
842 // Read in a loop until we found at least one image:
843 // Assign to: obs && stObs
844 CSerializable::Ptr newObs;
845 while (!obs && !stObs && !obs3D)
846 {
847 archiveFrom(*m_cap_rawlog) >> newObs;
848 if (IS_DERIVED(*newObs, CObservation))
849 {
850 CObservation::Ptr o = std::dynamic_pointer_cast<CObservation>(newObs);
851 if (!m_rawlog_camera_sensor_label.empty() && m_rawlog_camera_sensor_label != o->sensorLabel)
852 continue;
853
854 if (IS_CLASS(*o, CObservationImage))
855 obs = std::dynamic_pointer_cast<CObservationImage>(o);
856 else if (IS_CLASS(*o, CObservationStereoImages))
857 stObs = std::dynamic_pointer_cast<CObservationStereoImages>(o);
858 else if (IS_CLASS(*o, CObservation3DRangeScan))
859 obs3D = std::dynamic_pointer_cast<CObservation3DRangeScan>(o);
860 }
861 else if (IS_CLASS(*newObs, CSensoryFrame))
862 {
863 CSensoryFrame::Ptr sf = std::dynamic_pointer_cast<CSensoryFrame>(newObs);
864
865 for (auto& o : *sf)
866 {
867 if (!m_rawlog_camera_sensor_label.empty() &&
868 m_rawlog_camera_sensor_label != o->sensorLabel)
869 continue;
870
871 if (IS_CLASS(*o, CObservationImage))
872 {
873 obs = std::dynamic_pointer_cast<CObservationImage>(o);
874 break;
875 }
876 else if (IS_CLASS(*o, CObservationStereoImages))
877 {
878 stObs = std::dynamic_pointer_cast<CObservationStereoImages>(o);
879 break;
880 }
881 else if (IS_CLASS(*o, CObservation3DRangeScan))
882 {
883 obs3D = std::dynamic_pointer_cast<CObservation3DRangeScan>(o);
884 break;
885 }
886 }
887 }
888 if (obs || stObs || obs3D)
889 {
890 // We must convert externally stored images into "normal
891 // in-memory" images.
892 const std::string old_dir = CImage::getImagesPathBase(); // Save current
893 CImage::setImagesPathBase(m_rawlog_detected_images_dir);
894
895 if (obs && obs->image.isExternallyStored())
896 {
897 bool loadOk = obs->image.loadFromFile(obs->image.getExternalStorageFileAbsolutePath());
898 ASSERT_(loadOk);
899 }
900
901 if (obs3D && obs3D->hasIntensityImage && obs3D->intensityImage.isExternallyStored())
902 {
903 bool loadOk = obs3D->intensityImage.loadFromFile(
904 obs3D->intensityImage.getExternalStorageFileAbsolutePath());
905 ASSERT_(loadOk);
906 }
907
908 if (stObs && stObs->imageLeft.isExternallyStored())
909 {
910 bool loadOk =
911 stObs->imageLeft.loadFromFile(stObs->imageLeft.getExternalStorageFileAbsolutePath());
912 ASSERT_(loadOk);
913 }
914
915 if (stObs && stObs->hasImageRight && stObs->imageRight.isExternallyStored())
916 {
917 bool loadOk = stObs->imageRight.loadFromFile(
918 stObs->imageRight.getExternalStorageFileAbsolutePath());
919 ASSERT_(loadOk);
920 }
921
922 if (stObs && stObs->hasImageDisparity && stObs->imageDisparity.isExternallyStored())
923 {
924 bool loadOk = stObs->imageDisparity.loadFromFile(
925 stObs->imageDisparity.getExternalStorageFileAbsolutePath());
926 ASSERT_(loadOk);
927 }
928
929 CImage::setImagesPathBase(old_dir); // Restore
930 }
931 else
932 continue; // Keep reading
933 }
934 capture_ok = true;
935 }
936 else if (m_cap_flycap)
937 {
938 bool ok;
939 if (!m_cap_flycap->isStereo()) // Mono image
940 {
941 obs = std::make_shared<CObservationImage>();
942 ok = m_cap_flycap->getObservation(*obs);
943 }
944 else // Stereo camera connected
945 {
946 stObs = std::make_shared<CObservationStereoImages>();
947 ok = m_cap_flycap->getObservation(*stObs);
948 }
949
950 if (!ok)
951 { // Error
952 m_state = CGenericSensor::ssError;
953 THROW_EXCEPTION("Error grabbing image");
954 }
955 else
956 capture_ok = true;
957 }
958 else if (m_cap_flycap_stereo_l && m_cap_flycap_stereo_r)
959 {
960 stObs = std::make_shared<CObservationStereoImages>();
961
962 CObservationImage obsL, obsR;
963
964 bool ok1, ok2 = false;
965
966 ok1 = m_cap_flycap_stereo_r->getObservation(obsL);
967 if (ok1) ok2 = m_cap_flycap_stereo_l->getObservation(obsR);
968
969 if (!ok1 || !ok2)
970 {
971 // Error
972 m_state = CGenericSensor::ssError;
973 THROW_EXCEPTION("Error grabbing disparity images");
974 }
975 else
976 {
977 // Joint the two images as one stereo:
978 const double At = mrpt::system::timeDifference(obsL.timestamp, obsR.timestamp);
979 if (std::abs(At) > 0.1)
980 {
981 cout << "[CCamera, flycap_stereo] Warning: Too large delay "
982 "between left & right images: "
983 << At << " sec.\n";
984 }
985
986 // It seems that the timestamp is not always filled in from FlyCap
987 // driver?
988 stObs->timestamp =
989 (obsL.timestamp != INVALID_TIMESTAMP) ? obsL.timestamp : mrpt::Clock::now();
990 stObs->imageLeft = std::move(obsL.image);
991 stObs->imageRight = std::move(obsR.image);
992 capture_ok = true;
993 }
994 }
995 else if (m_myntd)
996 {
997 obs3D = std::make_shared<CObservation3DRangeScan>();
998
999 bool thereIsObs = m_myntd->getObservation(*obs3D);
1000 static int noObsCnt = 0;
1001
1002 if (!thereIsObs)
1003 {
1004 // obs3D.reset();
1005 if (noObsCnt++ > 100)
1006 {
1007 m_state = CGenericSensor::ssError;
1008 THROW_EXCEPTION("Error getting observations from MYNTEYE-D camera.");
1009 }
1010 }
1011 else
1012 {
1013 noObsCnt = 0;
1014 }
1015 capture_ok = true;
1016 }
1017 else
1018 {
1019 THROW_EXCEPTION(
1020 "There is no initialized camera driver: has 'initialize()' been "
1021 "called?");
1022 }
1023
1024 ASSERT_(capture_ok);
1025
1026 // Are we supposed to do a decimation??
1027 m_camera_grab_decimator_counter++;
1028 if (m_camera_grab_decimator_counter < m_camera_grab_decimator)
1029 {
1030 // Done here:
1031 out_obs.push_back(CObservation::Ptr());
1032 return;
1033 }
1034 // Continue as normal:
1035 m_camera_grab_decimator_counter = 0;
1036
1037 // Allow temporary failures to get any frame
1038 // ASSERT_(obs || stObs || obs3D || obsIMU);
1039
1040 // If we grabbed an image: prepare it and add it to the internal queue:
1041 if (obs)
1042 {
1043 obs->sensorLabel = m_sensorLabel;
1044 obs->setSensorPose(m_sensorPose);
1045 }
1046 else if (stObs)
1047 {
1048 stObs->sensorLabel = m_sensorLabel;
1049 stObs->setSensorPose(m_sensorPose);
1050 }
1051 else if (obs3D)
1052 {
1053 obs3D->sensorLabel = m_sensorLabel;
1054 obs3D->setSensorPose(m_sensorPose);
1055 }
1056 if (obsIMU)
1057 {
1058 obsIMU->sensorLabel = m_sensorLabel + "_IMU";
1059 obsIMU->setSensorPose(m_sensorPose);
1060 }
1061
1062 // Convert to grayscale if the user wants so and the driver did ignored us:
1063 if (m_capture_grayscale)
1064 {
1065 if (obs)
1066 {
1067 if (obs->image.isColor()) obs->image = obs->image.grayscale();
1068 }
1069 else if (stObs)
1070 {
1071 if (stObs->imageLeft.isColor()) stObs->imageLeft = stObs->imageLeft.grayscale();
1072 if (stObs->hasImageRight && stObs->imageRight.isColor())
1073 stObs->imageRight = stObs->imageRight.grayscale();
1074 if (stObs->hasImageDisparity && stObs->imageDisparity.isColor())
1075 stObs->imageDisparity = stObs->imageDisparity.grayscale();
1076 }
1077 else if (obs3D)
1078 {
1079 if (obs3D->hasIntensityImage && obs3D->intensityImage.isColor())
1080 obs3D->intensityImage = obs3D->intensityImage.grayscale();
1081 }
1082 }
1083
1084 // Before saving to disk, keep a copy for display, if needed:
1085 CImage img4gui, img4guiR;
1086 if (m_preview_win1 && m_preview_win1->isOpen())
1087 {
1088 if (stObs)
1089 {
1090 img4gui = stObs->imageLeft.makeDeepCopy();
1091 img4guiR = stObs->imageRight.makeDeepCopy();
1092 }
1093 else if (obs)
1094 img4gui = obs->image.makeDeepCopy();
1095 else
1096 img4gui = obs3D->intensityImage.makeDeepCopy();
1097 }
1098
1099 // External storage?
1100 // If true, we'll return nothing, but the observation will be
1101 // inserted from the thread.
1102 bool delayed_insertion_in_obs_queue = false;
1103 if (!m_path_for_external_images.empty())
1104 {
1105 if (stObs) // If we have grabbed an stereo observation ...
1106 { // Stereo obs -------
1107 if (m_external_images_own_thread)
1108 {
1109 m_csToSaveList.lock();
1110
1111 // Select the "m_toSaveList" with the shortest pending queue:
1112 size_t idx_min = 0;
1113 for (size_t i = 0; i < m_toSaveList.size(); ++i)
1114 if (m_toSaveList[i].size() < m_toSaveList[idx_min].size()) idx_min = i;
1115 // Insert:
1116 m_toSaveList[idx_min].insert(TListObsPair(stObs->timestamp, stObs));
1117
1118 m_csToSaveList.unlock();
1119
1120 delayed_insertion_in_obs_queue = true;
1121 }
1122 else
1123 {
1124 const string filNameL = fileNameStripInvalidChars(trim(m_sensorLabel)) +
1125 mrpt::format(
1126 "_L_%f.%s", (double)mrpt::Clock::toDouble(stObs->timestamp),
1127 m_external_images_format.c_str());
1128 const string filNameR = fileNameStripInvalidChars(trim(m_sensorLabel)) +
1129 mrpt::format(
1130 "_R_%f.%s", (double)mrpt::Clock::toDouble(stObs->timestamp),
1131 m_external_images_format.c_str());
1132 const string filNameD = fileNameStripInvalidChars(trim(m_sensorLabel)) +
1133 mrpt::format(
1134 "_D_%f.%s", (double)mrpt::Clock::toDouble(stObs->timestamp),
1135 m_external_images_format.c_str());
1136 // cout << "[CCameraSensor] Saving " << filName << endl;
1137 if (!stObs->imageLeft.saveToFile(
1138 m_path_for_external_images + string("/") + filNameL,
1139 m_external_images_jpeg_quality))
1140 {
1141 THROW_EXCEPTION("Error saving externally-stored images");
1142 }
1143 stObs->imageLeft.setExternalStorage(filNameL);
1144
1145 if (stObs->hasImageRight)
1146 {
1147 if (!stObs->imageRight.saveToFile(
1148 m_path_for_external_images + string("/") + filNameR,
1149 m_external_images_jpeg_quality))
1150 {
1151 THROW_EXCEPTION("Error saving externally-stored images");
1152 }
1153
1154 stObs->imageRight.setExternalStorage(filNameR);
1155 }
1156 if (stObs->hasImageDisparity)
1157 {
1158 if (!stObs->imageDisparity.saveToFile(
1159 m_path_for_external_images + string("/") + filNameD,
1160 m_external_images_jpeg_quality))
1161 {
1162 THROW_EXCEPTION("Error saving externally-stored images");
1163 }
1164
1165 stObs->imageDisparity.setExternalStorage(filNameD);
1166 }
1167 }
1168 }
1169 else if (obs)
1170 { // Monocular image obs -------
1171 if (m_external_images_own_thread)
1172 {
1173 m_csToSaveList.lock();
1174
1175 // Select the "m_toSaveList" with the shortest pending queue:
1176 size_t idx_min = 0;
1177 for (size_t i = 0; i < m_toSaveList.size(); ++i)
1178 if (m_toSaveList[i].size() < m_toSaveList[idx_min].size()) idx_min = i;
1179
1180 // Insert:
1181 m_toSaveList[idx_min].insert(TListObsPair(obs->timestamp, obs));
1182
1183 m_csToSaveList.unlock();
1184 delayed_insertion_in_obs_queue = true;
1185 }
1186 else
1187 {
1188 string filName = fileNameStripInvalidChars(trim(m_sensorLabel)) +
1189 mrpt::format(
1190 "_%f.%s", (double)mrpt::Clock::toDouble(obs->timestamp),
1191 m_external_images_format.c_str());
1192 // cout << "[CCameraSensor] Saving " << filName << endl;
1193 if (!obs->image.saveToFile(
1194 m_path_for_external_images + string("/") + filName, m_external_images_jpeg_quality))
1195 {
1196 THROW_EXCEPTION("Error saving externally-stored images");
1197 }
1198
1199 obs->image.setExternalStorage(filName);
1200 }
1201 } // end else
1202 }
1203
1204 // Show preview??
1205 if (m_preview_decimation > 0)
1206 { // Yes
1207 if (++m_preview_counter > m_preview_decimation)
1208 {
1209 m_preview_counter = 0;
1210
1211 // Create window the first time:
1212 if (!m_preview_win1)
1213 {
1214 string caption = string("Preview of ") + m_sensorLabel;
1215 if (stObs) caption += "-LEFT";
1216 if (m_preview_decimation > 1)
1217 caption += mrpt::format(" (decimation: %i)", m_preview_decimation);
1218 m_preview_win1 = mrpt::gui::CDisplayWindow::Create(caption);
1219 }
1220 if (stObs && !m_preview_win2)
1221 {
1222 string caption = string("Preview of ") + m_sensorLabel;
1223 if (stObs) caption += "-RIGHT";
1224 if (m_preview_decimation > 1)
1225 caption += mrpt::format(" (decimation: %i)", m_preview_decimation);
1226 m_preview_win2 = mrpt::gui::CDisplayWindow::Create(caption);
1227 }
1228 // Monocular image or Left from a stereo pair:
1229 if (m_preview_win1->isOpen() && img4gui.getWidth() > 0)
1230 {
1231 // Apply image reduction?
1232 if (m_preview_reduction >= 2)
1233 {
1234 unsigned int w = img4gui.getWidth();
1235 unsigned int h = img4gui.getHeight();
1236 CImage auxImg;
1237 img4gui.scaleImage(
1238 auxImg, w / m_preview_reduction, h / m_preview_reduction, IMG_INTERP_NN);
1239 m_preview_win1->showImage(auxImg);
1240 }
1241 else
1242 m_preview_win1->showImage(img4gui);
1243 }
1244
1245 // Right from a stereo pair:
1246 if (m_preview_win2 && m_preview_win2->isOpen() && stObs && stObs->hasImageRight &&
1247 img4gui.getWidth() > 0)
1248 {
1249 // Apply image reduction?
1250 if (m_preview_reduction >= 2)
1251 {
1252 unsigned int w = img4guiR.getWidth();
1253 unsigned int h = img4guiR.getHeight();
1254 CImage auxImg;
1255 img4guiR.scaleImage(
1256 auxImg, w / m_preview_reduction, h / m_preview_reduction, IMG_INTERP_NN);
1257 m_preview_win2->showImage(auxImg);
1258 }
1259 else
1260 m_preview_win2->showImage(img4guiR);
1261 }
1262
1263 // Disparity from a stereo pair:
1264 if (m_preview_win2 && m_preview_win2->isOpen() && stObs && stObs->hasImageDisparity)
1265 {
1266 // Apply image reduction?
1267 if (m_preview_reduction >= 2)
1268 {
1269 unsigned int w = stObs->imageDisparity.getWidth();
1270 unsigned int h = stObs->imageDisparity.getHeight();
1271 CImage auxImg;
1272 stObs->imageDisparity.scaleImage(
1273 auxImg, w / m_preview_reduction, h / m_preview_reduction, IMG_INTERP_NN);
1274 m_preview_win2->showImage(auxImg);
1275 }
1276 else
1277 m_preview_win2->showImage(stObs->imageDisparity);
1278 }
1279 }
1280 } // end show preview
1281
1282 if (!delayed_insertion_in_obs_queue)
1283 {
1284 if (stObs) out_obs.push_back(CObservation::Ptr(stObs));
1285 if (obs) out_obs.push_back(CObservation::Ptr(obs));
1286 if (obs3D) out_obs.push_back(CObservation::Ptr(obs3D));
1287 }
1288 return;
1289 }
1290
1291 /* -----------------------------------------------------
1292 doProcess
1293 ----------------------------------------------------- */
1294 void CCameraSensor::doProcess()
1295 {
1296 vector<CSerializable::Ptr> out_obs;
1297 getNextFrame(out_obs);
1298 appendObservations(out_obs);
1299 }
1300
1301 /* -----------------------------------------------------
1302 setSoftwareTriggerLevel
1303 ----------------------------------------------------- */
1304 void CCameraSensor::setSoftwareTriggerLevel(bool level)
1305 {
1306 if (m_cap_dc1394)
1307 {
1308 if (!m_cap_dc1394->setSoftwareTriggerLevel(level))
1309 { // Error
1310 m_state = CGenericSensor::ssError;
1311 THROW_EXCEPTION("Error setting Trigger level by software");
1312 }
1313 }
1314 else
1315 {
1316 THROW_EXCEPTION("Software trigger is not implemented for this camera type");
1317 }
1318 }
1319
1320 /* -----------------------------------------------------
1321 setPathForExternalImages
1322 ----------------------------------------------------- */
1323 void CCameraSensor::setPathForExternalImages(const std::string& directory)
1324 {
1325 if (!mrpt::system::createDirectory(directory))
1326 {
1327 THROW_EXCEPTION_FMT(
1328 "Cannot create the directory for externally saved images: `%s`", directory.c_str());
1329 }
1330 m_path_for_external_images = directory;
1331 }
1332
1333 /* ------------------------------------------------------------------------
1334 prepareVideoSourceFromUserSelection
1335 ------------------------------------------------------------------------ */
1336 CCameraSensor::Ptr mrpt::hwdrivers::prepareVideoSourceFromUserSelection()
1337 {
1338 #if MRPT_HAS_WXWIDGETS
1339 // Create the main wxThread, if it doesn't exist yet:
1340 if (!mrpt::gui::WxSubsystem::createOneInstanceMainThread())
1341 {
1342 std::cerr << "[mrpt::hwdrivers::prepareVideoSourceFromUserSelection] "
1343 "Error initiating Wx subsystem."
1344 << std::endl;
1345 return CCameraSensor::Ptr(); // Error!
1346 }
1347
1348 std::promise<void> semDlg;
1349 std::promise<mrpt::gui::detail::TReturnAskUserOpenCamera> dlgSelection;
1350
1351 // Create window:
1352 auto* REQ = new WxSubsystem::TRequestToWxMainThread[1];
1353 REQ->OPCODE = 700;
1354 REQ->sourceCameraSelectDialog = true;
1355 REQ->voidPtr = reinterpret_cast<void*>(&semDlg);
1356 REQ->voidPtr2 = reinterpret_cast<void*>(&dlgSelection);
1357 WxSubsystem::pushPendingWxRequest(REQ);
1358
1359 // Wait for the window to realize and signal it's alive:
1360 if (!WxSubsystem::isConsoleApp())
1361 {
1362 std::this_thread::sleep_for(20ms); // Force at least 1-2 timer ticks for processing the event:
1363 wxApp::GetInstance()->Yield(true);
1364 }
1365
1366 // wait for window construction:
1367 int maxTimeout =
1368 #ifdef _DEBUG
1369 30000;
1370 #else
1371 6000;
1372 #endif
1373 // If we have an "MRPT_WXSUBSYS_TIMEOUT_MS" environment variable, use that
1374 // timeout instead:
1375 const char* envVal = getenv("MRPT_WXSUBSYS_TIMEOUT_MS");
1376 if (envVal) maxTimeout = atoi(envVal);
1377
1378 if (semDlg.get_future().wait_for(std::chrono::milliseconds(maxTimeout)) ==
1379 std::future_status::timeout)
1380 {
1381 cerr << "[prepareVideoSourceFromUserSelection] Timeout waiting window "
1382 "creation."
1383 << endl;
1384 return CCameraSensor::Ptr();
1385 }
1386
1387 // wait for user selection:
1388 auto future = dlgSelection.get_future();
1389 future.wait();
1390 const auto& ret = future.get();
1391
1392 // If the user didn't accept the dialog, return now:
1393 if (!ret.accepted_by_user) return CCameraSensor::Ptr();
1394
1395 mrpt::config::CConfigFileMemory selectedConfig(ret.selectedConfig);
1396
1397 CCameraSensor::Ptr cam = std::make_shared<CCameraSensor>();
1398 cam->loadConfig(selectedConfig, "CONFIG");
1399 cam->initialize(); // This will raise an exception if necessary
1400
1401 return cam;
1402 #else
1403 THROW_EXCEPTION("MRPT compiled without wxWidgets");
1404 #endif // MRPT_HAS_WXWIDGETS
1405 }
1406
1407 /* ------------------------------------------------------------------------
1408 prepareVideoSourceFromPanel
1409 ------------------------------------------------------------------------ */
1410 CCameraSensor::Ptr mrpt::hwdrivers::prepareVideoSourceFromPanel(void* _panel)
1411 {
1412 #if MRPT_HAS_WXWIDGETS
1413
1414 try
1415 {
1416 CConfigFileMemory cfg;
1417 writeConfigFromVideoSourcePanel(_panel, "CONFIG", &cfg);
1418
1419 // Try to open the camera:
1420 CCameraSensor::Ptr video = std::make_shared<CCameraSensor>();
1421 video->loadConfig(cfg, "CONFIG");
1422
1423 // This will raise an exception if necessary
1424 video->initialize();
1425
1426 return video;
1427 }
1428 catch (const std::exception& e)
1429 {
1430 cerr << endl << e.what() << endl;
1431 wxMessageBox(_("Couldn't open video source"), _("Error"));
1432 return CCameraSensor::Ptr();
1433 }
1434 #else
1435 THROW_EXCEPTION("MRPT compiled without wxWidgets");
1436 #endif // MRPT_HAS_WXWIDGETS
1437 }
1438
1439 /* ------------------------------------------------------------------------
1440 writeConfigFromVideoSourcePanel
1441 ------------------------------------------------------------------------ */
1442 void mrpt::hwdrivers::writeConfigFromVideoSourcePanel(
1443 void* _panel, const std::string& sect, mrpt::config::CConfigFileBase* cfg)
1444 {
1445 MRPT_START
1446 #if MRPT_HAS_WXWIDGETS
1447 ASSERT_(_panel);
1448 auto* panel = reinterpret_cast<mrpt::gui::CPanelCameraSelection*>(_panel);
1449 ASSERTMSG_(panel, "panel must be of type mrpt::gui::CPanelCameraSelection *");
1450 panel->writeConfigFromVideoSourcePanel(sect, cfg);
1451
1452 #else
1453 THROW_EXCEPTION("MRPT compiled without wxWidgets");
1454 #endif // MRPT_HAS_WXWIDGETS
1455 MRPT_END
1456 }
1457
1458 /* ------------------------------------------------------------------------
1459 readConfigIntoVideoSourcePanel
1460 ------------------------------------------------------------------------ */
1461 void mrpt::hwdrivers::readConfigIntoVideoSourcePanel(
1462 void* _panel, const std::string& sect, const mrpt::config::CConfigFileBase* cfg)
1463 {
1464 MRPT_START
1465 #if MRPT_HAS_WXWIDGETS
1466 ASSERT_(_panel);
1467 auto* panel = reinterpret_cast<mrpt::gui::CPanelCameraSelection*>(_panel);
1468 ASSERTMSG_(panel, "panel must be of type mrpt::gui::CPanelCameraSelection *");
1469
1470 panel->readConfigIntoVideoSourcePanel(sect, cfg);
1471
1472 #else
1473 THROW_EXCEPTION("MRPT compiled without wxWidgets");
1474 #endif // MRPT_HAS_WXWIDGETS
1475 MRPT_END
1476 }
1477
1478 /* -----------------------------------------------------
1479 THREAD: Saver of external images
1480 ----------------------------------------------------- */
1481 void CCameraSensor::thread_save_images(unsigned int my_working_thread_index)
1482 {
1483 while (!m_threadImagesSaverShouldEnd)
1484 {
1485 TListObservations newObs;
1486
1487 // is there any new image?
1488 m_csToSaveList.lock();
1489 m_toSaveList[my_working_thread_index].swap(newObs);
1490 m_csToSaveList.unlock();
1491
1492 for (auto i = newObs.begin(); i != newObs.end(); ++i)
1493 {
1494 // Optional user-code hook:
1495 if (m_hook_pre_save)
1496 {
1497 if (IS_DERIVED(*i->second, CObservation))
1498 {
1499 mrpt::obs::CObservation::Ptr obs =
1500 std::dynamic_pointer_cast<mrpt::obs::CObservation>(i->second);
1501 m_hook_pre_save(obs, m_hook_pre_save_param);
1502 }
1503 }
1504
1505 if (IS_CLASS(*i->second, CObservationImage))
1506 {
1507 CObservationImage::Ptr obs = std::dynamic_pointer_cast<CObservationImage>(i->second);
1508
1509 string filName = fileNameStripInvalidChars(trim(m_sensorLabel)) +
1510 mrpt::format(
1511 "_%f.%s", (double)mrpt::Clock::toDouble(obs->timestamp),
1512 m_external_images_format.c_str());
1513
1514 if (!obs->image.saveToFile(
1515 m_path_for_external_images + string("/") + filName, m_external_images_jpeg_quality))
1516 {
1517 THROW_EXCEPTION("Error saving externally-stored images");
1518 }
1519
1520 obs->image.setExternalStorage(filName);
1521 }
1522 else if (IS_CLASS(*i->second, CObservationStereoImages))
1523 {
1524 CObservationStereoImages::Ptr stObs =
1525 std::dynamic_pointer_cast<CObservationStereoImages>(i->second);
1526
1527 const string filNameL = fileNameStripInvalidChars(trim(m_sensorLabel)) +
1528 mrpt::format(
1529 "_L_%f.%s", (double)mrpt::Clock::toDouble(stObs->timestamp),
1530 m_external_images_format.c_str());
1531 const string filNameR = fileNameStripInvalidChars(trim(m_sensorLabel)) +
1532 mrpt::format(
1533 "_R_%f.%s", (double)mrpt::Clock::toDouble(stObs->timestamp),
1534 m_external_images_format.c_str());
1535 const string filNameD = fileNameStripInvalidChars(trim(m_sensorLabel)) +
1536 mrpt::format(
1537 "_D_%f.%s", (double)mrpt::Clock::toDouble(stObs->timestamp),
1538 m_external_images_format.c_str());
1539
1540 if (!stObs->imageLeft.saveToFile(
1541 m_path_for_external_images + string("/") + filNameL,
1542 m_external_images_jpeg_quality))
1543 {
1544 THROW_EXCEPTION("Error saving externally-stored images");
1545 }
1546
1547 stObs->imageLeft.setExternalStorage(filNameL);
1548
1549 if (stObs->hasImageRight)
1550 {
1551 if (!stObs->imageRight.saveToFile(
1552 m_path_for_external_images + string("/") + filNameR,
1553 m_external_images_jpeg_quality))
1554 {
1555 THROW_EXCEPTION("Error saving externally-stored images");
1556 }
1557
1558 stObs->imageRight.setExternalStorage(filNameR);
1559 }
1560 if (stObs->hasImageDisparity)
1561 {
1562 if (!stObs->imageDisparity.saveToFile(
1563 m_path_for_external_images + string("/") + filNameD,
1564 m_external_images_jpeg_quality))
1565 {
1566 THROW_EXCEPTION("Error saving externally-stored images");
1567 }
1568
1569 stObs->imageDisparity.setExternalStorage(filNameD);
1570 }
1571 }
1572
1573 // Append now:
1574 appendObservation(i->second);
1575 }
1576
1577 std::this_thread::sleep_for(2ms);
1578 }
1579 }