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

Last change on this file since 82 was 81, checked in by ndiasnak, 10 years ago

add ladybug interface component

File size: 12.2 KB
RevLine 
[81]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");
34
35/* *Constructor*/
36LadybugComponent::LadybugComponent(QString name) : ComponentBase(name)
37{
38 lbFrameCount = 0;
39 lbCurrentTime = 0;
40 lbElapsedTime = 0;
41 lbLastUpdateTime = 0;
42
43 LOG_TRACE ("constructor(" << name << ")");
44}
45
46/* *Destructor */
47LadybugComponent::~LadybugComponent()
48{
49 LOG_TRACE ("destructor");
50}
51
52/* *Called by the ComponentManager to start the component*/
53void LadybugComponent::startActivity()
54{
55 lbTotalMBWritten = 0.0;
56 lbTotalNumberOfImagesWritten = 0;
57 lbNumOfFrames = 0;
58 THREAD_ALIVE = true;
59 start();
60 //LOG_TRACE(Q_FUNC_INFO);
61}
62
63/* *Called by the ComponentManager to stop the component*/
64void LadybugComponent::stopActivity()
65{
66 printf (" \n*** Stopping Activity ***\n");
67 LadybugError error;
68
69 lbIsRecording = false;
70 THREAD_ALIVE = false;
71
72 // wait the thread termination
73 if (!wait(1000)) {
74 LOG_ERROR(name() << ":The thread doesn't respond for 1 second, it has been terminated");
75 terminate();
76 }
77
78 /*error = ladybugStopStream (lbStreamContext);
79 HandleError(error);
80
81 // Allocate memory for the 6 processed images
82 for (unsigned int uiCamera = 0; uiCamera < LADYBUG_NUM_CAMERAS; uiCamera++)
83 {
84 delete lbArpBuffers[uiCamera];
85 }
86
87 printf( "Stopping camera...\n" );
88 error = ladybugStop(lbContext);
89 HandleError(error);
90
91 error = ladybugDestroyStreamContext ( &lbStreamContext );
92 HandleError(error);
93
94 // Destroy the context
95 printf( "Destroying context...\n" );
96 error = ladybugDestroyContext(&lbContext);
97 HandleError(error);*/
98}
99
100/* *Called by the ComponentManager to pass the XML parameters to the component*/
101ComponentBase::COMPONENT_CONFIGURATION LadybugComponent::configureComponent(XmlComponentConfig config)
102{
103 if (config.getProperty("saveVideoStream") != QString::null) {
104 lbSaveVideoStream = config.getProperty("saveVideoStream").toInt();
105 }
106 if (config.getProperty("saveJPGImages") != QString::null) {
107 lbSaveJPGImages = config.getProperty("saveJPGImages").toInt();
108 }
109 if (config.getProperty("savePanoImage") != QString::null) {
110 lbSavePanoImage = config.getProperty("savePanoImage").toInt();
111 }
112 if (config.getProperty("trigger") != QString::null) {
113 lbTriggerOn = config.getProperty("trigger").toInt();
114 }
115 if (config.getProperty("trigger_mode") != QString::null) {
116 lbTriggerMode = config.getProperty("trigger_mode").toInt();
117 }
118 if (config.getProperty("trigger_source") != QString::null) {
119 lbTriggerSource = config.getProperty("trigger_source").toInt();
120 }
121 if (config.getProperty("trigger_param") != QString::null) {
122 lbTriggerParam = config.getProperty("trigger_param").toInt();
123 }
124 if (config.getProperty("trigger_polarity") != QString::null) {
125 lbTriggerPolarity = config.getProperty("trigger_polarity").toInt();
126 }
127
128 return ComponentBase::CONFIGURED_OK;
129}
130
131/* *Initialize default values to the camera*/
132void LadybugComponent::InitDefault()
133{
134
135}
136
137/* *Thread loop*/
138void LadybugComponent::run()
139{
140 LadybugError error;
141 LadybugImage image;
142
143 StartCamera();
144 CreateImageBuffers();
145
146 while (THREAD_ALIVE)
147 {
148 lbCurrentTime = GetTickCount(); // road_time_t timestamp = road_time(); // ui64 ; unit microsec ; Orig 01/01/1970 00:00
149 lbFrameCount++;
150
151 lbElapsedTime = lbCurrentTime - lbLastUpdateTime;
152 if (lbElapsedTime >= 1000)
153 {
154 std::cout << "FPS: " << (lbFrameCount * 1000.0 / lbElapsedTime) << std::endl;
155 lbFrameCount = 0;
156 lbLastUpdateTime = lbCurrentTime;
157 }
158
159 error = ladybugGrabImage( lbContext, &image );
160 // t = road_time(); // timestamp
161 HandleError(error);
162
163 if (error == LADYBUG_OK)
164 {
165 // Save videostream
166 if ( lbStreamContext != NULL && lbSaveVideoStream)
167 {
168 SaveVideoStream(&image);
169 }
170
171 // Save JPG for each camera
172 if (lbSaveJPGImages)
173 {
174 SaveJPGImages(&image);
175 }
176
177 // Save Panoramic Images
178 if (lbSavePanoImage)
179 {
180 SavePanoImage(&image);
181 }
182 lbNumOfFrames++;
183 }
184 }
185
186
187 //error = ladybugConvertImage( context, &image, NULL );
188 LOG_INFO("Finishing acquisition...\n");
189
190 if (lbSaveVideoStream)
191 {
192 error = ladybugStopStream (lbStreamContext);
193 HandleError(error);
194 }
195
196 triggerMode.bOnOff = false;
197
198 error = ladybugSetTriggerMode( lbContext, &triggerMode);
199 HandleError(error);
200
201 //
202 // Clean up the buffers
203 //
204 for( int uiCamera = 0; uiCamera < LADYBUG_NUM_CAMERAS; uiCamera++ )
205 {
206 delete [] lbArpBuffers[ uiCamera ];
207 }
208
209 printf (" \n*** Stopping Activity ***\n");
210
211 printf( "Stopping camera...\n" );
212 error = ladybugStop(lbContext);
213 HandleError(error);
214
215 error = ladybugDestroyStreamContext ( &lbStreamContext );
216 HandleError(error);
217
218 // Destroy the context
219 printf( "Destroying context...\n" );
220 error = ladybugDestroyContext(&lbContext);
221 HandleError(error);
222}
223
224void LadybugComponent::StartCamera()
225{
226 LadybugError error;
227
228 // Initialize context
229 error = ladybugCreateContext(&lbContext);
230 HandleError(error);
231
232 error = ladybugCreateStreamContext(&lbStreamContext);
233 HandleError(error);
234
235 error = ladybugInitializeFromIndex(lbContext, 0);
236 HandleError(error);
237
238 error = ladybugGetCameraInfo(lbContext, &lbCamInfo);
239 HandleError(error);
240
241 error = ladybugLoadConfig(lbContext, NULL);
242 HandleError(error);
243
244 error = ladybugSetAutoShutterRange(lbContext, LADYBUG_AUTO_SHUTTER_MOTION);
245 HandleError(error);
246
247 /*// Initialize Trigger
248 error = ladybugGetTriggerModeInfo( lbContext, &triggerModeInfo );
249 HandleError(error);
250
251 error = ladybugGetTriggerMode( lbContext, &triggerMode);
252 HandleError(error);
253
254 // Set camera to trigger mode 0
255 // A source of 7 means software trigger
256 triggerMode.bOnOff = false;
257 triggerMode.uiSource = 0; //7
258 triggerMode.uiMode = 0;
259 triggerMode.uiParameter = 0;
260 triggerMode.uiPolarity = 0;*/
261
262 error = ladybugSetPanoramicViewingAngle( lbContext, LADYBUG_FRONT_0_POLE_5);
263 HandleError(error);
264
265 error = ladybugSetColorProcessingMethod(lbContext, LADYBUG_NEAREST_NEIGHBOR_FAST);
266 HandleError(error);
267
268 // Configure output images in Ladybug library
269 error = ladybugConfigureOutputImages( lbContext, LADYBUG_PANORAMIC );
270 HandleError(error);
271
272 error = ladybugSetOffScreenImageSize(lbContext, LADYBUG_PANORAMIC, PANORAMIC_IMAGE_WIDTH, PANORAMIC_IMAGE_HEIGHT );
273 HandleError(error);
274
275 if (lbSaveVideoStream)
276 {
277 char pszOutputFilePath[256] = {0};
278 char pszStreamNameToOpen[256] = {0};
279 char buf[_MAX_PATH];
280 HRESULT hres = SHGetFolderPath( NULL, CSIDL_PERSONAL, NULL, 0, buf);
281 if ( hres == S_OK)
282 {
283 // Set stream name to "My Documents" folder
284 sprintf( pszStreamNameToOpen, "%s\\Ladybug\\PGR\\%s\0", buf, "LadybugCameraStream" );
285 }
286 else
287 {
288 // Set stream name to current folder
289 strcpy( pszOutputFilePath, buf);
290 }
291
292 error = ladybugInitializeStreamForWriting(lbStreamContext, pszStreamNameToOpen, lbContext, pszOutputFilePath, false);
293 HandleError(error);
294 //printf( "Recording to %s\n", pszOutputFilePath );
295 }
296
297 printf("Starting %s (%u)...\n", lbCamInfo.pszModelName, lbCamInfo.serialHead);
298 error = ladybugStart(lbContext, LADYBUG_DATAFORMAT_HALF_HEIGHT_RAW8 );
299 HandleError(error);
300}
301
302void LadybugComponent::SaveVideoStream(LadybugImage* image)
303{
304 LadybugError error;
305 error = ladybugWriteImageToStream( lbStreamContext, image, &lbTotalMBWritten, &lbTotalNumberOfImagesWritten );
306 if ( error != LADYBUG_OK )
307 {
308 printf("Error while saving images...\n");
309 //
310 // Stop recording if a write error occurs
311 // If the return value is LADYBUG_ERROR_DISK_NOT_ENOUGH_SPACE,
312 // the disk is full.
313 //
314 lbIsRecording = false;
315 ladybugStopStream ( lbStreamContext );
316 _DISPLAY_ERROR_MSG_AND_RETURN
317 }
318}
319
320void LadybugComponent::SaveJPGImages(LadybugImage* image)
321{
322 LadybugError error;
323 cv::Mat col_img;
324
325 char pszPath[256] = {0};
326 char pszOutputFilePath[256] = {0};
327 char buf[_MAX_PATH];
328 HRESULT hres = SHGetFolderPath( NULL, CSIDL_PERSONAL, NULL, 0, buf);
329 if ( hres == S_OK)
330 {
331 sprintf( pszPath, "%s\\Ladybug\\Images\0", buf);
332 }
333 else
334 {
335 // Set stream name to current folder
336 strcpy( pszPath, buf);
337 }
338
339 error = ladybugConvertImage(lbContext, image, lbArpBuffers, LADYBUG_BGRU);
340 HandleError(error);
341
342 // Save the image as 6 individual raw (unstitched, distorted) images
343 for (unsigned int uiCamera = 0; uiCamera < LADYBUG_NUM_CAMERAS; uiCamera++)
344 {
345 LadybugProcessedImage processedImage;
346 processedImage.pData = lbArpBuffers[uiCamera];
347 processedImage.pixelFormat = LADYBUG_BGRU;
348 processedImage.uiCols = image->uiCols;
349 processedImage.uiRows = image->uiRows;
350
351 col_img = cv::Mat(processedImage.uiRows, processedImage.uiCols, CV_8UC4);
352 col_img.data = processedImage.pData;
353
354 sprintf(pszOutputFilePath, "%s\\ladybug_%d_cam_%02u.bmp", pszPath, lbTotalNumberOfImagesWritten, uiCamera);
355 error = ladybugSaveImage(lbContext, &processedImage, pszOutputFilePath, LADYBUG_FILEFORMAT_BMP);
356 HandleError(error);
357
358 //printf("Saved camera %u image to %s.\n", uiCamera, pszOutputFilePath);
359
360 /*// Showing the result
361 cv::String name = cv::format("Gray%d",uiCamera);
362 cv::resize(col_img, col_img, cv::Size(), 0.25, 0.25, cv::INTER_NEAREST);
363 cv::namedWindow( name, CV_WINDOW_AUTOSIZE );
364 cv::imshow( name, col_img );
365 cv::waitKey(10);*/
366 }
367}
368
369void LadybugComponent::SavePanoImage(LadybugImage* image)
370{
371 LadybugError error;
372 char pszPath[256] = {0};
373 char pszOutputFilePath[256] = {0};
374 char buf[_MAX_PATH];
375 HRESULT hres = SHGetFolderPath( NULL, CSIDL_PERSONAL, NULL, 0, buf);
376 if ( hres == S_OK)
377 {
378 sprintf( pszPath, "%s\\Ladybug\\Panoramic\0", buf);
379 }
380 else
381 {
382 // Set stream name to current folder
383 strcpy( pszPath, buf);
384 }
385
386 // Convert the image to 6 RGB buffers
387 error = ladybugConvertImage(lbContext, image, lbArpBuffers);
388 HandleError(error);
389
390 // Send the RGB buffers to the graphics card
391 error = ladybugUpdateTextures(lbContext, LADYBUG_NUM_CAMERAS, const_cast<const unsigned char**>( lbArpBuffers ));
392 HandleError(error);
393
394 // Stitch the images (inside the graphics card) and retrieve the output to the user's memory
395 LadybugProcessedImage processedImage;
396 error = ladybugRenderOffScreenImage(lbContext, LADYBUG_PANORAMIC, LADYBUG_BGR, &processedImage);
397 HandleError(error);
398
399 // Save the output image to a file
400 sprintf(pszOutputFilePath, "%s\\PanoStitchOutput_%d.jpg", pszPath, lbNumOfFrames);
401 error = ladybugSaveImage(lbContext, &processedImage, pszOutputFilePath, LADYBUG_FILEFORMAT_JPG);
402 HandleError(error);
403
404}
405
406void LadybugComponent::CreateImageBuffers()
407{
408 LadybugError error;
409 LadybugImage image;
410
411 // Creating buffers for images
412 error = LADYBUG_FAILED;
413 for ( int i = 0; i < 10 && error != LADYBUG_OK; i++)
414 {
415 error = ladybugGrabImage(lbContext, &image);
416 }
417 HandleError(error);
418
419 unsigned int uiRawCols, uiRawRows;
420 uiRawCols = image.uiCols;// / 2;
421 uiRawRows = image.uiRows;// / 2;
422
423 error = ladybugInitializeAlphaMasks( lbContext, uiRawCols, uiRawRows );
424 HandleError(error);
425 error = ladybugSetAlphaMasking( lbContext, true );
426 HandleError(error);
427
428 /// Allocate memory for the 6 processed images
429 for (unsigned int uiCamera = 0; uiCamera < LADYBUG_NUM_CAMERAS; uiCamera++)
430 {
431 lbArpBuffers[uiCamera] = new unsigned char[image.uiRows * image.uiCols * 4];
432 // Initialize the entire buffer so that the alpha channel has a valid (maximum) value.
433 memset(lbArpBuffers[uiCamera], 0xff, image.uiRows * image.uiCols * 4);
434 }
435}
436
437void LadybugComponent::HandleError(LadybugError error)
438{
439 if( error != LADYBUG_OK )
440 {
441 printf ("Error: Ladybug library reported - %s\n", ladybugErrorToString( error ) );
442 exit( 1 );
443 }
444 //else
445 //printf("ok...\n");
446};
447
448}
Note: See TracBrowser for help on using the repository browser.