source: pacpussensors/trunk/Ladybug/LadybugComponent.cpp@ 98

Last change on this file since 98 was 98, checked in by ndiasnak, 9 years ago

Added: timestamp and DbiteFile to LadybugComponent
File size: 15.8 KB
Line 
1// Ladybug.cpp : définit les fonctions exportées pour l'application DLL.
2//
3
4#include "LadybugComponent.h"
5
6#ifdef WIN32
7#include <Windows.h>
8#endif
9
10#include <qfile.h>
11#include <qapplication.h>
12
13#include <iostream>
14//#include <sstream>
15//#include <string>
16#include <qdebug.h>
17#include <shlobj.h>
18
19//#include <QMetaType>
20#include "Pacpus/kernel/ComponentFactory.h"
21#include "Pacpus/kernel/Log.h"
22//#include "Pacpus/kernel/DbiteException.h"
23//#include "Pacpus/kernel/DbiteFileTypes.h"
24
25using namespace std;
26using namespace pacpus;
27
28namespace pacpus{
29
30DECLARE_STATIC_LOGGER("pacpus.base.LadybugComponent");
31
32// Construct the factory
33static ComponentFactory<LadybugComponent> sFactory("LadybugComponent");
34static const size_t kMaxFilepathLength = 128;
35
36/* *Constructor*/
37LadybugComponent::LadybugComponent(QString name) : ComponentBase(name)
38{
39 lbFrameCount = 0;
40 lbCurrentTime = 0;
41 lbElapsedTime = 0;
42 lbLastUpdateTime = 0;
43
44 LOG_TRACE ("constructor(" << name << ")");
45}
46
47/* *Destructor */
48LadybugComponent::~LadybugComponent()
49{
50 LOG_TRACE ("destructor");
51}
52
53/* *Called by the ComponentManager to start the component*/
54void LadybugComponent::startActivity()
55{
56 lbTotalMBWritten = 0.0;
57 lbTotalNumberOfImagesWritten = 0;
58 lbNumOfFrames = 0;
59
60 if (lbSaveJPGImages)
61 {
62 try
63 {
64 QString dbtFileName_ = "Camera_1.dbt";
65 mDbtImage1.open(dbtFileName_.toStdString(), WriteMode, FILE_DBT_UNKNOWN, kMaxFilepathLength);
66 } catch (DbiteException & e) {
67 LOG_ERROR("error opening dbt file: " << e.what());
68 }
69
70 try
71 {
72 QString dbtFileName_ = "Camera_2.dbt";
73 mDbtImage1.open(dbtFileName_.toStdString(), WriteMode, FILE_DBT_UNKNOWN, kMaxFilepathLength);
74 } catch (DbiteException & e) {
75 LOG_ERROR("error opening dbt file: " << e.what());
76 }
77
78 try
79 {
80 QString dbtFileName_ = "Camera_3.dbt";
81 mDbtImage1.open(dbtFileName_.toStdString(), WriteMode, FILE_DBT_UNKNOWN, kMaxFilepathLength);
82 } catch (DbiteException & e) {
83 LOG_ERROR("error opening dbt file: " << e.what());
84 }
85
86 try
87 {
88 QString dbtFileName_ = "Camera_4.dbt";
89 mDbtImage1.open(dbtFileName_.toStdString(), WriteMode, FILE_DBT_UNKNOWN, kMaxFilepathLength);
90 } catch (DbiteException & e) {
91 LOG_ERROR("error opening dbt file: " << e.what());
92 }
93
94 try
95 {
96 QString dbtFileName_ = "Camera_5.dbt";
97 mDbtImage1.open(dbtFileName_.toStdString(), WriteMode, FILE_DBT_UNKNOWN, kMaxFilepathLength);
98 } catch (DbiteException & e) {
99 LOG_ERROR("error opening dbt file: " << e.what());
100 }
101
102 try
103 {
104 QString dbtFileName_ = "Camera_6.dbt";
105 mDbtImage1.open(dbtFileName_.toStdString(), WriteMode, FILE_DBT_UNKNOWN, kMaxFilepathLength);
106 } catch (DbiteException & e) {
107 LOG_ERROR("error opening dbt file: " << e.what());
108 }
109 }
110
111 if (lbSavePanoImage)
112 {
113 try
114 {
115 QString dbtFileName_ = "CameraPano.dbt";
116 mDbtImage1.open(dbtFileName_.toStdString(), WriteMode, FILE_DBT_UNKNOWN, kMaxFilepathLength);
117 } catch (DbiteException & e) {
118 LOG_ERROR("error opening dbt file: " << e.what());
119 }
120 }
121
122 THREAD_ALIVE = true;
123 start();
124 //LOG_TRACE(Q_FUNC_INFO);
125}
126
127/* *Called by the ComponentManager to stop the component*/
128void LadybugComponent::stopActivity()
129{
130 printf (" \n*** Stopping Activity ***\n");
131 LadybugError error;
132
133 // Close DBT files
134 if (lbSaveJPGImages) {
135 mDbtImage1.close();
136 mDbtImage2.close();
137 mDbtImage3.close();
138 mDbtImage4.close();
139 mDbtImage5.close();
140 mDbtImage6.close();
141 }
142
143 if (lbSavePanoImage) {
144 mDbtImagePano.close();
145 }
146
147 lbIsRecording = false;
148 THREAD_ALIVE = false;
149
150 // wait the thread termination
151 if (!wait(1000)) {
152 LOG_ERROR(name() << ":The thread doesn't respond for 1 second, it has been terminated");
153 terminate();
154 }
155
156 /*error = ladybugStopStream (lbStreamContext);
157 HandleError(error);
158
159 // Allocate memory for the 6 processed images
160 for (unsigned int uiCamera = 0; uiCamera < LADYBUG_NUM_CAMERAS; uiCamera++)
161 {
162 delete lbArpBuffers[uiCamera];
163 }
164
165 printf( "Stopping camera...\n" );
166 error = ladybugStop(lbContext);
167 HandleError(error);
168
169 error = ladybugDestroyStreamContext ( &lbStreamContext );
170 HandleError(error);
171
172 // Destroy the context
173 printf( "Destroying context...\n" );
174 error = ladybugDestroyContext(&lbContext);
175 HandleError(error);*/
176}
177
178/* *Called by the ComponentManager to pass the XML parameters to the component*/
179ComponentBase::COMPONENT_CONFIGURATION LadybugComponent::configureComponent(XmlComponentConfig config)
180{
181 if (config.getProperty("saveVideoStream") != QString::null) {
182 lbSaveVideoStream = config.getProperty("saveVideoStream").toInt();
183 }
184 if (config.getProperty("saveJPGImages") != QString::null) {
185 lbSaveJPGImages = config.getProperty("saveJPGImages").toInt();
186 }
187 if (config.getProperty("savePanoImage") != QString::null) {
188 lbSavePanoImage = config.getProperty("savePanoImage").toInt();
189 }
190 if (config.getProperty("trigger") != QString::null) {
191 lbTriggerOn = config.getProperty("trigger").toInt();
192 }
193 if (config.getProperty("trigger_mode") != QString::null) {
194 lbTriggerMode = config.getProperty("trigger_mode").toInt();
195 }
196 if (config.getProperty("trigger_source") != QString::null) {
197 lbTriggerSource = config.getProperty("trigger_source").toInt();
198 }
199 if (config.getProperty("trigger_param") != QString::null) {
200 lbTriggerParam = config.getProperty("trigger_param").toInt();
201 }
202 if (config.getProperty("trigger_polarity") != QString::null) {
203 lbTriggerPolarity = config.getProperty("trigger_polarity").toInt();
204 }
205
206 return ComponentBase::CONFIGURED_OK;
207}
208
209/* *Initialize default values to the camera*/
210void LadybugComponent::InitDefault()
211{
212
213}
214
215/* *Thread loop*/
216void LadybugComponent::run()
217{
218 LadybugError error;
219 LadybugImage image;
220
221 StartCamera();
222 CreateImageBuffers();
223
224 // Timestamp
225 road_time_t timestamp = 0;
226 road_timerange_t tr = 0;
227
228 while (THREAD_ALIVE)
229 {
230
231 lbCurrentTime = GetTickCount(); // road_time_t timestamp = road_time(); // ui64 ; unit microsec ; Orig 01/01/1970 00:00
232 lbFrameCount++;
233
234 lbElapsedTime = lbCurrentTime - lbLastUpdateTime;
235 if (lbElapsedTime >= 1000)
236 {
237 std::cout << "FPS: " << (lbFrameCount * 1000.0 / lbElapsedTime) << std::endl;
238 lbFrameCount = 0;
239 lbLastUpdateTime = lbCurrentTime;
240 }
241
242 error = ladybugGrabImage( lbContext, &image );
243 timestamp = road_time(); // timestamp
244 HandleError(error);
245
246 if (error == LADYBUG_OK)
247 {
248 // Save videostream
249 if ( lbStreamContext != NULL && lbSaveVideoStream)
250 {
251 SaveVideoStream(&image);
252 }
253
254 // Save JPG for each camera
255 if (lbSaveJPGImages)
256 {
257 SaveJPGImages(&image);
258 }
259
260 // Save Panoramic Images
261 if (lbSavePanoImage)
262 {
263 SavePanoImage(&image);
264 }
265 lbNumOfFrames++;
266 }
267 }
268
269
270 //error = ladybugConvertImage( context, &image, NULL );
271 LOG_INFO("Finishing acquisition...\n");
272
273 if (lbSaveVideoStream)
274 {
275 error = ladybugStopStream (lbStreamContext);
276 HandleError(error);
277 }
278
279 triggerMode.bOnOff = false;
280
281 error = ladybugSetTriggerMode( lbContext, &triggerMode);
282 HandleError(error);
283
284 //
285 // Clean up the buffers
286 //
287 for( int uiCamera = 0; uiCamera < LADYBUG_NUM_CAMERAS; uiCamera++ )
288 {
289 delete [] lbArpBuffers[ uiCamera ];
290 }
291
292 printf (" \n*** Stopping Activity ***\n");
293
294 printf( "Stopping camera...\n" );
295 error = ladybugStop(lbContext);
296 HandleError(error);
297
298 error = ladybugDestroyStreamContext ( &lbStreamContext );
299 HandleError(error);
300
301 // Destroy the context
302 printf( "Destroying context...\n" );
303 error = ladybugDestroyContext(&lbContext);
304 HandleError(error);
305}
306
307void LadybugComponent::StartCamera()
308{
309 LadybugError error;
310
311 // Initialize context
312 error = ladybugCreateContext(&lbContext);
313 HandleError(error);
314
315 error = ladybugCreateStreamContext(&lbStreamContext);
316 HandleError(error);
317
318 error = ladybugInitializeFromIndex(lbContext, 0);
319 HandleError(error);
320
321 error = ladybugGetCameraInfo(lbContext, &lbCamInfo);
322 HandleError(error);
323
324 error = ladybugLoadConfig(lbContext, NULL);
325 HandleError(error);
326
327 error = ladybugSetAutoShutterRange(lbContext, LADYBUG_AUTO_SHUTTER_MOTION);
328 HandleError(error);
329
330
331 // Use it to trigger the camera with the gps signal
332 /*// Initialize Trigger
333 error = ladybugGetTriggerModeInfo( lbContext, &triggerModeInfo );
334 HandleError(error);
335
336 error = ladybugGetTriggerMode( lbContext, &triggerMode);
337 HandleError(error);
338
339 // Set camera to trigger mode 0
340 // A source of 7 means software trigger
341 triggerMode.bOnOff = false;
342 triggerMode.uiSource = 0; //7
343 triggerMode.uiMode = 0;
344 triggerMode.uiParameter = 0;
345 triggerMode.uiPolarity = 0;*/
346
347 error = ladybugSetPanoramicViewingAngle( lbContext, LADYBUG_FRONT_0_POLE_5);
348 HandleError(error);
349
350 error = ladybugSetColorProcessingMethod(lbContext, LADYBUG_NEAREST_NEIGHBOR_FAST);
351 HandleError(error);
352
353 // Configure output images in Ladybug library
354 error = ladybugConfigureOutputImages( lbContext, LADYBUG_PANORAMIC );
355 HandleError(error);
356
357 error = ladybugSetOffScreenImageSize(lbContext, LADYBUG_PANORAMIC, PANORAMIC_IMAGE_WIDTH, PANORAMIC_IMAGE_HEIGHT );
358 HandleError(error);
359
360 if (lbSaveVideoStream)
361 {
362 char pszOutputFilePath[256] = {0};
363 char pszStreamNameToOpen[256] = {0};
364 char buf[_MAX_PATH];
365 HRESULT hres = SHGetFolderPath( NULL, CSIDL_PERSONAL, NULL, 0, buf);
366 if ( hres == S_OK)
367 {
368 // Set stream name to "My Documents" folder
369 sprintf( pszStreamNameToOpen, "%s\\Ladybug\\PGR\\%s\0", buf, "LadybugCameraStream" );
370 }
371 else
372 {
373 // Set stream name to current folder
374 strcpy( pszOutputFilePath, buf);
375 }
376
377 error = ladybugInitializeStreamForWriting(lbStreamContext, pszStreamNameToOpen, lbContext, pszOutputFilePath, false);
378 HandleError(error);
379 //printf( "Recording to %s\n", pszOutputFilePath );
380 }
381
382 printf("Starting %s (%u)...\n", lbCamInfo.pszModelName, lbCamInfo.serialHead);
383 error = ladybugStart(lbContext, LADYBUG_DATAFORMAT_HALF_HEIGHT_RAW8 );
384 HandleError(error);
385}
386
387void LadybugComponent::SaveVideoStream(LadybugImage* image)
388{
389 LadybugError error;
390 error = ladybugWriteImageToStream( lbStreamContext, image, &lbTotalMBWritten, &lbTotalNumberOfImagesWritten );
391 if ( error != LADYBUG_OK )
392 {
393 printf("Error while saving images...\n");
394 //
395 // Stop recording if a write error occurs
396 // If the return value is LADYBUG_ERROR_DISK_NOT_ENOUGH_SPACE,
397 // the disk is full.
398 //
399 lbIsRecording = false;
400 ladybugStopStream ( lbStreamContext );
401 _DISPLAY_ERROR_MSG_AND_RETURN
402 }
403}
404
405void LadybugComponent::SaveJPGImages(LadybugImage* image)
406{
407 LadybugError error;
408 cv::Mat col_img;
409
410 char pszPath[256] = {0};
411 char pszOutputFilePath[256] = {0};
412 char buf[_MAX_PATH];
413 HRESULT hres = SHGetFolderPath( NULL, CSIDL_PERSONAL, NULL, 0, buf);
414 if ( hres == S_OK)
415 {
416 sprintf( pszPath, "%s\\Ladybug\\Images\0", buf);
417 }
418 else
419 {
420 // Set stream name to current folder
421 strcpy( pszPath, buf);
422 }
423
424 error = ladybugConvertImage(lbContext, image, lbArpBuffers, LADYBUG_BGRU);
425 HandleError(error);
426
427 // Save the image as 6 individual raw (unstitched, distorted) images
428 for (unsigned int uiCamera = 0; uiCamera < LADYBUG_NUM_CAMERAS; uiCamera++)
429 {
430 LadybugProcessedImage processedImage;
431 processedImage.pData = lbArpBuffers[uiCamera];
432 processedImage.pixelFormat = LADYBUG_BGRU;
433 processedImage.uiCols = image->uiCols;
434 processedImage.uiRows = image->uiRows;
435
436 col_img = cv::Mat(processedImage.uiRows, processedImage.uiCols, CV_8UC4);
437 col_img.data = processedImage.pData;
438
439 sprintf(pszOutputFilePath, "%s\\ladybug_%d_cam_%02u.bmp", pszPath, lbTotalNumberOfImagesWritten, uiCamera);
440 error = ladybugSaveImage(lbContext, &processedImage, pszOutputFilePath, LADYBUG_FILEFORMAT_BMP);
441 HandleError(error);
442
443 if (uiCamera == 0)
444 {
445 try {
446 mDbtImage1.writeRecord(time, tr, (char *) processedImage.pData, kMaxFilepathLength);
447 } catch (DbiteException & e) {
448 LOG_ERROR("error writing data: " << e.what());
449 }
450 } else if (uiCamera == 1) {
451 try {
452 mDbtImage2.writeRecord(time, tr, (char *) processedImage.pData, kMaxFilepathLength);
453 } catch (DbiteException & e) {
454 LOG_ERROR("error writing data: " << e.what());
455 }
456 }else if (uiCamera == 2) {
457 try {
458 mDbtImage3.writeRecord(time, tr, (char *) processedImage.pData, kMaxFilepathLength);
459 } catch (DbiteException & e) {
460 LOG_ERROR("error writing data: " << e.what());
461 }
462 }else if (uiCamera == 3) {
463 try {
464 mDbtImage4.writeRecord(time, tr, (char *) processedImage.pData, kMaxFilepathLength);
465 } catch (DbiteException & e) {
466 LOG_ERROR("error writing data: " << e.what());
467 }
468 }else if (uiCamera == 4) {
469 try {
470 mDbtImage5.writeRecord(time, tr, (char *) processedImage.pData, kMaxFilepathLength);
471 } catch (DbiteException & e) {
472 LOG_ERROR("error writing data: " << e.what());
473 }
474 }else if (uiCamera == 5) {
475 try {
476 mDbtImage6.writeRecord(time, tr, (char *) processedImage.pData, kMaxFilepathLength);
477 } catch (DbiteException & e) {
478 LOG_ERROR("error writing data: " << e.what());
479 }
480 }
481
482 //printf("Saved camera %u image to %s.\n", uiCamera, pszOutputFilePath);
483
484 /*// Showing the result
485 cv::String name = cv::format("Gray%d",uiCamera);
486 cv::resize(col_img, col_img, cv::Size(), 0.25, 0.25, cv::INTER_NEAREST);
487 cv::namedWindow( name, CV_WINDOW_AUTOSIZE );
488 cv::imshow( name, col_img );
489 cv::waitKey(10);*/
490 }
491}
492
493void LadybugComponent::SavePanoImage(LadybugImage* image)
494{
495 LadybugError error;
496 char pszPath[256] = {0};
497 char pszOutputFilePath[256] = {0};
498 char buf[_MAX_PATH];
499 HRESULT hres = SHGetFolderPath( NULL, CSIDL_PERSONAL, NULL, 0, buf);
500 if ( hres == S_OK)
501 {
502 sprintf( pszPath, "%s\\Ladybug\\Panoramic\0", buf);
503 }
504 else
505 {
506 // Set stream name to current folder
507 strcpy( pszPath, buf);
508 }
509
510 // Convert the image to 6 RGB buffers
511 error = ladybugConvertImage(lbContext, image, lbArpBuffers);
512 HandleError(error);
513
514 // Send the RGB buffers to the graphics card
515 error = ladybugUpdateTextures(lbContext, LADYBUG_NUM_CAMERAS, const_cast<const unsigned char**>( lbArpBuffers ));
516 HandleError(error);
517
518 // Stitch the images (inside the graphics card) and retrieve the output to the user's memory
519 LadybugProcessedImage processedImage;
520 error = ladybugRenderOffScreenImage(lbContext, LADYBUG_PANORAMIC, LADYBUG_BGR, &processedImage);
521 HandleError(error);
522
523 // Save the output image to a file
524 sprintf(pszOutputFilePath, "%s\\PanoStitchOutput_%d.jpg", pszPath, lbNumOfFrames);
525 error = ladybugSaveImage(lbContext, &processedImage, pszOutputFilePath, LADYBUG_FILEFORMAT_JPG);
526 HandleError(error);
527
528 try {
529 mDbtImagePano.writeRecord(time, tr, (char *) processedImage.pData, kMaxFilepathLength);
530 } catch (DbiteException & e) {
531 LOG_ERROR("error writing data: " << e.what());
532 }
533
534}
535
536void LadybugComponent::CreateImageBuffers()
537{
538 LadybugError error;
539 LadybugImage image;
540
541 // Creating buffers for images
542 error = LADYBUG_FAILED;
543 for ( int i = 0; i < 10 && error != LADYBUG_OK; i++)
544 {
545 error = ladybugGrabImage(lbContext, &image);
546 }
547 HandleError(error);
548
549 unsigned int uiRawCols, uiRawRows;
550 uiRawCols = image.uiCols;// / 2;
551 uiRawRows = image.uiRows;// / 2;
552
553 error = ladybugInitializeAlphaMasks( lbContext, uiRawCols, uiRawRows );
554 HandleError(error);
555 error = ladybugSetAlphaMasking( lbContext, true );
556 HandleError(error);
557
558 /// Allocate memory for the 6 processed images
559 for (unsigned int uiCamera = 0; uiCamera < LADYBUG_NUM_CAMERAS; uiCamera++)
560 {
561 lbArpBuffers[uiCamera] = new unsigned char[image.uiRows * image.uiCols * 4];
562 // Initialize the entire buffer so that the alpha channel has a valid (maximum) value.
563 memset(lbArpBuffers[uiCamera], 0xff, image.uiRows * image.uiCols * 4);
564 }
565}
566
567void LadybugComponent::HandleError(LadybugError error)
568{
569 if( error != LADYBUG_OK )
570 {
571 printf ("Error: Ladybug library reported - %s\n", ladybugErrorToString( error ) );
572 exit( 1 );
573 }
574 //else
575 //printf("ok...\n");
576};
577
578}
Note: See TracBrowser for help on using the repository browser.