| 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 |
} |