Context Navigation

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

Last change on this file since 67 was 67, checked in by phudelai, 10 years ago
Modification du grabber
File size: 42.1 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
24	using namespace std;
25	using namespace pacpus;
26
27	DECLARE_STATIC_LOGGER("pacpus.base.Flea3Grabber");
28
29	// Construct the factory
30	static ComponentFactory<Flea3Grabber> sFactory("Flea3Grabber");
31
32	const int kMaxFilepathLength = 512; // TODO: should be same for all images
33
34
35	////////////////////////////////////////////////////////////////////////////////
36	/// Constructor
37	////////////////////////////////////////////////////////////////////////////////
38	Flea3Grabber::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	////////////////////////////////////////////////////////////////////////////////
52	Flea3Grabber::~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	////////////////////////////////////////////////////////////////////////////////
69	void 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	////////////////////////////////////////////////////////////////////////////////
134	void 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	////////////////////////////////////////////////////////////////////////////////
163	ComponentBase::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)
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	////////////////////////////////////////////////////////////////////////////////
359	void 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)widthheight*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	////////////////////////////////////////////////////////////////////////////////
425	void 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	////////////////////////////////////////////////////////////////////////////////
1014	void Flea3Grabber::changeImage()
1015	{
1016	img2send_ = ++img2send_ & 0x03;
1017	}

Note: See TracBrowser for help on using the repository browser.

Download in other formats: