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