source: pacpussensors/trunk/PtGreyCameras/Flea3Grabber.cpp

Last change on this file was 69, checked in by phudelai, 10 years ago

updated
File size: 42.2 KB
Line 
1/*******************************************************************************
2// created: 2013/10/25 - 19:36
3// filename: Flea3Grabber.cpp
4//
5// author: Danilo Alves de Lima and Pierre Hudelaine
6// Copyright Heudiasyc UMR UTC/CNRS 6599
7//
8// version: $Id: $
9//
10// purpose: Grabbe images from multiple flea3
11//
12*******************************************************************************/
13
14#include "Flea3Grabber.h"
15
16#include <iomanip>
17#include <iostream>
18
19#include "Pacpus/kernel/DbiteException.h"
20#include "Pacpus/kernel/DbiteFileTypes.h"
21#include "Pacpus/kernel/ComponentFactory.h"
22#include "Pacpus/kernel/Log.h"
23
24using namespace std;
25using namespace pacpus;
26
27DECLARE_STATIC_LOGGER("pacpus.base.Flea3Grabber");
28
29// Construct the factory
30static ComponentFactory<Flea3Grabber> sFactory("Flea3Grabber");
31
32const int kMaxFilepathLength = 512; // TODO: should be same for all images
33
34
35////////////////////////////////////////////////////////////////////////////////
36/// Constructor
37////////////////////////////////////////////////////////////////////////////////
38Flea3Grabber::Flea3Grabber(QString name)
39 : ComponentBase(name)
40{
41 LOG_TRACE(Q_FUNC_INFO);
42
43 this->nbrCamera_ = 1;
44 this->masterCamera_ = 0;
45 this->img2send_ = 0;
46}
47
48
49////////////////////////////////////////////////////////////////////////////////
50/// Destructor
51////////////////////////////////////////////////////////////////////////////////
52Flea3Grabber::~Flea3Grabber()
53{
54 LOG_TRACE(Q_FUNC_INFO);
55
56 for (int i = 0; i < nbrCamera_; i++)
57 {
58 if (this->shmem_images_[i])
59 delete this->shmem_images_[i];
60
61 this->shmem_images_[i] = NULL;
62 }
63}
64
65
66////////////////////////////////////////////////////////////////////////////////
67/// Called by the ComponentManager to start the component
68////////////////////////////////////////////////////////////////////////////////
69void Flea3Grabber::startActivity()
70{
71 LOG_TRACE(Q_FUNC_INFO);
72
73 for (int i = 0; i < nbrCamera_; i++)
74 {
75
76 if (((FlyCapture2::PixelFormat)this->settings_[i].cam_PixelFormat == FlyCapture2::PIXEL_FORMAT_MONO8)||
77 ((FlyCapture2::PixelFormat)this->settings_[i].cam_PixelFormat == FlyCapture2::PIXEL_FORMAT_MONO12)||
78 ((FlyCapture2::PixelFormat)this->settings_[i].cam_PixelFormat == FlyCapture2::PIXEL_FORMAT_MONO16))
79 {
80 this->settings_[i].cam_channels = 1;
81 }
82 else
83 {
84 if (this->settings_[i].cam_ColorProcessingAlgorithm == 1)
85 this->settings_[i].cam_channels = 1;
86 else
87 this->settings_[i].cam_channels = 3;
88 }
89
90 // If do not rescale the image before save
91 this->settings_[i].mMaxImageOutputSize = sizeof(unsigned char) * this->settings_[i].cam_width * this->settings_[i].cam_height * this->settings_[i].cam_channels;
92
93 if (this->settings_[i].image_scale == 1.0)
94 this->settings_[i].mSaveImageSize = this->settings_[i].mMaxImageOutputSize;
95 else
96 this->settings_[i].mSaveImageSize = sizeof(unsigned char)*((int)(this->settings_[i].cam_width * this->settings_[i].image_scale)) * ((int)(this->settings_[i].cam_height * this->settings_[i].image_scale)) * this->settings_[i].cam_channels;
97
98 // Create output files for recording
99 if (isRecording())
100 {
101 try
102 {
103 QString dbtFileName_ = this->settings_[i].mOutputDirectory.filePath(name() + ".dbt");
104
105 if (this->settings_[i].save2dbt)
106 {
107 //std::cout << "Save file : " << dbtFileName_.toStdString() << std::endl;
108 this->mDbtImage.open(dbtFileName_.toStdString(), WriteMode, FILE_DBT_UNKNOWN, this->settings_[i].mSaveImageSize);
109 //this->mDbtImage.open(dbtFileName_.toStdString(), WriteMode, FILE_JPEG, kMaxFilepathLength);
110 }
111 else
112 {
113 //std::cout << "Save file : " << dbtFileName_.toStdString() << std::endl;
114 this->mDbtImage.open(dbtFileName_.toStdString(), WriteMode, FILE_IMAGE, kMaxFilepathLength);
115 //this->mDbtImage.open(dbtFileName_.toStdString(), WriteMode, FILE_JPEG, kMaxFilepathLength);
116 }
117 }
118 catch (DbiteException & e)
119 {
120 cerr << "error opening dbt file: " << e.what() << endl;
121 }
122 }
123 }
124
125 // Run thread
126 THREAD_ALIVE = true;
127 start();
128}
129
130
131////////////////////////////////////////////////////////////////////////////////
132/// Called by the ComponentManager to stop the component
133////////////////////////////////////////////////////////////////////////////////
134void Flea3Grabber::stopActivity()
135{
136 LOG_TRACE(Q_FUNC_INFO);
137
138 if (THREAD_ALIVE)
139 {
140 // Stop thread
141 THREAD_ALIVE = false;
142
143 while(is_running)
144 {
145 msleep(10);
146 }
147 }
148
149 // Close DBT file
150 if (isRecording())
151 {
152 this->mDbtImage.close();
153 }
154
155 LOG_INFO("stopped component '" << name() << "'");
156}
157
158
159////////////////////////////////////////////////////////////////////////////////
160/// Called by the ComponentManager to pass the XML parameters to the
161/// component
162////////////////////////////////////////////////////////////////////////////////
163ComponentBase::COMPONENT_CONFIGURATION Flea3Grabber::configureComponent(XmlComponentConfig config)
164{
165 LOG_TRACE(Q_FUNC_INFO);
166
167 if (config.getProperty("number_of_cameras") != QString::null)
168 this->nbrCamera_ = config.getProperty("number_of_cameras").toInt();
169
170 if (config.getProperty("master_indice") != QString::null) // Master camera number
171 this->masterCamera_ = config.getProperty("master_indice").toInt();
172
173 if (config.getProperty("use_shmem") != QString::null)
174 this->use_shmem = (bool)config.getProperty("use_shmem").toInt();
175
176 if (config.getProperty("shmem_corrected") != QString::null)
177 this->shmem_corrected = (bool)config.getProperty("shmem_corrected").toInt();
178
179 if (config.getProperty("cam_translationx") != QString::null)
180 trans_[0] = config.getProperty("cam_translationx").toFloat();
181 if (config.getProperty("cam_translationy") != QString::null)
182 trans_[1] = config.getProperty("cam_translationy").toFloat();
183 if (config.getProperty("cam_translationz") != QString::null)
184 trans_[2] = config.getProperty("cam_translationz").toFloat();
185
186 if (config.getProperty("cam_rotationx") != QString::null)
187 rot_[0] = config.getProperty("cam_rotationx").toFloat();
188 if (config.getProperty("cam_rotationy") != QString::null)
189 rot_[1] = config.getProperty("cam_rotationy").toFloat();
190 if (config.getProperty("cam_rotationz") != QString::null)
191 rot_[2] = config.getProperty("cam_rotationz").toFloat();
192
193 for (int i = 0; i < nbrCamera_; i++)
194 {
195 // Initialize with default values
196 settings_.push_back(camSetting());
197 this->InitDefault(i);
198
199 if (config.getProperty("cam_serial" + QString::number(i)) != QString::null)
200 settings_[i].cam_serial = config.getProperty("cam_serial" + QString::number(i)).toInt();
201
202 //---------------------------------------- Camera configuration --------------------------------------------
203 if (config.getProperty("auto_FrameRate" + QString::number(i)) != QString::null)
204 settings_[i].auto_FrameRate = (config.getProperty("auto_FrameRate" + QString::number(i)) == "true" ? true : false);
205
206 if (config.getProperty("cam_FrameRate" + QString::number(i)) != QString::null)
207 settings_[i].cam_FrameRate = config.getProperty("cam_FrameRate" + QString::number(i)).toDouble();
208
209 if (config.getProperty("auto_Gain" + QString::number(i)) != QString::null)
210 settings_[i].auto_Gain = (config.getProperty("auto_Gain" + QString::number(i)) == "true" ? true : false);
211
212 if (config.getProperty("cam_Gain" + QString::number(i)) != QString::null)
213 settings_[i].cam_Gain = config.getProperty("cam_Gain" + QString::number(i)).toDouble();
214
215 if (config.getProperty("auto_Exposure" + QString::number(i)) != QString::null)
216 settings_[i].auto_Exposure = (config.getProperty("auto_Exposure" + QString::number(i)) == "true" ? true : false);
217
218 if (config.getProperty("cam_Exposure" + QString::number(i)) != QString::null)
219 settings_[i].cam_Exposure = config.getProperty("cam_Exposure" + QString::number(i)).toDouble();
220
221 if (config.getProperty("auto_Shutter" + QString::number(i)) != QString::null)
222 settings_[i].auto_Shutter = (config.getProperty("auto_Shutter" + QString::number(i)) == "true" ? true : false);
223
224 if (config.getProperty("cam_Shutter" + QString::number(i)) != QString::null)
225 settings_[i].cam_Shutter = config.getProperty("cam_Shutter" + QString::number(i)).toDouble();
226
227 if (config.getProperty("auto_ExposurebyCode" + QString::number(i)) != QString::null)
228 settings_[i].auto_ExposurebyCode = (config.getProperty("auto_ExposurebyCode" + QString::number(i)) == "true" ? true : false);
229
230 if (config.getProperty("cam_ExposurebyCode_tshold" + QString::number(i)) != QString::null)
231 settings_[i].cam_ExposurebyCode_tshold = config.getProperty("cam_ExposurebyCode_tshold" + QString::number(i)).toDouble();
232 //----------------------------------------------------------------------------------------------------------
233
234 if (config.getProperty("cam_video_mode" + QString::number(i)) != QString::null)
235 settings_[i].cam_video_mode = config.getProperty("cam_video_mode" + QString::number(i)).toInt();
236
237 if (config.getProperty("cam_mode" + QString::number(i)) != QString::null)
238 settings_[i].cam_mode = config.getProperty("cam_mode" + QString::number(i)).toInt();
239
240 if (config.getProperty("cam_PixelFormat" + QString::number(i)) != QString::null)
241 settings_[i].cam_PixelFormat = config.getProperty("cam_PixelFormat" + QString::number(i)).toInt();
242
243 if (config.getProperty("cam_ColorProcessingAlgorithm" + QString::number(i)) != QString::null)
244 settings_[i].cam_ColorProcessingAlgorithm = config.getProperty("cam_ColorProcessingAlgorithm" + QString::number(i)).toInt();
245
246 if (config.getProperty("cam_start_point_left" + QString::number(i)) != QString::null)
247 settings_[i].cam_start_point_left = config.getProperty("cam_start_point_left" + QString::number(i)).toInt();
248
249 if (config.getProperty("cam_start_point_top" + QString::number(i)) != QString::null)
250 settings_[i].cam_start_point_top = config.getProperty("cam_start_point_top" + QString::number(i)).toInt();
251
252 if (config.getProperty("cam_width" + QString::number(i)) != QString::null)
253 settings_[i].cam_width = config.getProperty("cam_width" + QString::number(i)).toInt();
254
255 if (config.getProperty("cam_height" + QString::number(i)) != QString::null)
256 settings_[i].cam_height = config.getProperty("cam_height" + QString::number(i)).toInt();
257
258 //-------------------------------- Trigger/strobe options -------------------------------------------
259
260 if (config.getProperty("cam_trigger_mode" + QString::number(i)) != QString::null)
261 settings_[i].cam_trigger_mode = config.getProperty("cam_trigger_mode" + QString::number(i)).toInt();
262
263 if (config.getProperty("cam_trigger_enable" + QString::number(i)) != QString::null)
264 settings_[i].cam_trigger_enable = config.getProperty("cam_trigger_enable" + QString::number(i)).toInt();
265
266 if (config.getProperty("cam_trigger_parameter" + QString::number(i)) != QString::null)
267 settings_[i].cam_trigger_parameter = config.getProperty("cam_trigger_parameter" + QString::number(i)).toInt();
268
269 if (config.getProperty("cam_trigger_polarity" + QString::number(i)) != QString::null)
270 settings_[i].cam_trigger_polarity = config.getProperty("cam_trigger_polarity" + QString::number(i)).toInt();
271
272 if (config.getProperty("cam_trigger_source" + QString::number(i)) != QString::null)
273 settings_[i].cam_trigger_source = config.getProperty("cam_trigger_source" + QString::number(i)).toInt();
274
275 if (config.getProperty("cam_trigger_dest" + QString::number(i)) != QString::null)
276 settings_[i].cam_trigger_dest = config.getProperty("cam_trigger_dest" + QString::number(i)).toInt();
277
278 if (config.getProperty("cam_strobe_enable" + QString::number(i)) != QString::null)
279 settings_[i].cam_strobe_enable = config.getProperty("cam_strobe_enable" + QString::number(i)).toInt();
280
281 if (config.getProperty("cam_strobe_source" + QString::number(i)) != QString::null)
282 settings_[i].cam_strobe_source = config.getProperty("cam_strobe_source" + QString::number(i)).toInt();
283
284 if (config.getProperty("cam_strobe_polarity" + QString::number(i)) != QString::null)
285 settings_[i].cam_strobe_polarity = config.getProperty("cam_strobe_polarity" + QString::number(i)).toInt();
286
287 if (config.getProperty("cam_strobe_delay" + QString::number(i)) != QString::null)
288 settings_[i].cam_strobe_delay = config.getProperty("cam_strobe_delay" + QString::number(i)).toInt();
289
290 if (config.getProperty("cam_strobe_duration" + QString::number(i)) != QString::null)
291 settings_[i].cam_strobe_duration = config.getProperty("cam_strobe_duration" + QString::number(i)).toInt();
292
293 //-------------------------------- Camera settings --------------------------------------------------
294
295 if (config.getProperty("cam_fx" + QString::number(i)) != QString::null)
296 settings_[i].tmp_matrix[0][0] = config.getProperty("cam_fx" + QString::number(i)).toDouble();// / settings_[i].cam_width;
297 if (config.getProperty("cam_fy" + QString::number(i)) != QString::null)
298 settings_[i].tmp_matrix[1][1] = config.getProperty("cam_fy" + QString::number(i)).toDouble();// / settings_[i].cam_height;
299 if (config.getProperty("cam_cx" + QString::number(i)) != QString::null)
300 settings_[i].tmp_matrix[0][2] = config.getProperty("cam_cx" + QString::number(i)).toDouble();// / settings_[i].cam_width;
301 if (config.getProperty("cam_cy" + QString::number(i)) != QString::null)
302 settings_[i].tmp_matrix[1][2] = config.getProperty("cam_cy" + QString::number(i)).toDouble();// / settings_[i].cam_height;
303 settings_[i].tmp_matrix[2][2] = 1.0;
304
305 for (int j = 0; j < 8; j++) // 8 max parameters for the distortion in OpenCV
306 if (config.getProperty("cam_distk" + QString::number(j + 1) + QString::number(i)) != QString::null)
307 settings_[i].distCoe[j] = config.getProperty("cam_distk" + QString::number(j + 1) + QString::number(i)).toDouble();
308
309 //-------------------------------- Recording options ------------------------------------------------
310 if (config.getProperty("recording") != QString::null)
311 setRecording(config.getProperty("recording").toInt());
312
313 if (config.getProperty("image_scale" + QString::number(i)) != QString::null)
314 settings_[i].image_scale = config.getProperty("image_scale" + QString::number(i)).toDouble();
315
316 if (config.getProperty("image_compact" + QString::number(i)) != QString::null)
317 settings_[i].image_compact = config.getProperty("image_compact" + QString::number(i)).toInt();
318
319 if (config.getProperty("save2dbt" + QString::number(i)) != QString::null)
320 settings_[i].save2dbt = config.getProperty("save2dbt" + QString::number(i)).toInt();
321 //---------------------------------------------------------------------------------------------------
322
323 if (config.getProperty("showdebug" + QString::number(i)) != QString::null)
324 settings_[i].showdebug = (bool)config.getProperty("showdebug" + QString::number(i)).toInt();
325
326 if (config.getProperty("outputdir" + QString::number(i)) != QString::null)
327 {
328 if (isRecording())
329 {
330 settings_[i].mOutputDirectory.mkdir(config.getProperty("outputdir" + QString::number(i)));
331 settings_[i].mOutputDirectory.mkdir(config.getProperty("outputdir" + QString::number(i) + "/" + name()));
332 settings_[i].mOutputDirectory.setPath(config.getProperty("outputdir" + QString::number(i)));
333 }
334 }
335 else
336 {
337 if (isRecording())
338 {
339 settings_[i].mOutputDirectory.mkdir(name());
340 settings_[i].mOutputDirectory.setPath(settings_[i].mOutputDirectory.currentPath());
341 }
342 }
343 }
344
345 if (masterCamera_ >= nbrCamera_)
346 {
347 LOG_ERROR("masterCamera number error");
348 ComponentManager::getInstance()->stop(name());
349 }
350
351 LOG_INFO("configured component '" << name() << "'");
352 return ComponentBase::CONFIGURED_OK;
353}
354
355
356////////////////////////////////////////////////////////////////////////////////
357/// Initialize default values
358////////////////////////////////////////////////////////////////////////////////
359void Flea3Grabber::InitDefault(int indice)
360{
361 // Default
362 setRecording(0);
363
364 this->settings_[indice].cam_serial = 0; // Camera index to connect
365
366 // Camera configuration
367 this->settings_[indice].auto_FrameRate = true; // Set auto frame rate
368 this->settings_[indice].cam_FrameRate = 15.0; // Frame rates in frames per second = FlyCapture2::FRAMERATE_15
369 this->settings_[indice].auto_Gain = true; // Set auto gain
370 this->settings_[indice].cam_Gain = 1.0; // Gain value in db
371 this->settings_[indice].auto_Exposure = true; // Set auto exposure
372 this->settings_[indice].cam_Exposure = 1.322; // Auto exposure in EV
373 this->settings_[indice].auto_Shutter = true; // Set auto shutter
374 this->settings_[indice].cam_Shutter = 66.639; // Shutter in miliseconds
375 this->settings_[indice].auto_ExposurebyCode = false; // Set auto exposure by pos processing method
376 this->settings_[indice].cam_ExposurebyCode_tshold = 20.0; // Pecentage of white pixels for threshold
377 this->settings_[indice].cam_start_point_left = 0; // Image left point (for standard modes only)
378 this->settings_[indice].cam_start_point_top = 0; // Image top point (for standard modes only)
379 this->settings_[indice].cam_width = 1280; // Image width (for standard modes only)
380 this->settings_[indice].cam_height = 960; // image height (for standard modes only)
381 this->settings_[indice].cam_video_mode = FlyCapture2::VIDEOMODE_FORMAT7; // DCAM video modes
382 this->settings_[indice].cam_mode = FlyCapture2::MODE_0; // Camera modes for DCAM formats as well as Format7
383 this->settings_[indice].cam_PixelFormat = FlyCapture2::PIXEL_FORMAT_RAW8; // Pixel formats available for Format7 modes7
384 this->settings_[indice].cam_trigger_mode = 0; // Trigger mode
385 this->settings_[indice].cam_trigger_enable = 0; // Trigger enable (1 on // 0 off)
386 this->settings_[indice].cam_trigger_parameter = 0;
387 this->settings_[indice].cam_trigger_polarity = 0; // Trigger polarity (0 low // 1 high)
388 this->settings_[indice].cam_trigger_source = 0; // Source (GPIO)
389 this->settings_[indice].cam_trigger_dest = 0; // Dest (GPIO) input/output
390 this->settings_[indice].cam_strobe_source = 0;
391 this->settings_[indice].cam_strobe_enable = 0; // Enable strobe mode
392 this->settings_[indice].cam_strobe_polarity = 0; // Strobe polarity
393 this->settings_[indice].cam_strobe_delay = 0;
394 this->settings_[indice].cam_strobe_duration = 0;
395 this->settings_[indice].cam_ColorProcessingAlgorithm = FlyCapture2::NEAREST_NEIGHBOR; /**
396 * Color processing algorithms. Please refer to our knowledge base at
397 * article at http://www.ptgrey.com/support/kb/index.asp?a=4&q=33 for
398 * complete details for each algorithm.
399 */
400 this->settings_[indice].showdebug = false; // Show frame acquired
401
402 if (this->settings_[indice].cam_ColorProcessingAlgorithm == 1)
403 this->settings_[indice].cam_channels = 1;
404 else
405 this->settings_[indice].cam_channels = 3;
406
407 // Size of the image data sizeof(char)*width*height*channels
408 this->settings_[indice].mMaxImageOutputSize = sizeof(char) * this->settings_[indice].cam_width * this->settings_[indice].cam_height * this->settings_[indice].cam_channels;
409 this->settings_[indice].image_scale = 1.0; // Default scale
410 this->settings_[indice].image_compact = 0; // Don't compact the image data
411 this->settings_[indice].save2dbt = 0;
412
413 for (int i = 0; i < 3; i++)
414 for (int j = 0; j < 3; j++)
415 settings_[indice].tmp_matrix[i][j] = 0.0;
416
417 for (int i = 0; i < 8; i++) // 8 max for dist coeffs
418 settings_[indice].distCoe[i] = 0.0;
419}
420
421
422////////////////////////////////////////////////////////////////////////////////
423/// Thread loop
424////////////////////////////////////////////////////////////////////////////////
425void Flea3Grabber::run()
426{
427 LOG_TRACE(Q_FUNC_INFO);
428
429 this->is_running = true;
430 bool triggered = false;
431
432 // Tmp matrice used because of intelligent pointer of OpenCV and "intelligent" memory free
433 cv::Mat_<double> tmp_cameraMatrix(3,3); // 3x3 matrix
434 cv::Mat_<double> tmp_distCoeffs(8,1); // 5x1 matrix
435 cv::Mat_<double> R(3,1); // 3x1 matrix, rotation left to right camera
436 cv::Mat_<double> T(3,1); // 3x1 matrix, translation left to right proj. center
437 cv::Mat R1, R2, P1, P2, Q;
438 std::vector<cv::Mat> map1(nbrCamera_);
439 std::vector<cv::Mat> map2(nbrCamera_);
440 std::vector<cv::Mat> correctedImg(nbrCamera_);
441
442 for (int i = 0; i < nbrCamera_; i++)
443 {
444 correctedImg.push_back(cv::Mat(settings_[i].cam_height, settings_[i].cam_width, CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels)));
445 map1.push_back(cv::Mat());
446 map2.push_back(cv::Mat());
447
448 tmp_cameraMatrix << settings_[i].tmp_matrix[0][0], settings_[i].tmp_matrix[0][1], settings_[i].tmp_matrix[0][2], settings_[i].tmp_matrix[1][0], settings_[i].tmp_matrix[1][1], settings_[i].tmp_matrix[1][2], settings_[i].tmp_matrix[2][0], settings_[i].tmp_matrix[2][1], settings_[i].tmp_matrix[2][2];
449 settings_[i].matrix = tmp_cameraMatrix;
450 LOG_INFO(settings_[i].matrix);
451 tmp_distCoeffs << settings_[i].distCoe[0], settings_[i].distCoe[1], settings_[i].distCoe[2], settings_[i].distCoe[3], settings_[i].distCoe[4], settings_[i].distCoe[5], settings_[i].distCoe[6], settings_[i].distCoe[7];
452 settings_[i].distCoeffs = tmp_distCoeffs;
453 LOG_INFO(settings_[i].distCoeffs);
454 }
455
456 // Used only for two cameras
457 if (nbrCamera_ == 2)
458 {
459 // Initialisation des matrices pour les calculs OpenCV
460 R << rot_[0], rot_[1], rot_[2];
461 T << trans_[0], trans_[1], trans_[2];
462
463 cv::stereoRectify(settings_[0].matrix, settings_[0].distCoeffs, settings_[1].matrix, settings_[1].distCoeffs, cv::Size(settings_[0].cam_width, settings_[0].cam_height), R, T, R1, R2, P1, P2, Q, CV_CALIB_ZERO_DISPARITY);
464
465 cv::initUndistortRectifyMap(settings_[0].matrix, settings_[0].distCoeffs, R1, P1, cv::Size(settings_[0].cam_width, settings_[0].cam_height), CV_16SC2, map1.at(0), map2.at(0));
466 cv::initUndistortRectifyMap(settings_[1].matrix, settings_[1].distCoeffs, R2, P2, cv::Size(settings_[1].cam_width, settings_[1].cam_height), CV_16SC2, map1.at(1), map2.at(1));
467 }
468
469 std::vector<FlyCapture2::Camera *> cams;
470 std::vector<FlyCapture2::Image> convertedImage;
471
472 //==================================== Camera initialization =================================================
473 // Check the number of cameras
474 FlyCapture2::BusManager busMgr;
475 unsigned int numCameras;
476 FlyCapture2::Error error = busMgr.GetNumOfCameras(&numCameras);
477
478 if (error != FlyCapture2::PGRERROR_OK)
479 {
480 LOG_FATAL(error.GetDescription());
481 ComponentManager::getInstance()->stop(name());
482 }
483
484 if (this->nbrCamera_ != numCameras)
485 {
486 LOG_FATAL("Incoorect number of cameras...");
487 ComponentManager::getInstance()->stop(name());
488 }
489
490 for (int i = 0; i < this->nbrCamera_; i++)
491 {
492 if (this->use_shmem)
493 {
494 this->settings_[i].ImageHeader.image.width = this->settings_[i].cam_width;
495 this->settings_[i].ImageHeader.image.height = this->settings_[i].cam_height;
496 this->settings_[i].ImageHeader.image.channels = this->settings_[i].cam_channels;
497 this->settings_[i].ImageHeader.image.width_step = this->settings_[i].cam_width * this->settings_[i].cam_channels * sizeof(unsigned char);
498 this->settings_[i].ImageHeader.image.data_size = this->settings_[i].cam_width * this->settings_[i].cam_height;
499
500 this->settings_[i].img_mem_size = sizeof(TimestampedStructImage) + settings_[i].mMaxImageOutputSize;
501 this->settings_[i].img_mem = malloc(this->settings_[i].img_mem_size);
502
503 ShMem * tmp_shmem = new ShMem((name() + "_" + QString().setNum(settings_[i].cam_serial)).toStdString().c_str(), this->settings_[i].img_mem_size);
504
505 shmem_images_.push_back(tmp_shmem);
506 }
507
508 // Get camera from serial (in this case first camera in the bus)
509 FlyCapture2::PGRGuid guid;
510 error = busMgr.GetCameraFromSerialNumber(this->settings_[i].cam_serial, &guid);
511 if (error != FlyCapture2::PGRERROR_OK)
512 {
513 LOG_ERROR(error.GetDescription());
514 ComponentManager::getInstance()->stop(name());
515 }
516
517 cams.push_back(new FlyCapture2::Camera); // FlyCapture camera class
518
519 // Connect to a camerak
520 error = cams[i]->Connect(&guid);
521 if (error != FlyCapture2::PGRERROR_OK)
522 {
523 LOG_ERROR(error.GetDescription());
524 ComponentManager::getInstance()->stop(name());
525 }
526
527 // Get the camera information
528 FlyCapture2::CameraInfo camInfo;
529 error = cams[i]->GetCameraInfo(&camInfo);
530 if (error != FlyCapture2::PGRERROR_OK)
531 {
532 LOG_ERROR(error.GetDescription());
533 ComponentManager::getInstance()->stop(name());
534 }
535
536 // Just for visualization
537 /*printf(
538 "\n*** CAMERA INFORMATION ***\n"
539 "Serial number - %u\n"
540 "Camera model - %s\n"
541 "Camera vendor - %s\n"
542 "Sensor - %s\n"
543 "Resolution - %s\n"
544 "Firmware version - %s\n"
545 "Firmware build time - %s\n\n",
546 camInfo.serialNumber,
547 camInfo.modelName,
548 camInfo.vendorName,
549 camInfo.sensorInfo,
550 camInfo.sensorResolution,
551 camInfo.firmwareVersion,
552 camInfo.firmwareBuildTime);*/
553
554 // Query for available Format 7 modes
555 FlyCapture2::Format7Info fmt7Info;
556 bool supported;
557 fmt7Info.mode = (FlyCapture2::Mode)this->settings_[i].cam_mode;
558 error = cams[i]->GetFormat7Info(&fmt7Info, &supported);
559 if (error != FlyCapture2::PGRERROR_OK)
560 {
561 LOG_ERROR(error.GetDescription());
562 ComponentManager::getInstance()->stop(name());
563 }
564
565 if (((FlyCapture2::PixelFormat)this->settings_[i].cam_PixelFormat & fmt7Info.pixelFormatBitField) == 0)
566 {
567 // Pixel format not supported!
568 LOG_ERROR("Pixel format is not supported\n");
569 ComponentManager::getInstance()->stop(name());
570 }
571
572 // Configurate custom image
573 FlyCapture2::Format7ImageSettings fmt7ImageSettings;
574 fmt7ImageSettings.mode = (FlyCapture2::Mode)this->settings_[i].cam_mode;
575 fmt7ImageSettings.offsetX = this->settings_[i].cam_start_point_left;
576 fmt7ImageSettings.offsetY = this->settings_[i].cam_start_point_top;
577 fmt7ImageSettings.width = this->settings_[i].cam_width;
578 fmt7ImageSettings.height = this->settings_[i].cam_height;
579 fmt7ImageSettings.pixelFormat = (FlyCapture2::PixelFormat)this->settings_[i].cam_PixelFormat;
580
581 bool valid;
582 FlyCapture2::Format7PacketInfo fmt7PacketInfo;
583
584 // Validate the settings to make sure that they are valid
585 error = cams[i]->ValidateFormat7Settings(
586 &fmt7ImageSettings,
587 &valid,
588 &fmt7PacketInfo);
589 if (error != FlyCapture2::PGRERROR_OK)
590 {
591 LOG_ERROR(error.GetDescription());
592 ComponentManager::getInstance()->stop(name());
593 }
594
595 if (!valid)
596 {
597 // Settings are not valid
598 LOG_ERROR("Format7 settings are not valid");
599 ComponentManager::getInstance()->stop(name());
600 }
601
602 // Set the settings to the camera
603 error = cams[i]->SetFormat7Configuration(
604 &fmt7ImageSettings,
605 fmt7PacketInfo.recommendedBytesPerPacket);
606 if (error != FlyCapture2::PGRERROR_OK)
607 {
608 LOG_ERROR(error.GetDescription());
609 ComponentManager::getInstance()->stop(name());
610 }
611
612 // Define the color algorithm
613 FlyCapture2::Image::SetDefaultColorProcessing((FlyCapture2::ColorProcessingAlgorithm)this->settings_[i].cam_ColorProcessingAlgorithm);
614
615 // Start capturing images
616 error = cams[i]->StartCapture();
617 if (error != FlyCapture2::PGRERROR_OK)
618 {
619 LOG_ERROR(error.GetDescription());
620 ComponentManager::getInstance()->stop(name());
621 }
622
623 // Set frame rate property
624 FlyCapture2::Property cam_property;
625 cam_property.type = (FlyCapture2::PropertyType)16; //FlyCapture2::PropertyType::FRAME_RATE;
626 cam_property.absControl = true;
627 cam_property.autoManualMode = this->settings_[i].auto_FrameRate;
628 cam_property.onOff = true;
629 cam_property.onePush = false;
630 cam_property.absValue = (float)this->settings_[i].cam_FrameRate;
631 error = cams[i]->SetProperty(&cam_property);
632 if (error != FlyCapture2::PGRERROR_OK)
633 {
634 LOG_ERROR(error.GetDescription());
635 ComponentManager::getInstance()->stop(name());
636 }
637
638 // Set gain property
639 cam_property.type = (FlyCapture2::PropertyType)13; //FlyCapture2::PropertyType::GAIN;
640 cam_property.absControl = true;
641 cam_property.autoManualMode = this->settings_[i].auto_Gain;
642 cam_property.onOff = true;
643 cam_property.onePush = false;
644 cam_property.absValue = (float)this->settings_[i].cam_Gain;
645 error = cams[i]->SetProperty(&cam_property);
646 if (error != FlyCapture2::PGRERROR_OK)
647 {
648 LOG_ERROR(error.GetDescription());
649 ComponentManager::getInstance()->stop(name());
650 }
651
652 // Set exposure property
653 cam_property.type = (FlyCapture2::PropertyType)1; //FlyCapture2::PropertyType::AUTO_EXPOSURE;
654 cam_property.absControl = true;
655 cam_property.onePush = false;
656 cam_property.onOff = true;
657 cam_property.autoManualMode = this->settings_[i].auto_Exposure;
658 cam_property.absValue = (float)this->settings_[i].cam_Exposure;
659 error = cams[i]->SetProperty(&cam_property);
660 if (error != FlyCapture2::PGRERROR_OK)
661 {
662 LOG_ERROR(error.GetDescription());
663 ComponentManager::getInstance()->stop(name());
664 }
665
666 // Set shutter property
667 cam_property.type = (FlyCapture2::PropertyType)12; //FlyCapture2::PropertyType::SHUTTER;
668 cam_property.absControl = true;
669 cam_property.onePush = false;
670 cam_property.onOff = true;
671 cam_property.autoManualMode = this->settings_[i].auto_Shutter;
672 cam_property.absValue = (float)this->settings_[i].cam_Shutter;
673 error = cams[i]->SetProperty(&cam_property);
674 if (error != FlyCapture2::PGRERROR_OK)
675 {
676 LOG_ERROR(error.GetDescription());
677 ComponentManager::getInstance()->stop(name());
678 }
679
680 if (settings_[i].cam_trigger_enable)
681 {
682 FlyCapture2::TriggerMode trigger;
683 trigger.mode = settings_[i].cam_trigger_mode;
684 trigger.onOff = settings_[i].cam_trigger_enable;
685 trigger.parameter = settings_[i].cam_trigger_parameter;
686 trigger.polarity = settings_[i].cam_trigger_polarity;
687 trigger.source = settings_[i].cam_trigger_source;
688 error = cams[i]->SetTriggerMode(&trigger);
689 if (error != FlyCapture2::PGRERROR_OK)
690 {
691 LOG_ERROR(error.GetDescription());
692 ComponentManager::getInstance()->stop(name());
693 }
694
695 error = cams[i]->SetGPIOPinDirection(settings_[i].cam_trigger_dest, (i == masterCamera_)? 1 : 0);
696 if (error != FlyCapture2::PGRERROR_OK)
697 {
698 LOG_ERROR(error.GetDescription());
699 ComponentManager::getInstance()->stop(name());
700 }
701
702 triggered = true;
703 }
704
705 if (settings_[i].cam_strobe_enable)
706 {
707 FlyCapture2::StrobeControl strobe;
708 strobe.onOff = settings_[i].cam_strobe_enable;
709 strobe.source = settings_[i].cam_strobe_source;
710 strobe.delay = settings_[i].cam_strobe_delay;
711 strobe.duration = settings_[i].cam_strobe_duration;
712 strobe.polarity = settings_[i].cam_strobe_polarity;
713 error = cams[i]->SetStrobe(&strobe);
714 if (error != FlyCapture2::PGRERROR_OK)
715 {
716 LOG_ERROR(error.GetDescription());
717 ComponentManager::getInstance()->stop(name());
718 }
719 }
720
721 //============================================================================================================
722
723 //---------------------------------- Recording parameters ------------------------------------
724 //size_t imageCount = 0;
725
726 // Timestamp
727 //road_time_t time = 0;
728 //road_timerange_t tr = 0;
729
730 const char * const imagePrefix = "image";
731 const char * const imageNameDelimiter = "-";
732 const char * const imageExtension = this->settings_[i].image_compact ? ".jpg" :
733 (((FlyCapture2::PixelFormat)this->settings_[i].cam_PixelFormat == FlyCapture2::PIXEL_FORMAT_MONO8)||
734 ((FlyCapture2::PixelFormat)this->settings_[i].cam_PixelFormat == FlyCapture2::PIXEL_FORMAT_MONO12)||
735 ((FlyCapture2::PixelFormat)this->settings_[i].cam_PixelFormat == FlyCapture2::PIXEL_FORMAT_MONO16)) ? ".pgm" : ".ppm";
736
737 const char kFillCharacter = '0';
738 const int kNumberWidth = 5;
739 //--------------------------------------------------------------------------------------------
740 FlyCapture2::Image tmp_img;
741 convertedImage.push_back(tmp_img);
742 }
743
744 // Raw image
745 FlyCapture2::Image rawImage;
746
747 //bool shmen_init = false;
748 // Because the API doesn't work without (saw this with FlyCapture2.exe)
749 if (settings_[masterCamera_].cam_trigger_enable)
750 {
751 cams[masterCamera_]->FireSoftwareTrigger();
752 usleep(50000);
753
754 cams[masterCamera_]->FireSoftwareTrigger();
755 usleep(50000);
756 }
757
758 img_sending_ = new ShMem("Flea3Grabber_sending", settings_[0].mMaxImageOutputSize);
759
760
761 road_time_t time;
762
763 while (THREAD_ALIVE)
764 {
765 if (settings_[masterCamera_].cam_trigger_enable)
766 {
767 error = cams[masterCamera_]->FireSoftwareTrigger();
768 if (error != FlyCapture2::PGRERROR_OK)
769 {
770 LOG_ERROR(error.GetDescription());
771 continue;
772 }
773 }
774
775 time = road_time();
776
777 //cout << settings_[1].timeStamp.microSeconds - settings_[0].timeStamp.microSeconds << endl;
778
779 for (int i = 0; i < this->nbrCamera_; i++)
780 {
781 // Retrieve an image
782 error = cams[i]->RetrieveBuffer(&rawImage);
783 if (error != FlyCapture2::PGRERROR_OK)
784 {
785 LOG_ERROR(error.GetDescription());
786 continue;
787 }
788
789
790 // Image timestamp
791 //settings_[i].timeStamp = rawImage.GetTimeStamp();
792
793 //time = road_time(); // Use the same time base of the other sensors
794
795 if (this->settings_[i].cam_channels == 1)
796 {
797 // Convert the raw image
798 error = rawImage.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &convertedImage[i]);
799 if (error != FlyCapture2::PGRERROR_OK)
800 {
801 LOG_ERROR(error.GetDescription());
802 ComponentManager::getInstance()->stop(name());
803 }
804 }
805 else
806 {
807 // Convert the raw image
808 error = rawImage.Convert(FlyCapture2::PIXEL_FORMAT_BGR, &convertedImage[i]);
809 if (error != FlyCapture2::PGRERROR_OK)
810 {
811 LOG_ERROR(error.GetDescription());
812 ComponentManager::getInstance()->stop(name());
813 }
814 }
815
816 cv::remap(cv::Mat(convertedImage[i].GetRows(), convertedImage[i].GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), convertedImage[i].GetData(), (int)((double)convertedImage[i].GetReceivedDataSize()/(double)convertedImage[i].GetRows())), correctedImg[i], map1.at(i), map2.at(i), CV_INTER_LINEAR);
817
818 if (this->use_shmem)
819 {
820 // Complete timestamp header of the right image
821 this->settings_[i].ImageHeader.time = time;
822 this->settings_[i].ImageHeader.timerange = 0;
823
824 // Copy images header and data to memory
825 memcpy(this->settings_[i].img_mem, &settings_[i].ImageHeader, sizeof(TimestampedStructImage));
826
827 if (shmem_corrected)
828 memcpy((void*)((TimestampedStructImage*)this->settings_[i].img_mem + 1), (void*)correctedImg[i].data, this->settings_[i].ImageHeader.image.data_size);
829 else
830 memcpy((void*)((TimestampedStructImage*)this->settings_[i].img_mem + 1), (void*)convertedImage[i].GetData(), this->settings_[i].ImageHeader.image.data_size);
831
832 this->shmem_images_[i]->write(this->settings_[i].img_mem, this->settings_[i].img_mem_size);
833 }
834
835 if (this->settings_[i].showdebug)
836 {
837 cv::namedWindow(QString("Flea3Component - Current Reference Frame " + QString::number(i)).toStdString(), CV_WINDOW_NORMAL);
838 cv::imshow(QString("Flea3Component - Current Reference Frame " + QString::number(i)).toStdString(), cv::Mat(convertedImage[i].GetRows(), convertedImage[i].GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), convertedImage[i].GetData(), (int)((double)convertedImage[i].GetReceivedDataSize()/(double)convertedImage[i].GetRows())));
839
840 cv::namedWindow(QString("Flea3Component - Current Corrected Frame " + QString::number(i)).toStdString(), CV_WINDOW_NORMAL);
841 cv::imshow(QString("Flea3Component - Current Corrected Frame " + QString::number(i)).toStdString(), cv::Mat(convertedImage[i].GetRows(), convertedImage[i].GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), correctedImg[i].data, (int)((double)convertedImage[i].GetReceivedDataSize()/(double)convertedImage[i].GetRows())));
842 cv::waitKey(1);
843 }
844
845 /////////////////////
846 // Write images to disk
847 /*if (isRecording())
848 {
849 if (this->settings_[i].save2dbt)
850 {
851 //------------------------------------------------ Save image in the dbt file ------------------------------------------------------------
852 if (this->settings_[i].image_scale == 1.0)
853 {
854 try
855 {
856 this->mDbtImage.writeRecord(time, tr, (char *)convertedImage.GetData(), convertedImage.GetDataSize());
857 }
858 catch (DbiteException & e)
859 {
860 cerr << "error opening dbt file: " << e.what() << endl;
861 }
862 }
863 else
864 {
865 // convert to cv::mat
866 unsigned int rowBytes = (int)((double)convertedImage.GetReceivedDataSize()/(double)convertedImage.GetRows());
867 cv::Mat Img_BGR = cv::Mat(convertedImage.GetRows(), convertedImage.GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), convertedImage.GetData(), rowBytes);
868
869 // Scaled image
870 cv::Mat img_resized;
871
872 cv::resize(Img_BGR, img_resized, cv::Size((int)(Img_BGR.cols * this->settings_[i].image_scale), (int)(Img_BGR.rows * this->settings_[i].image_scale)), 0.0, 0.0, CV_INTER_CUBIC);
873
874 try
875 {
876 this->mDbtImage.writeRecord(time, tr, (char*)img_resized.data, this->mSaveImageSize);
877 }
878 catch (DbiteException & e)
879 {
880 cerr << "error opening dbt file: " << e.what() << endl;
881 }
882 }
883 //----------------------------------------------------------------------------------------------------------------------------------------
884 }
885 else
886 {
887 //------------------------------------------------------------ Save image file ------------------------------------------------------------
888 // Save the image to file
889 stringstream imageNameSs;
890
891 imageNameSs << name().toStdString() << "/" << imagePrefix << imageNameDelimiter << setfill(kFillCharacter) << setw(kNumberWidth) << imageCount << imageExtension;
892 string imageName = this->settings_[i].mOutputDirectory.filePath(imageNameSs.str().c_str()).toStdString();
893
894 //LOG_INFO("Recording frame...");
895 if ((this->settings_[i].image_compact == 0) && (this->settings_[i].image_scale == 1.0))
896 {
897 // Save the image. If a file format is not passed in, then the file
898 // extension is parsed to attempt to determine the file format.
899 error = convertedImage.Save(const_cast<char *>(imageName.c_str()));
900 if (error != FlyCapture2::PGRERROR_OK)
901 {
902 LOG_ERROR(error.GetDescription());
903 ComponentManager::getInstance()->stop(name());
904 }
905 }
906 else if (this->settings_[i].image_scale == 1.0)
907 {
908 // convert to cv::mat
909 unsigned int rowBytes = (int)((double)convertedImage.GetReceivedDataSize()/(double)convertedImage.GetRows());
910 cv::Mat Img_BGR = cv::Mat(convertedImage.GetRows(), convertedImage.GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), convertedImage.GetData(), rowBytes);
911
912 // Save the image. If a file format is not passed in, then the file
913 // extension is parsed to attempt to determine the file format.
914 if (!cv::imwrite(const_cast<char *>(imageName.c_str()), Img_BGR))
915 {
916 LOG_ERROR("Error to save the Flea image!");
917 ComponentManager::getInstance()->stop(name());
918 }
919 }
920 else
921 {
922 // convert to cv::mat
923 unsigned int rowBytes = (int)((double)convertedImage.GetReceivedDataSize()/(double)convertedImage.GetRows());
924 cv::Mat Img_BGR = cv::Mat(convertedImage.GetRows(), convertedImage.GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), convertedImage.GetData(), rowBytes);
925
926 // Scaled image
927 cv::Mat img_resized;
928
929 cv::resize(Img_BGR, img_resized, cv::Size((int)(Img_BGR.cols * this->settings_[i].image_scale), (int)(Img_BGR.rows * this->settings_[i].image_scale)), 0.0, 0.0, CV_INTER_CUBIC);
930
931 // Save the image. If a file format is not passed in, then the file
932 // extension is parsed to attempt to determine the file format.
933 if (!cv::imwrite(const_cast<char *>(imageName.c_str()), img_resized))
934 {
935 LOG_ERROR("Error to save the Flea image!");
936 ComponentManager::getInstance()->stop(name());
937 }
938 }
939
940 try
941 {
942 this->mDbtImage.writeRecord(time, tr, this->settings_[i].mOutputDirectory.filePath(imageName.c_str()).toStdString().c_str(), kMaxFilepathLength);
943 }
944 catch (DbiteException & e)
945 {
946 cerr << "error opening dbt file: " << e.what() << endl;
947 }
948 //--------------------------------------------------------------------------------------------------------------------------------------------
949 }
950 }*/
951
952 //LOG_TRACE("image no. " << imageCount << " acquired successfully");
953 //++imageCount;
954 }
955
956 switch (img2send_)
957 {
958 case 0:
959 this->img_sending_->write((void*)convertedImage[0].GetData(), settings_[0].mMaxImageOutputSize);
960 break;
961 case 1:
962 this->img_sending_->write((void*)correctedImg[0].data, settings_[0].mMaxImageOutputSize);
963 break;
964 case 2:
965 this->img_sending_->write((void*)convertedImage[1].GetData(), settings_[0].mMaxImageOutputSize);
966 break;
967 case 3:
968 this->img_sending_->write((void*)correctedImg[1].data, settings_[0].mMaxImageOutputSize);
969 break;
970 default:
971 // Should not happen
972 break;
973 }
974 }
975
976 for (int i = 0; i < this->nbrCamera_; i++)
977 {
978 //====================================== Camera finalization =================================================
979 LOG_INFO(QString("Finished grabbing images!" + QString::number(settings_[i].cam_serial)));
980 // Stop capturing images
981 error = cams[i]->StopCapture();
982 if (error != FlyCapture2::PGRERROR_OK)
983 {
984 LOG_ERROR(error.GetDescription());
985 ComponentManager::getInstance()->stop(name());
986 }
987
988 // Disconnect the camera
989 error = cams[i]->Disconnect();
990 if (error != FlyCapture2::PGRERROR_OK)
991 {
992 LOG_ERROR(error.GetDescription());
993 ComponentManager::getInstance()->stop(name());
994 }
995 //============================================================================================================
996
997 if (this->shmem_images_[i])
998 delete shmem_images_[i];
999
1000 this->shmem_images_[i] = NULL;
1001
1002 this->is_running = false;
1003
1004 // Destroy the window frame
1005 if (this->settings_[i].showdebug)
1006 cvDestroyAllWindows();
1007 }
1008}
1009
1010
1011////////////////////////////////////////////////////////////////////////////////
1012/// Slot to change the image
1013////////////////////////////////////////////////////////////////////////////////
1014void Flea3Grabber::changeImage()
1015{
1016 img2send_ = ++img2send_ & 0x03;
1017}
Note: See TracBrowser for help on using the repository browser.