Debian Codegraph @

libs/hwdrivers/src/CImageGrabber_FlyCapture2.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/hwdrivers/CImageGrabber_FlyCapture2.h>
13 #include <mrpt/system/datetime.h>
14 #include <mrpt/system/string_utils.h>
15
16 #if MRPT_HAS_FLYCAPTURE2
17 #include <FlyCapture2.h>
18 using namespace FlyCapture2;
19 #endif
20 #if MRPT_HAS_TRICLOPS
21 #include <fc2triclops.h>
22 #include <triclops.h>
23 using namespace Fc2Triclops;
24 #endif
25
26 #if MRPT_HAS_OPENCV
27 #include <opencv2/imgproc/imgproc_c.h>
28
29 #include <opencv2/core/core.hpp>
30 #include <opencv2/highgui/highgui.hpp>
31 #include <opencv2/imgproc/imgproc.hpp>
32 #endif
33
34 #define CHECK_FC2_ERROR(_err) \
35 { \
36 if (_err != PGRERROR_OK) \
37 { \
38 THROW_EXCEPTION_FMT("FlyCapture2 error:\n%s", _err.GetDescription()); \
39 } \
40 }
41 #define CHECK_TRICLOPS_ERROR(_err) \
42 { \
43 if (_err != TriclopsErrorOk) \
44 { \
45 THROW_EXCEPTION_FMT("Triclops Error:\n'%s'", triclopsErrorToString(_err)); \
46 } \
47 }
48 #define FC2_CAM reinterpret_cast<FlyCapture2::Camera*>(m_camera)
49 #define FC2_CAM_INFO reinterpret_cast<FlyCapture2::CameraInfo*>(m_camera_info)
50 #define FC2_BUF_IMG reinterpret_cast<FlyCapture2::Image*>(m_img_buffer)
51 #define TRI_CONTEXT reinterpret_cast<TriclopsContext*>(m_triclops)
52
53 using namespace mrpt::hwdrivers;
54 using namespace std;
55
56 #if MRPT_HAS_FLYCAPTURE2
57 // Declare tables to convert strings to their #define values:
58 template <typename T>
59 struct fc2_str_val
60 {
61 const char* str;
62 T val;
63 };
64
65 const fc2_str_val<VideoMode> fc2_VideoMode_table[] = {
66 { "VIDEOMODE_160x120YUV444", VIDEOMODE_160x120YUV444},
67 { "VIDEOMODE_320x240YUV422", VIDEOMODE_320x240YUV422},
68 { "VIDEOMODE_640x480YUV411", VIDEOMODE_640x480YUV411},
69 { "VIDEOMODE_640x480YUV422", VIDEOMODE_640x480YUV422},
70 { "VIDEOMODE_640x480RGB", VIDEOMODE_640x480RGB},
71 { "VIDEOMODE_640x480Y8", VIDEOMODE_640x480Y8},
72 { "VIDEOMODE_640x480Y16", VIDEOMODE_640x480Y16},
73 { "VIDEOMODE_800x600YUV422", VIDEOMODE_800x600YUV422},
74 { "VIDEOMODE_800x600RGB", VIDEOMODE_800x600RGB},
75 { "VIDEOMODE_800x600Y8", VIDEOMODE_800x600Y8},
76 { "VIDEOMODE_800x600Y16", VIDEOMODE_800x600Y16},
77 { "VIDEOMODE_1024x768YUV422", VIDEOMODE_1024x768YUV422},
78 { "VIDEOMODE_1024x768RGB", VIDEOMODE_1024x768RGB},
79 { "VIDEOMODE_1024x768Y8", VIDEOMODE_1024x768Y8},
80 { "VIDEOMODE_1024x768Y16", VIDEOMODE_1024x768Y16},
81 { "VIDEOMODE_1280x960YUV422", VIDEOMODE_1280x960YUV422},
82 { "VIDEOMODE_1280x960RGB", VIDEOMODE_1280x960RGB},
83 { "VIDEOMODE_1280x960Y8", VIDEOMODE_1280x960Y8},
84 { "VIDEOMODE_1280x960Y16", VIDEOMODE_1280x960Y16},
85 {"VIDEOMODE_1600x1200YUV422", VIDEOMODE_1600x1200YUV422},
86 { "VIDEOMODE_1600x1200RGB", VIDEOMODE_1600x1200RGB},
87 { "VIDEOMODE_1600x1200Y8", VIDEOMODE_1600x1200Y8},
88 { "VIDEOMODE_1600x1200Y16", VIDEOMODE_1600x1200Y16},
89 { "VIDEOMODE_FORMAT7", VIDEOMODE_FORMAT7}
90 };
91 fc2_str_val<FrameRate> fc2_FrameRate_table[] = {
92 { "FRAMERATE_1_875", FlyCapture2::FRAMERATE_1_875},
93 { "FRAMERATE_3_75", FlyCapture2::FRAMERATE_3_75},
94 { "FRAMERATE_7_5", FlyCapture2::FRAMERATE_7_5},
95 { "FRAMERATE_15", FlyCapture2::FRAMERATE_15},
96 { "FRAMERATE_30", FlyCapture2::FRAMERATE_30},
97 { "FRAMERATE_60", FlyCapture2::FRAMERATE_60},
98 { "FRAMERATE_120", FlyCapture2::FRAMERATE_120},
99 { "FRAMERATE_240", FlyCapture2::FRAMERATE_240},
100 {"FRAMERATE_FORMAT7", FlyCapture2::FRAMERATE_FORMAT7}
101 };
102 fc2_str_val<GrabMode> fc2_GrabMode_table[] = {
103 { "DROP_FRAMES", DROP_FRAMES},
104 {"BUFFER_FRAMES", BUFFER_FRAMES}
105 };
106
107 #define GET_CONV_TABLE(type) \
108 vector<fc2_str_val<type>> fc2_vals_gen(type) \
109 { \
110 size_t n = sizeof(fc2_##type##_table) / sizeof(fc2_##type##_table[0]); \
111 vector<fc2_str_val<type>> vec(&fc2_##type##_table[0], &fc2_##type##_table[n]); \
112 return vec; \
113 }
114 GET_CONV_TABLE(VideoMode)
115 GET_CONV_TABLE(FrameRate)
116 GET_CONV_TABLE(GrabMode)
117
118 template <typename T>
119 T fc2_defstr2num(const std::string& str)
120 {
121 vector<fc2_str_val<T>> fc2_vals = fc2_vals_gen(T());
122 const std::string s = mrpt::system::trim(str);
123 for (size_t i = 0; i < fc2_vals.size(); i++)
124 {
125 if (mrpt::system::strCmpI(fc2_vals[i].str, s.c_str())) return fc2_vals[i].val;
126 }
127 THROW_EXCEPTION_FMT("Error: Unknown FlyCapture2 constant: %s", s.c_str());
128 }
129
130 template <typename T>
131 const char* fc2_defnum2str(const T& val)
132 {
133 vector<fc2_str_val<T>> fc2_vals = fc2_vals_gen(T());
134 size_t i = static_cast<int>(val);
135 if (i < fc2_vals.size())
136 return fc2_vals[i].str;
137 else
138 THROW_EXCEPTION_FMT("Error: Unknown FlyCapture2 enum: %i", static_cast<int>(val));
139 }
140 #endif
141
142 TCaptureOptions_FlyCapture2::TCaptureOptions_FlyCapture2() :
143 videomode(), //("VIDEOMODE_640x480Y8"),
144 framerate(), // ("FRAMERATE_30"),
145 grabmode("BUFFER_FRAMES")
146
147 {
148 memset(camera_guid, 0, 4 * sizeof(camera_guid[0]));
149 }
150
151 void TCaptureOptions_FlyCapture2::loadOptionsFrom(
152 const mrpt::config::CConfigFileBase& cfg, const std::string& sect, const std::string& prefix)
153 {
154 camera_index = cfg.read_int(sect, prefix + string("camera_index"), camera_index);
155 open_by_guid = cfg.read_bool(sect, prefix + string("open_by_guid"), open_by_guid);
156
157 if (open_by_guid)
158 {
159 string sGUID = cfg.read_string(sect, prefix + string("camera_guid"), "", true);
160 vector<string> sGUIDparts;
161 mrpt::system::tokenize(sGUID, "- \t\r\n", sGUIDparts);
162 ASSERTMSG_(
163 sGUIDparts.size() == 4, "GUID format error: must have four blocks like XXX-XXX-XXX-XXX");
164
165 for (int i = 0; i < 4; i++) sscanf(sGUIDparts[i].c_str(), "%X", &camera_guid[i]);
166 }
167
168 videomode = cfg.read_string(sect, prefix + string("videomode"), videomode);
169 framerate = cfg.read_string(sect, prefix + string("framerate"), framerate);
170 grabmode = cfg.read_string(sect, prefix + string("grabmode"), grabmode);
171 numBuffers = cfg.read_uint64_t(sect, prefix + string("numBuffers"), numBuffers);
172 grabTimeout = cfg.read_int(sect, prefix + string("grabTimeout"), grabTimeout);
173
174 trigger_enabled = cfg.read_bool(sect, prefix + string("trigger_enabled"), trigger_enabled);
175 trigger_polarity = cfg.read_int(sect, prefix + string("trigger_polarity"), trigger_polarity);
176 trigger_source = cfg.read_int(sect, prefix + string("trigger_source"), trigger_source);
177 trigger_mode = cfg.read_int(sect, prefix + string("trigger_mode"), trigger_mode);
178
179 strobe_enabled = cfg.read_bool(sect, prefix + string("strobe_enabled"), strobe_enabled);
180 strobe_source = cfg.read_int(sect, prefix + string("strobe_source"), strobe_source);
181 strobe_polarity = cfg.read_int(sect, prefix + string("strobe_polarity"), strobe_polarity);
182 strobe_delay = cfg.read_float(sect, prefix + string("strobe_delay"), strobe_delay);
183 strobe_duration = cfg.read_float(sect, prefix + string("strobe_duration"), strobe_duration);
184
185 autoexposure_auto = cfg.read_bool(sect, prefix + string("autoexposure_auto"), autoexposure_auto);
186 autoexposure_onOff =
187 cfg.read_bool(sect, prefix + string("autoexposure_onOFf"), autoexposure_onOff);
188 autoexposure_abs = cfg.read_bool(sect, prefix + string("autoexposure_abs"), autoexposure_abs);
189 autoexposure_EV = cfg.read_float(sect, prefix + string("autoexposure_EV"), autoexposure_EV);
190
191 shutter_auto = cfg.read_bool(sect, prefix + string("shutter_auto"), shutter_auto);
192 shutter_abs = cfg.read_bool(sect, prefix + string("shutter_abs"), shutter_abs);
193 shutter_time_ms = cfg.read_float(sect, prefix + string("shutter_time_ms"), shutter_time_ms);
194
195 gain_auto = cfg.read_bool(sect, prefix + string("gain_auto"), gain_auto);
196 gain_abs = cfg.read_bool(sect, prefix + string("gain_abs"), gain_abs);
197 gain_dB = cfg.read_float(sect, prefix + string("gain_dB"), gain_dB);
198
199 stereo_mode = cfg.read_bool(sect, prefix + string("stereo_mode"), stereo_mode);
200 rect_width = cfg.read_uint64_t(sect, prefix + string("rect_width"), rect_width);
201 rect_height = cfg.read_uint64_t(sect, prefix + string("rect_height"), rect_height);
202 }
203
204 // ---------------------------------------------------------------
205 /** Default constructor */
206 CImageGrabber_FlyCapture2::CImageGrabber_FlyCapture2()
207 {
208 #if MRPT_HAS_FLYCAPTURE2
209 m_img_buffer = new FlyCapture2::Image();
210 #endif
211 }
212
213 /** Constructor + open */
214 CImageGrabber_FlyCapture2::CImageGrabber_FlyCapture2(const TCaptureOptions_FlyCapture2& options) :
215 m_camera(nullptr), m_camera_info(nullptr), m_img_buffer(nullptr), m_triclops(nullptr)
216 {
217 #if MRPT_HAS_FLYCAPTURE2
218 m_img_buffer = new FlyCapture2::Image();
219 #endif
220 this->open(options);
221 }
222
223 /** Destructor */
224 CImageGrabber_FlyCapture2::~CImageGrabber_FlyCapture2()
225 {
226 #if MRPT_HAS_FLYCAPTURE2
227 this->close();
228 delete FC2_BUF_IMG;
229 m_img_buffer = nullptr;
230 #endif
231 }
232
233 /** Tries to open the camera with the given options. Raises an exception on
234 * error. \sa close() */
235 void CImageGrabber_FlyCapture2::open(
236 const TCaptureOptions_FlyCapture2& options, const bool startCapture)
237 {
238 #if MRPT_HAS_FLYCAPTURE2
239 FlyCapture2::Error fe;
240
241 cout << "[CImageGrabber_FlyCapture2::open] FlyCapture2 version: "
242 << CImageGrabber_FlyCapture2::getFC2version() << std::endl;
243
244 this->close();
245 this->m_options = options;
246
247 // Determine camera to open:
248 // -----------------------------------
249 PGRGuid guid;
250 if (m_options.open_by_guid)
251 {
252 // Open by GUID:
253 for (int i = 0; i < 4; i++) guid.value[i] = m_options.camera_guid[i];
254 }
255 else
256 {
257 // Open by camera index:
258 BusManager busMgr;
259 unsigned int numCameras;
260 fe = busMgr.GetNumOfCameras(&numCameras);
261 CHECK_FC2_ERROR(fe)
262
263 if (m_options.camera_index >= numCameras)
264 THROW_EXCEPTION(mrpt::format(
265 "Error: camera_index to open is '%u', but only '%u' "
266 "cameras were detected in the system.",
267 m_options.camera_index, numCameras));
268
269 fe = busMgr.GetCameraFromIndex(m_options.camera_index, &guid);
270 CHECK_FC2_ERROR(fe)
271 }
272
273 // Connect to camera:
274 m_camera = new FlyCapture2::Camera();
275 m_camera_info = new FlyCapture2::CameraInfo();
276
277 cout << mrpt::format(
278 "[CImageGrabber_FlyCapture2::open] Opening camera with GUID= "
279 "%08X-%08X-%08X-%08X...\n",
280 guid.value[0], guid.value[1], guid.value[2], guid.value[3]);
281 fe = FC2_CAM->Connect(&guid);
282 CHECK_FC2_ERROR(fe)
283 fe = FC2_CAM->GetCameraInfo(FC2_CAM_INFO);
284 CHECK_FC2_ERROR(fe)
285
286 const FlyCapture2::CameraInfo* ci = FC2_CAM_INFO;
287
288 cout << mrpt::format(
289 "[CImageGrabber_FlyCapture2::open] Camera connected ok:\n"
290 " Serial number - %u\n"
291 " Camera model - %s\n"
292 " Camera vendor - %s\n"
293 " Sensor - %s\n"
294 " Resolution - %s\n"
295 " Firmware version - %s\n"
296 " Firmware build time - %s\n\n",
297 ci->serialNumber, ci->modelName, ci->vendorName, ci->sensorInfo, ci->sensorResolution,
298 ci->firmwareVersion, ci->firmwareBuildTime);
299
300 // Set camera config:
301 if (!m_options.videomode.empty() && !m_options.framerate.empty())
302 {
303 bool isSupported = false;
304
305 if (!m_options.stereo_mode)
306 {
307 FlyCapture2::VideoMode vidMode = fc2_defstr2num<FlyCapture2::VideoMode>(m_options.videomode);
308 FlyCapture2::FrameRate vidRate = fc2_defstr2num<FlyCapture2::FrameRate>(m_options.framerate);
309
310 fe = FC2_CAM->GetVideoModeAndFrameRateInfo(vidMode, vidRate, &isSupported);
311 CHECK_FC2_ERROR(fe)
312
313 if (!isSupported)
314 {
315 FlyCapture2::VideoMode curVidMode;
316 FlyCapture2::FrameRate curVidRate;
317 fe = FC2_CAM->GetVideoModeAndFrameRate(&curVidMode, &curVidRate);
318
319 THROW_EXCEPTION(mrpt::format(
320 "Camera mode '%s' + '%s' is not supported by this "
321 "camera. Current mode is %d, current rate is %d.",
322 m_options.videomode.c_str(), m_options.framerate.c_str(), static_cast<int>(curVidMode),
323 static_cast<int>(curVidRate)));
324 }
325
326 fe = FC2_CAM->SetVideoModeAndFrameRate(vidMode, vidRate);
327 CHECK_FC2_ERROR(fe)
328 }
329 else
330 {
331 #if MRPT_HAS_TRICLOPS
332 Fc2Triclops::ErrorType fte;
333 // Configure camera for Stereo mode
334 StereoCameraMode mode = TWO_CAMERA;
335 fte = setStereoMode(*(FC2_CAM), mode);
336 if (fte) handleFc2TriclopsError(fte, "setStereoMode");
337
338 // Generate Triclops context
339
340 m_triclops = new TriclopsContext;
341 fte = getContextFromCamera(FC2_CAM_INFO->serialNumber, TRI_CONTEXT);
342 if (fte != ERRORTYPE_OK) handleFc2TriclopsError(fte, "getContextFromCamera");
343
344 // ------------------------------------------------------
345 // TRICLOPS CONFIGURATION
346 // ------------------------------------------------------
347 // Current Format7 settings
348 /*
349 Format7ImageSettings f7settings;
350 unsigned int f7PacketSize;
351 float f7Percentage;
352 fe = FC2_CAM->GetFormat7Configuration(&f7settings, &f7PacketSize,
353 &f7Percentage);
354 CHECK_FC2_ERROR(fe)
355 */
356
357 TriclopsError te;
358 // Set rectified resolution
359 te = triclopsSetResolution(*(TRI_CONTEXT), m_options.rect_height, m_options.rect_width);
360 CHECK_TRICLOPS_ERROR(te);
361 // Retrieve camera parameters
362 te = triclopsGetBaseline(*(TRI_CONTEXT), &m_baseline);
363 CHECK_TRICLOPS_ERROR(te);
364 te = triclopsGetFocalLength(*(TRI_CONTEXT), &m_focalLength);
365 CHECK_TRICLOPS_ERROR(te);
366 te = triclopsGetImageCenter(*(TRI_CONTEXT), &m_centerRow, &m_centerCol);
367 CHECK_TRICLOPS_ERROR(te);
368 #else
369 THROW_EXCEPTION("MRPT compiled without support for Triclops");
370 #endif
371 }
372 }
373
374 {
375 FlyCapture2::VideoMode curVidMode;
376 FlyCapture2::FrameRate curVidRate;
377 fe = FC2_CAM->GetVideoModeAndFrameRate(&curVidMode, &curVidRate);
378 if (fe == PGRERROR_OK)
379 cout << mrpt::format(
380 "[CImageGrabber_FlyCapture2::open] Current camera mode is %s, "
381 "current rate is %s.\n",
382 fc2_defnum2str<FlyCapture2::VideoMode>(curVidMode),
383 fc2_defnum2str<FlyCapture2::FrameRate>(curVidRate));
384 }
385
386 // Set trigger:
387 FlyCapture2::TriggerModeInfo trigInfo;
388 FC2_CAM->GetTriggerModeInfo(&trigInfo);
389
390 FlyCapture2::TriggerMode trig;
391 trig.onOff = m_options.trigger_enabled;
392 if (m_options.trigger_enabled)
393 {
394 trig.mode = m_options.trigger_mode;
395 trig.polarity = m_options.trigger_polarity;
396 trig.source = m_options.trigger_source;
397 }
398 fe = FC2_CAM->SetTriggerMode(&trig);
399 CHECK_FC2_ERROR(fe)
400
401 // Strobe:
402 if (m_options.strobe_enabled)
403 {
404 FlyCapture2::StrobeControl strobe;
405
406 strobe.onOff = m_options.strobe_enabled;
407 strobe.delay = m_options.strobe_delay;
408 strobe.duration = m_options.strobe_duration;
409 strobe.polarity = m_options.strobe_polarity;
410 strobe.source = m_options.strobe_source;
411
412 fe = FC2_CAM->SetStrobe(&strobe);
413 CHECK_FC2_ERROR(fe)
414 }
415
416 // Set configs:
417 FlyCapture2::FC2Config fc2conf;
418 FC2_CAM->GetConfiguration(&fc2conf);
419 CHECK_FC2_ERROR(fe)
420
421 fc2conf.grabMode = fc2_defstr2num<FlyCapture2::GrabMode>(m_options.grabmode);
422 if (m_options.grabTimeout >= 0) fc2conf.grabTimeout = m_options.grabTimeout;
423
424 fc2conf.numBuffers = m_options.numBuffers;
425
426 fe = FC2_CAM->SetConfiguration(&fc2conf);
427 CHECK_FC2_ERROR(fe)
428
429 // Autoexposure:
430 {
431 FlyCapture2::Property p;
432 p.type = FlyCapture2::AUTO_EXPOSURE;
433 FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
434 CHECK_FC2_ERROR(error)
435 p.autoManualMode = m_options.autoexposure_auto; // true=auto
436 p.onOff = m_options.autoexposure_onOff; // true=on
437 p.absControl = m_options.autoexposure_abs; // true=abs
438 p.absValue = m_options.autoexposure_EV; // abs value in Exposure Value (EV)
439 fe = FC2_CAM->SetProperty(&p);
440 CHECK_FC2_ERROR(fe)
441 }
442
443 // Brightness:
444 {
445 FlyCapture2::Property p;
446 p.type = FlyCapture2::BRIGHTNESS;
447 FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
448 CHECK_FC2_ERROR(error)
449 p.autoManualMode = true; // true=auto
450 // p.absControl = true;
451 // p.absValue = Brightness;
452 fe = FC2_CAM->SetProperty(&p);
453 CHECK_FC2_ERROR(fe)
454 }
455
456 // Shutter:
457 {
458 FlyCapture2::Property p;
459 p.type = FlyCapture2::SHUTTER;
460 FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
461 CHECK_FC2_ERROR(error)
462 p.autoManualMode = m_options.shutter_auto; // true=auto
463 p.absControl = m_options.shutter_abs; // true=abs
464 p.absValue = m_options.shutter_time_ms;
465 // p.onOff = false;
466 fe = FC2_CAM->SetProperty(&p);
467 CHECK_FC2_ERROR(fe)
468 }
469
470 // Gain:
471 {
472 FlyCapture2::Property p;
473 p.type = FlyCapture2::GAIN;
474 FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
475 CHECK_FC2_ERROR(error)
476 p.autoManualMode = m_options.gain_auto; // true=auto
477 p.absControl = m_options.gain_abs; // true=abs
478 p.absValue = m_options.gain_dB; // abs value in dB (decibeles)
479 // p.onOff = false;
480 fe = FC2_CAM->SetProperty(&p);
481 CHECK_FC2_ERROR(fe)
482 }
483
484 // Framecounter:
485 EmbeddedImageInfo eii;
486 fe = FC2_CAM->GetEmbeddedImageInfo(&eii);
487 if (fe == PGRERROR_OK)
488 {
489 if (eii.frameCounter.available) eii.frameCounter.onOff = true;
490 if (eii.timestamp.available) eii.timestamp.onOff = true;
491 if (eii.exposure.available) eii.exposure.onOff = true;
492 if (eii.brightness.available) eii.brightness.onOff = true;
493
494 // Enable all:
495 FC2_CAM->SetEmbeddedImageInfo(&eii);
496 }
497
498 // Start:
499 if (startCapture) this->startCapture();
500 #else
501 THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
502 #endif
503 }
504
505 /** Start the actual image capture of the camera. Must be called after open(),
506 * only when "startCapture" was set to false. */
507 void CImageGrabber_FlyCapture2::startCapture()
508 {
509 #if MRPT_HAS_FLYCAPTURE2
510 if (!m_camera)
511 {
512 THROW_EXCEPTION("Camera is not opened. Call open() first.");
513 }
514
515 FlyCapture2::Error error = FC2_CAM->StartCapture();
516 CHECK_FC2_ERROR(error)
517
518 #else
519 THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
520 #endif
521 }
522
523 /** Starts a synchronous capture of several cameras, which must have been
524 * already opened. */
525 void CImageGrabber_FlyCapture2::startSyncCapture(
526 int numCameras, const CImageGrabber_FlyCapture2** cameras_array)
527 {
528 #if MRPT_HAS_FLYCAPTURE2
529
530 std::vector<const FlyCapture2::Camera*> cam_ptrs(numCameras);
531
532 for (int i = 0; i < numCameras; i++)
533 {
534 const CImageGrabber_FlyCapture2* obj = cameras_array[i];
535 if (!obj->m_camera)
536 {
537 THROW_EXCEPTION_FMT("Camera #%i in list is not opened. Call open() first.", i);
538 }
539
540 FlyCapture2::Camera* cam = reinterpret_cast<FlyCapture2::Camera*>(obj->m_camera);
541 cam_ptrs[i] = cam;
542 }
543
544 if (!cam_ptrs.empty())
545 {
546 FlyCapture2::Error error = FlyCapture2::Camera::StartSyncCapture(cam_ptrs.size(), &cam_ptrs[0]);
547 CHECK_FC2_ERROR(error)
548 }
549 #else
550 THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
551 #endif
552 }
553
554 /** Stop capture. */
555 void CImageGrabber_FlyCapture2::stopCapture()
556 {
557 #if MRPT_HAS_FLYCAPTURE2
558 if (m_camera)
559 {
560 Error error;
561
562 // Stop grabbing:
563 error = FC2_CAM->StopCapture();
564 CHECK_FC2_ERROR(error)
565 }
566 #else
567 THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
568 #endif
569 }
570
571 /** Closes the opened camera, if any. Called automatically on object
572 * destruction. */
573 void CImageGrabber_FlyCapture2::close()
574 {
575 #if MRPT_HAS_FLYCAPTURE2
576 try
577 {
578 this->stopCapture();
579 }
580 catch (...)
581 {
582 }
583
584 // Disconnect the camera
585 try
586 {
587 if (m_camera) FC2_CAM->Disconnect();
588 }
589 catch (...)
590 {
591 }
592
593 // Delete objects:
594 try
595 {
596 if (m_camera) delete FC2_CAM;
597 }
598 catch (...)
599 {
600 }
601 try
602 {
603 if (m_camera_info) delete FC2_CAM_INFO;
604 }
605 catch (...)
606 {
607 }
608 #if MRPT_HAS_TRICLOPS
609 try
610 {
611 if (m_triclops) delete TRI_CONTEXT;
612 }
613 catch (...)
614 {
615 }
616 #endif
617
618 m_camera = nullptr;
619 m_camera_info = nullptr;
620
621 #else
622 THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
623 #endif
624 }
625
626 /** Returns the PGR FlyCapture2 library version */
627 std::string CImageGrabber_FlyCapture2::getFC2version()
628 {
629 #if MRPT_HAS_FLYCAPTURE2
630 FlyCapture2::FC2Version fc2Version;
631 FlyCapture2::Utilities::GetLibraryVersion(&fc2Version);
632 return mrpt::format(
633 "%d.%d.%d.%d", fc2Version.major, fc2Version.minor, fc2Version.type, fc2Version.build);
634 #else
635 THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
636 #endif
637 }
638
639 /*-------------------------------------------------------------
640 get the image - MONO
641 -------------------------------------------------------------*/
642 // Grab image from the camera. This method blocks until the next frame is
643 // captured.
644 // return: false on any error.
645 bool CImageGrabber_FlyCapture2::getObservation(mrpt::obs::CObservationImage& out_observation)
646 {
647 #if MRPT_HAS_FLYCAPTURE2
648 if (!m_camera)
649 {
650 std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Camera is "
651 "not opened. Call open() first.\n";
652 return false;
653 }
654 try
655 {
656 FlyCapture2::Error error;
657 FlyCapture2::Image image;
658 error = FC2_CAM->RetrieveBuffer(&image);
659 CHECK_FC2_ERROR(error)
660
661 FlyCapture2::TimeStamp timestamp = image.GetTimeStamp();
662 // White balance, etc.
663 // FlyCapture2::ImageMetadata imd = image.GetMetadata();
664 // Determine if it's B/W or color:
665 FlyCapture2::PixelFormat pf = image.GetPixelFormat();
666 const bool is_color =
667 pf == PIXEL_FORMAT_RGB8 || pf == PIXEL_FORMAT_RGB16 || pf == PIXEL_FORMAT_S_RGB16 ||
668 pf == PIXEL_FORMAT_RAW8 || pf == PIXEL_FORMAT_RAW16 || pf == PIXEL_FORMAT_RAW12 ||
669 pf == PIXEL_FORMAT_BGR || pf == PIXEL_FORMAT_BGRU || pf == PIXEL_FORMAT_RGBU ||
670 pf == PIXEL_FORMAT_BGR16 || pf == PIXEL_FORMAT_BGRU16 || pf == PIXEL_FORMAT_422YUV8_JPEG;
671 // Decode image:
672 error = image.Convert(is_color ? PIXEL_FORMAT_BGR : PIXEL_FORMAT_MONO8, FC2_BUF_IMG);
673 CHECK_FC2_ERROR(error)
674 // Convert PGR FlyCapture2 image ==> OpenCV format:
675 unsigned int img_rows, img_cols, img_stride;
676 FC2_BUF_IMG->GetDimensions(&img_rows, &img_cols, &img_stride);
677 out_observation.image.loadFromMemoryBuffer(
678 img_cols, img_rows, is_color, FC2_BUF_IMG->GetData());
679 // It seems timestamp is not always correctly filled in the incoming
680 // imgs:
681 if (timestamp.seconds != 0)
682 out_observation.timestamp =
683 mrpt::Clock::fromDouble(timestamp.seconds + 1e-6 * timestamp.microSeconds);
684 else
685 out_observation.timestamp = mrpt::Clock::now();
686 return true;
687 }
688 catch (const std::exception& e)
689 {
690 std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Error:\n" << e.what() << std::endl;
691 return false;
692 }
693 #else
694 THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
695 #endif
696 }
697
698 /*-------------------------------------------------------------
699 get the image - STEREO
700 -------------------------------------------------------------*/
701 // Grab image from the camera. This method blocks until the next frame is
702 // captured.
703 // return: false on any error.
704 bool CImageGrabber_FlyCapture2::getObservation(mrpt::obs::CObservationStereoImages& out_observation)
705 {
706 #if MRPT_HAS_FLYCAPTURE2 && MRPT_HAS_TRICLOPS && MRPT_HAS_OPENCV
707 if (!m_camera)
708 {
709 std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Camera is "
710 "not opened. Call open() first.\n";
711 return false;
712 }
713
714 try
715 {
716 FlyCapture2::Error ferr;
717 Fc2Triclops::ErrorType fterr;
718 FlyCapture2::Image image;
719 ferr = FC2_CAM->RetrieveBuffer(&image);
720 CHECK_FC2_ERROR(ferr)
721 mrpt::system::TTimeStamp ts_retrieved = mrpt::Clock::now();
722 FlyCapture2::TimeStamp timestamp = image.GetTimeStamp();
723
724 // White balance, etc.
725 // FlyCapture2::ImageMetadata imd = image.GetMetadata();
726
727 // ------------------------------------------
728 // Extract images from common interleaved image:
729 // ------------------------------------------
730 FlyCapture2::Image rawImage[2];
731
732 // Convert the pixel interleaved raw data to de-interleaved raw data
733 fterr = Fc2Triclops::unpackUnprocessedRawOrMono16Image(
734 image, true,
735 rawImage[0], // Right image
736 rawImage[1]); // Left image
737 if (fterr != Fc2Triclops::ERRORTYPE_OK)
738 {
739 Fc2Triclops::handleFc2TriclopsError(fterr, "unprocessedRawOrMono16Image()");
740 return false;
741 }
742
743 // Convert each raw image to RGBU image (for color images)
744 unsigned int img_rows, img_cols, img_stride;
745 for (int i = 0; i < 2; ++i)
746 {
747 FlyCapture2::Image rgbuImage;
748 ferr = rawImage[i].SetColorProcessing(FlyCapture2::HQ_LINEAR);
749 CHECK_FC2_ERROR(ferr)
750 ferr = rawImage[i].Convert(PIXEL_FORMAT_BGR, &rgbuImage);
751 CHECK_FC2_ERROR(ferr)
752
753 // get unrectified images:
754 rgbuImage.GetDimensions(&img_rows, &img_cols, &img_stride);
755 uint8_t* ptrData = rgbuImage.GetData();
756
757 // Convert PGR image ==> OpenCV format:
758 auto tmpImg = cv::Mat(img_rows, img_cols, CV_8UC3, ptrData, img_stride);
759
760 // Fill output stereo observation
761 auto& im = (i == 0 ? out_observation.imageRight : out_observation.imageLeft);
762 im = mrpt::img::CImage(tmpImg, mrpt::img::DEEP_COPY);
763 }
764
765 out_observation.hasImageRight = true;
766
767 // It seems timestamp is not always correctly filled in the incoming
768 // imgs:
769 if (timestamp.seconds != 0)
770 out_observation.timestamp =
771 mrpt::Clock::fromDouble(timestamp.seconds + 1e-6 * timestamp.microSeconds);
772 else
773 out_observation.timestamp = ts_retrieved;
774
775 out_observation.rightCameraPose.x(m_baseline);
776 out_observation.rightCameraPose.y(0);
777 out_observation.rightCameraPose.z(0);
778
779 out_observation.rightCameraPose.quat().r(1);
780 out_observation.rightCameraPose.quat().x(0);
781 out_observation.rightCameraPose.quat().y(0);
782 out_observation.rightCameraPose.quat().z(0);
783
784 out_observation.cameraPose.x(0);
785 out_observation.cameraPose.y(0);
786 out_observation.cameraPose.z(0);
787
788 out_observation.cameraPose.quat().r(1);
789 out_observation.cameraPose.quat().x(0);
790 out_observation.cameraPose.quat().y(0);
791 out_observation.cameraPose.quat().z(0);
792
793 out_observation.leftCamera.setIntrinsicParamsFromValues(
794 m_focalLength, m_focalLength, m_centerCol, m_centerRow);
795 out_observation.rightCamera.setIntrinsicParamsFromValues(
796 m_focalLength, m_focalLength, m_centerCol, m_centerRow);
797 return true;
798 }
799 catch (const std::exception& e)
800 {
801 std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Error:\n" << e.what() << std::endl;
802 return false;
803 }
804 #else
805 THROW_EXCEPTION("MRPT compiled without support for FlyCapture2, Triclops or OpenCV");
806 #endif
807 }