source: pacpussensors/trunk/PtGreyCameras/BumblebeeXB3.cpp@ 113

Last change on this file since 113 was 48, checked in by phudelai, 11 years ago

Flea3 repository name changed for PtGreyCameras more general name
File size: 16.3 KB
RevLine 
[48]1/// Point Grey Research Bumblebee XB3 acquisition component.
2///
3/// created @date 2012-07-25
4/// @author Marek Kurdej
5/// @version $Id: $
6
7#include "BumblebeeXB3.h"
8
9#include <cassert>
10#include <iomanip>
11#include <iostream>
12#include <PGRFlyCaptureStereo.h>
13#include <qdir.h>
14#include <qscopedpointer.h>
15#include <string>
16#include <sstream>
17
18#include "Pacpus/kernel/ComponentFactory.h"
19#include "Pacpus/kernel/DbiteException.h"
20#include "Pacpus/kernel/DbiteFileTypes.h"
21#include "Pacpus/kernel/Log.h"
22
23using namespace std;
24
25namespace pacpus {
26
27DECLARE_STATIC_LOGGER("pacpus.base.BumblebeeXB3");
28
29/// Construct the factory
30ComponentFactory<BumblebeeXB3> sFactory("BumblebeeXB3");
31
32static const size_t kMaxFilepathLength = 128; // TODO: should be same for all images
33
34/// Macro to check, report on, and handle Flycapture API error codes.
35#define _HANDLE_FLYCAPTURE_ERROR( description, error ) \
36{ \
37 if ( error != FLYCAPTURE_OK ) { \
38 LOG_FATAL( \
39 "*** Flycapture Error '" << flycaptureErrorToString( error ) \
40 << "' at line " << __LINE__ \
41 << ":\n\t" << description \
42 ); \
43 } \
44}
45
46/// Macro to check, report on, and handle Triclops API error codes.
47#define _HANDLE_TRICLOPS_ERROR( description, error ) \
48{ \
49 if ( error != TriclopsErrorOk ) { \
50 LOG_FATAL( \
51 "*** Triclops Error '" << triclopsErrorToString( error ) \
52 << "' at line " << __LINE__ \
53 << " :\n\t" << description \
54 ); \
55 } \
56}
57
58BumblebeeXB3::BumblebeeXB3(QString name)
59 : ComponentBase(name)
60 , mTriclops(NULL)
61 , mFlycapture(NULL)
62 , mIsRecording(false)
63 , mVerbosityLevel(0)
64 , mShMemLeft(NULL)
65 //, mShMemTop(NULL)
66 , mShMemRight(NULL)
67{
68}
69
70BumblebeeXB3::~BumblebeeXB3()
71{
72}
73
74ComponentBase::COMPONENT_CONFIGURATION BumblebeeXB3::configureComponent(XmlComponentConfig config)
75{
76 if (config.getProperty("recording") != QString::null) {
77 mIsRecording = (0 < config.getProperty("recording").toInt());
78 }
79 if (config.getProperty("verbose") != QString::null) {
80 mVerbosityLevel = config.getProperty("verbose").toInt();
81 }
82
83 return ComponentBase::CONFIGURED_OK;
84}
85
86void BumblebeeXB3::startActivity()
87{
88 TriclopsError te;
89 FlyCaptureError fe;
90
91 // Open the camera
92 fe = flycaptureCreateContext( &mFlycapture );
93 _HANDLE_FLYCAPTURE_ERROR( "flycaptureCreateContext()", fe );
94 if ( fe != FLYCAPTURE_OK ) {
95 return;
96 }
97
98 // Initialize the Flycapture context, device no. 0
99 fe = flycaptureInitialize( mFlycapture, 0 );
100 _HANDLE_FLYCAPTURE_ERROR( "flycaptureInitialize()", fe );
101 if ( fe != FLYCAPTURE_OK ) {
102 return;
103 }
104
105 // Save the camera's calibration file, and return the path
106 char * szCalFile;
107 fe = flycaptureGetCalibrationFileFromCamera( mFlycapture, &szCalFile );
108 _HANDLE_FLYCAPTURE_ERROR( "flycaptureGetCalibrationFileFromCamera()", fe );
109 if ( fe != FLYCAPTURE_OK ) {
110 return;
111 }
112
113 LOG_DEBUG("FlyCapture camera configuration file: " << szCalFile);
114
115 // Create a Triclops context from the cameras calibration file
116 te = triclopsGetDefaultContextFromFile( &mTriclops, szCalFile );
117 _HANDLE_TRICLOPS_ERROR( "triclopsGetDefaultContextFromFile()", te );
118 if (te != TriclopsErrorOk) {
119 return;
120 }
121
122 // Get camera information
123 FlyCaptureInfoEx pInfo;
124 fe = flycaptureGetCameraInfo( mFlycapture, &pInfo );
125 _HANDLE_FLYCAPTURE_ERROR( "flycatpureGetCameraInfo()", fe );
126 if ( fe != FLYCAPTURE_OK ) {
127 return;
128 }
129
130 // Get pixel format
131 if (pInfo.CameraType == FLYCAPTURE_COLOR) {
132 mPixelFormat = FLYCAPTURE_RAW16;
133 } else {
134 mPixelFormat = FLYCAPTURE_MONO16;
135 }
136
137 // Get max image size
138 mMaxRows = mMaxCols = 0;
139
140 switch (pInfo.CameraModel) {
141 case FLYCAPTURE_BUMBLEBEE2:
142 {
143 unsigned long ulValue;
144 const unsigned long ulRegister = 0x1F28;
145 flycaptureGetCameraRegister( mFlycapture, ulRegister, &ulValue );
146
147 const unsigned long ulMask = 0x2;
148 if ( ( ulValue & ulMask ) == 0 ) {
149 // Hi-res BB2
150 mMaxCols = 1024;
151 mMaxRows = 768;
152 } else {
153 // Low-res BB2
154 mMaxCols = 640;
155 mMaxRows = 480;
156 }
157 }
158 break;
159
160 case FLYCAPTURE_BUMBLEBEEXB3:
161 mMaxCols = 1280;
162 mMaxRows = 960;
163 break;
164
165 default:
166 te = TriclopsErrorInvalidCamera;
167 _HANDLE_TRICLOPS_ERROR( "triclopsCheckCameraModel()", te );
168 break;
169 }
170
171 // Create shared memory segments
172 mMaxImageSize = mMaxCols * mMaxRows * 4 /* RGBA = 32bpp */;
173 mShMemLeft = new ShMem((/*ComponentBase::componentName*/name() + "-left").toStdString().c_str(), mMaxImageSize);
174 //mShMemTop = new ShMem((ComponentBase::componentName + "-top").toStdString().c_str(), mMaxImageSize);
175 mShMemRight = new ShMem((/*ComponentBase::componentName*/name() + "-right").toStdString().c_str(), mMaxImageSize);
176
177 // Create output files for recording
178 if (mIsRecording) {
179 try {
180 QString dbtFileName_ = /*ComponentBase::componentName*/name() + "-left.dbt";
181 mDbtLeft.open(dbtFileName_.toStdString(), WriteMode, STEREO_LEFT_IMAGE, kMaxFilepathLength);
182 } catch (DbiteException & e) {
183 LOG_ERROR("error opening dbt file: " << e.what());
184 }
185 //try {
186 // QString dbtFileName_ = ComponentBase::componentName + "-top.dbt";
187 // mDbtTop.open(dbtFileName_.toStdString(), WriteMode, STEREO_TOP_IMAGE, kMaxFilepathLength);
188 //} catch (DbiteException & e) {
189 // LOG_ERROR("error opening dbt file: " << e.what());
190 //}
191 try {
192 QString dbtFileName_ = /*ComponentBase::componentName*/name() + "-right.dbt";
193 mDbtRight.open(dbtFileName_.toStdString(), WriteMode, STEREO_RIGHT_IMAGE, kMaxFilepathLength);
194 } catch (DbiteException & e) {
195 LOG_ERROR("error opening dbt file: " << e.what());
196 }
197 }
198
199 // Run thread
200 /*ComponentBase::*/THREAD_ALIVE = true;
201 // execute main thread loop run()
202 start();
203}
204
205void BumblebeeXB3::stopActivity()
206{
207 /*ComponentBase::*/THREAD_ALIVE = false;
208
209 // wait the thread termination
210 if (!wait(1000)) {
211 LOG_ERROR(name() << ":The thread doesn't respond for 1 second, it has been terminated");
212 terminate();
213 }
214
215 TriclopsError te;
216 FlyCaptureError fe;
217
218 // Close the camera
219 if (mFlycapture) {
220 fe = flycaptureStop( mFlycapture );
221 _HANDLE_FLYCAPTURE_ERROR( "flycaptureStop()", fe );
222 }
223
224 // Destroy the Triclops context
225 if (mTriclops) {
226 te = triclopsDestroyContext( mTriclops ) ;
227 mTriclops = NULL;
228 _HANDLE_TRICLOPS_ERROR("triclopsDestroyContext()", te);
229 }
230
231 // Destroy the Flycapture context
232 if (mFlycapture) {
233 fe = flycaptureDestroyContext( mFlycapture );
234 mFlycapture = NULL;
235 _HANDLE_FLYCAPTURE_ERROR( "flycaptureDestroyContext()", fe );
236 }
237
238 // Close DBT files
239 if (mIsRecording) {
240 mDbtLeft.close();
241 //mDbtTop.close();
242 mDbtRight.close();
243 }
244
245 // Delete shared memory segments
246 delete mShMemLeft; mShMemLeft = NULL;
247 //delete mShMemTop; mShMemTop = NULL;
248 delete mShMemRight; mShMemRight = NULL;
249}
250
251void BumblebeeXB3::run()
252{
253 TriclopsError te;
254 FlyCaptureError fe;
255
256 // Start transferring images from the camera to the computer
257 const unsigned int uiMode = 3;
258 const unsigned int uiImagePosLeft = 0, uiImagePosTop = 0;
259 const float fBandwidth = 100; // percent
260 fe = flycaptureStartCustomImage(mFlycapture, uiMode, uiImagePosLeft, uiImagePosTop, mMaxCols, mMaxRows, fBandwidth, mPixelFormat);
261 _HANDLE_FLYCAPTURE_ERROR( "flycaptureStartCustomImage()", fe );
262 if (fe != FLYCAPTURE_OK) {
263 return;
264 }
265
266 // set rectified resolution
267 te = triclopsSetResolution( mTriclops, mMaxRows, mMaxCols );
268 _HANDLE_TRICLOPS_ERROR( "triclopsSetResolution()", te );
269 if (te != TriclopsErrorOk) {
270 return;
271 }
272
273 FlyCaptureImage flycaptureImage;
274
275 size_t imageCount = 0;
276
277 QDir outputDirectory;
278 if (mIsRecording) {
279 outputDirectory.mkdir(/*ComponentBase::componentName*/name());
280 outputDirectory.setPath(/*ComponentBase::componentName*/name());
281 }
282
283 const char * const imagePrefix = "rectified";
284 const char * const imageNameDelimiter = "-";
285 const char * const imageExtension = (mPixelFormat == FLYCAPTURE_RAW16) ? ".ppm" : ".pgm";
286
287 const char kFillCharacter = '0';
288 const int kNumberWidth = 5;
289
290 /////////////////////
291 // Grab an image from the camera
292 fe = flycaptureGrabImage2( mFlycapture, &flycaptureImage );
293 _HANDLE_FLYCAPTURE_ERROR( "flycaptureGrabImage()", fe );
294 if (fe != FLYCAPTURE_OK) {
295 return;
296 }
297
298 // Extract information from the FlycaptureImage
299 int imageCols = flycaptureImage.iCols;
300 int imageRows = flycaptureImage.iRows;
301 int imageRowInc = flycaptureImage.iRowInc;
302 int iSideBySideImages = flycaptureImage.iNumImages;
303 LOG_INFO("imageCols = " << imageCols);
304 LOG_INFO("imageRows = " << imageRows);
305 LOG_INFO("imageRowInc = " << imageRowInc);
306 LOG_INFO("iSideBySideImages = " << iSideBySideImages);
307
308 /////////////////////
309 // Create buffers for holding the color and mono images
310 QScopedArrayPointer<unsigned char> rowIntColor(new unsigned char[ imageCols * imageRows * iSideBySideImages * 4 /* RGBA = 32bpp */ ]);
311
312 // Pointers to positions in the color buffer that correspond to the beginning
313 // of the red, green and blue sections
314 unsigned char * redColor = NULL;
315 unsigned char * greenColor = NULL;
316 unsigned char * blueColor = NULL;
317
318 redColor = rowIntColor.data();
319 if (flycaptureImage.iNumImages == 2) {
320 greenColor = redColor + ( 4 * imageCols );
321 blueColor = redColor + ( 4 * imageCols );
322 }
323 if (flycaptureImage.iNumImages == 3) {
324 greenColor = redColor + ( 4 * imageCols );
325 blueColor = redColor + ( 2 * 4 * imageCols );
326 }
327
328 /////////////////////
329 // Create a temporary FlyCaptureImage for preparing the stereo image
330 FlyCaptureImage tempImage;
331 tempImage.pData = rowIntColor.data();
332
333 // Timestamp
334 road_time_t time = 0;
335 road_timerange_t tr = 0;
336
337 /////////////////////
338 while (/*ComponentBase::*/THREAD_ALIVE) {
339 /////////////////////
340 // Grab an image from the camera
341 fe = flycaptureGrabImage2( mFlycapture, &flycaptureImage );
342 _HANDLE_FLYCAPTURE_ERROR( "flycaptureGrabImage()", fe );
343 if (fe != FLYCAPTURE_OK) {
344 continue;
345 }
346
347 // Get timestamp from capture
348 unsigned long timeStampSeconds = flycaptureImage.timeStamp.ulSeconds;
349 unsigned long timeStampMicroSeconds = flycaptureImage.timeStamp.ulMicroSeconds;
350 time = static_cast<road_time_t>(timeStampSeconds) * 1000 * 1000 + timeStampMicroSeconds;
351
352 /////////////////////
353 // Convert the pixel interleaved raw data to row interleaved format
354 fe = flycapturePrepareStereoImage( mFlycapture, flycaptureImage, NULL, &tempImage );
355 _HANDLE_FLYCAPTURE_ERROR( "flycapturePrepareStereoImage()", fe );
356 if (fe != FLYCAPTURE_OK) {
357 continue;
358 }
359
360 /////////////////////
361 // Use the row interleaved images to build up a packed TriclopsInput.
362 // A packed triclops input will contain a single image with 32 bpp.
363 TriclopsInput colorInputRight;
364 te = triclopsBuildPackedTriclopsInput(
365 imageCols, imageRows, imageRowInc * 4,
366 timeStampSeconds, timeStampMicroSeconds,
367 redColor, &colorInputRight );
368 _HANDLE_TRICLOPS_ERROR( "triclopsBuildPackedTriclopsInput()", te );
369 if (te != TriclopsErrorOk) {
370 continue;
371 }
372
373 //TriclopsInput colorInputTop;
374 //te = triclopsBuildPackedTriclopsInput(
375 // imageCols, imageRows, imageRowInc * 4,
376 // timeStampSeconds, timeStampMicroSeconds,
377 // greenColor, &colorInputTop );
378 //_HANDLE_TRICLOPS_ERROR( "triclopsBuildPackedTriclopsInput()", te );
379 //if (te != TriclopsErrorOk) {
380 // continue;
381 //}
382
383 TriclopsInput colorInputLeft;
384 te = triclopsBuildPackedTriclopsInput(
385 imageCols, imageRows, imageRowInc * 4,
386 timeStampSeconds, timeStampMicroSeconds,
387 blueColor, &colorInputLeft );
388 _HANDLE_TRICLOPS_ERROR( "triclopsBuildPackedTriclopsInput()", te );
389 if (te != TriclopsErrorOk) {
390 continue;
391 }
392
393 /////////////////////
394 // Rectify the color image
395 TriclopsPackedColorImage colorImageRight;
396 te = triclopsRectifyPackedColorImage(mTriclops, TriCam_RIGHT, &colorInputRight, &colorImageRight);
397 _HANDLE_TRICLOPS_ERROR( "triclopsRectifyPackedColorImage()", te );
398 if (te != TriclopsErrorOk) {
399 continue;
400 }
401
402 // TriclopsPackedColorImage colorImageTop;
403 //te = triclopsRectifyPackedColorImage(mTriclops, TriCam_TOP, &colorInputTop, &colorImageTop);
404 //_HANDLE_TRICLOPS_ERROR( "triclopsRectifyPackedColorImage()", te );
405 //if (te != TriclopsErrorOk) {
406 // continue;
407 //}
408
409 TriclopsPackedColorImage colorImageLeft;
410 te = triclopsRectifyPackedColorImage(mTriclops, TriCam_LEFT, &colorInputLeft, &colorImageLeft);
411 _HANDLE_TRICLOPS_ERROR( "triclopsRectifyPackedColorImage()", te );
412 if (te != TriclopsErrorOk) {
413 continue;
414 }
415
416 /////////////////////
417 // Copy images to shared memory
418 mShMemLeft->write(colorImageLeft.data, mMaxImageSize);
419 //mShMemTop->write(colorImageTop.data, mMaxImageSize);
420 mShMemRight->write(colorImageRight.data, mMaxImageSize);
421
422 /////////////////////
423 // Write images to disk
424 if (mIsRecording) {
425 // Save the color rectified image to file
426 {
427 stringstream imageNameRightSs; imageNameRightSs << imagePrefix << imageNameDelimiter << "right" << imageNameDelimiter << setfill(kFillCharacter) << setw(kNumberWidth) << imageCount << imageExtension;
428 string imageNameRight = outputDirectory.filePath(imageNameRightSs.str().c_str()).toStdString();
429 te = triclopsSavePackedColorImage(&colorImageRight, const_cast<char *>(imageNameRight.c_str()));
430 _HANDLE_TRICLOPS_ERROR( "triclopsSavePackedColorImage()", te );
431 try {
432 mDbtRight.writeRecord(time, tr, imageNameRight.c_str(), kMaxFilepathLength);
433 } catch (DbiteException & e) {
434 LOG_ERROR("error writing data: " << e.what());
435 }
436 }
437
438 //{
439 // stringstream imageNameTopSS; imageNameTopSS << imagePrefix << imageNameDelimiter << "top" << imageNameDelimiter << setfill(kFillCharacter) << setw(kNumberWidth) << imageCount << imageExtension;
440 // string imageNameTop = outputDirectory.filePath(imageNameTopSS.str().c_str()).toStdString();
441 // te = triclopsSavePackedColorImage(&colorImageTop, const_cast<char *>(imageNameTop.c_str()));
442 // _HANDLE_TRICLOPS_ERROR( "triclopsSavePackedColorImage()", te );
443 // try {
444 // mDbtTop.writeRecord(time, tr, imageNameTop.c_str(), kMaxFilepathLength);
445 // } catch (DbiteException & e) {
446 // LOG_ERROR("error writing data: " << e.what());
447 // }
448 //}
449
450 {
451 stringstream imageNameLeftSs; imageNameLeftSs << imagePrefix << imageNameDelimiter << "left" << imageNameDelimiter << setfill(kFillCharacter) << setw(kNumberWidth) << imageCount << imageExtension;
452 string imageNameLeft = outputDirectory.filePath(imageNameLeftSs.str().c_str()).toStdString();
453 te = triclopsSavePackedColorImage(&colorImageLeft, const_cast<char *>(imageNameLeft.c_str()));
454 _HANDLE_TRICLOPS_ERROR( "triclopsSavePackedColorImage()", te );
455 try {
456 mDbtLeft.writeRecord(time, tr, imageNameLeft.c_str(), kMaxFilepathLength);
457 } catch (DbiteException & e) {
458 LOG_ERROR("error writing data: " << e.what());
459 }
460 }
461 }
462
463 LOG_TRACE("image no. " << imageCount << " acquired successfully");
464 ++imageCount;
465 }
466
467 LOG_INFO("finished acquisition");
468}
469
470} // namespace pacpus
Note: See TracBrowser for help on using the repository browser.