source: pacpussensors/trunk/PtGreyCameras/Flea3Grabber.cpp@ 59

Last change on this file since 59 was 56, checked in by phudelai, 10 years ago

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