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

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

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