Context Navigation

← Previous Revision
Latest Revision
Next Revision →
Blame
Revision Log

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

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

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

Download in other formats: