Changeset 50 in pacpussensors for trunk/PtGreyCameras/Flea3Component.cpp


Ignore:
Timestamp:
Jun 12, 2014, 11:17:03 AM (10 years ago)
Author:
phudelai
Message:

Flea3Component: Shared memory changed for stereovision
StereoVisionDisparity: Added for the PFE of Pierre

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/PtGreyCameras/Flea3Component.cpp

    r48 r50  
    1919#include <iomanip>
    2020#include <string>
    21 
    2221#include "opencv2/objdetect/objdetect.hpp"
    2322#include "opencv2/highgui/highgui.hpp"
    … …  
    4746const int kMaxFilepathLength = 512; // TODO: should be same for all images
    4847
    49 static const string Flea3MemoryName_img = "Flea3Image";
     48static const string Flea3MemoryName_img = "Flea3_";
    5049
    5150//////////////////////////////////////////////////////////////////////////
    … …  
    8584        // output data
    8685        this->shmem_image = 0;                  // Shared memory control access to the image data
     86
     87        stereoVision = false;
     88        cam_side = "";
    8789       
    8890}
    … …  
    125127        }
    126128
     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
    127141        // Create output files for recording
    128         if (this->isRecording())
     142        if (isRecording())
    129143        {
    130144                try
    131145                {
    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);
     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                        }
    136160                }
    137161                catch (DbiteException & e)
    … …  
    142166               
    143167        // Run thread
    144         /*ComponentBase::*/THREAD_ALIVE = true;
     168        THREAD_ALIVE = true;
    145169        start();
    146170}
    … …  
    152176        LOG_TRACE(Q_FUNC_INFO);
    153177
    154         if(/*ComponentBase::*/THREAD_ALIVE)
     178        if(THREAD_ALIVE)
    155179        {
    156180                // Stop thread
    157                 /*ComponentBase::*/THREAD_ALIVE = false;
     181                THREAD_ALIVE = false;
    158182
    159183                while(is_running)
    … …  
    165189
    166190        // Close DBT file
    167         if (this->isRecording())
     191        if (isRecording())
    168192        {
    169193                this->mDbtImage.close();
    … …  
    182206        // Initialize with default values
    183207        this->InitDefault();
    184 
    185         if (config.getProperty("recording") != QString::null)
    186                 setRecording(config.getProperty("recording").toInt());
    187208       
    188209        if (config.getProperty("cam_serial") != QString::null)
    … …  
    245266                this->cam_height = config.getProperty("cam_height").toInt();
    246267
     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
    247275        if (config.getProperty("image_compact") != QString::null)
    248276                this->image_compact = config.getProperty("image_compact").toInt();
    249277
     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
    250291        if (config.getProperty("showdebug") != QString::null)
    251292                this->showdebug = (bool)config.getProperty("showdebug").toInt();
    252293
    253294        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                 }
     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();
    271315
    272316        LOG_INFO("configured component '" << name() << "'");
    … …  
    280324{
    281325        // Default
    282         //recording = 0;
     326        setRecording(0);
    283327
    284328        this->cam_serial = 0;                                                                                           // Camera index to connect
    … …  
    319363        this->mMaxImageOutputSize = sizeof(char)*this->cam_width*this->cam_height*this->cam_channels;
    320364
     365        this->image_scale = 1.0;                                // Default scale
    321366        this->image_compact = 0;                                // Don't compact the image data
     367        this->save2dbt = 0;
    322368}
    323369
    … …  
    580626        FlyCapture2::Image convertedImage;
    581627
     628                //LOG_INFO("Converting frame...");
    582629                if(this->cam_channels == 1)
    583630                {
    … …  
    601648                }
    602649
     650                //LOG_INFO("Copy to shared memory...");
    603651                ////////////////////////////////////////////////////////////////////////////////////////
    604652                if(this->use_shmem)
    … …  
    608656                        {
    609657                                //---------------------------- 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 
     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);
    617672                                //------------------------------------------------------------------------------------------------
    618673
    … …  
    622677                        //std::cout << "Share image created : " << this->img_mem_size << "\n";
    623678
    624                         // Copy images data to memory
    625                         this->shmem_image->write(convertedImage.GetData(), this->img_mem_size);
     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);
    626687                }
    627688
    … …  
    630691                /////////////////////
    631692                // Write images to disk
    632                 if (this->isRecording())
    633                 {
    634                         // Save the image to file
    635                         stringstream imageNameSs;
     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;
    636736                               
    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;
     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                                //--------------------------------------------------------------------------------------------------------------------------------------------
    656795                        }
    657796                }
    658                
     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
    659842                if(this->showdebug)
    660843                {
    … …  
    664847                        cv::namedWindow( "Flea3Component - Current Reference Frame", CV_WINDOW_KEEPRATIO );
    665848                        cv::imshow("Flea3Component - Current Reference Frame", Img_BGR);
    666                         cv::waitKey(10);
    667                 }
    668                 LOG_TRACE("image no. " << imageCount << " acquired successfully");
    669                 ++imageCount;
     849                        cv::waitKey(1);
     850                }
    670851               
    671852        }
    … …  
    699880
    700881        this->is_running = false;
     882
     883        // Destroy the window frame
     884        if(this->showdebug)
     885                cvDestroyAllWindows();
    701886}
Note: See TracChangeset for help on using the changeset viewer.