source: pacpussensors/trunk/PtGreyCameras/Flea3Component.cpp@ 110

Last change on this file since 110 was 51, checked in by phudelai, 10 years ago

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