Changeset 53 in pacpussensors for trunk/PtGreyCameras/Flea3Grabber.cpp


Ignore:
Timestamp:
Jun 20, 2014, 5:36:27 PM (10 years ago)
Author:
phudelai
Message:

StereoVisionDisparity updated

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/PtGreyCameras/Flea3Grabber.cpp

    r51 r53  
    1515
    1616#include <iomanip>
    17 #include "opencv2/objdetect/objdetect.hpp"
    18 #include "opencv2/calib3d/calib3d.hpp"
    19 #include "opencv2/imgproc/imgproc.hpp"
    20 #include "opencv2/core/core.hpp"
    21 #include "opencv2/highgui/highgui.hpp"
     17#include <iostream>
    2218
    2319#include "Pacpus/kernel/DbiteException.h"
    … …  
    176172
    177173        if (config.getProperty("use_shmem") != QString::null)
    178                         this->use_shmem = (bool)config.getProperty("use_shmem").toInt();
    179 
     174                this->use_shmem = (bool)config.getProperty("use_shmem").toInt();
     175
     176        if (config.getProperty("shmem_corrected") != QString::null)
     177                this->shmem_corrected = (bool)config.getProperty("shmem_corrected").toInt();
     178
     179        if (config.getProperty("cam_translationx") != QString::null)
     180                trans_[0] = config.getProperty("cam_translationx").toFloat();
     181        if (config.getProperty("cam_translationy") != QString::null)
     182                trans_[1] = config.getProperty("cam_translationy").toFloat();
     183        if (config.getProperty("cam_translationz") != QString::null)
     184                trans_[2] = config.getProperty("cam_translationz").toFloat();
     185
     186        if (config.getProperty("cam_rotationx") != QString::null)
     187                rot_[0] = config.getProperty("cam_rotationx").toFloat();
     188        if (config.getProperty("cam_rotationy") != QString::null)
     189                rot_[1] = config.getProperty("cam_rotationy").toFloat();
     190        if (config.getProperty("cam_rotationz") != QString::null)
     191                rot_[2] = config.getProperty("cam_rotationz").toFloat();
     192       
    180193        for (int i = 0; i < nbrCamera_; i++)
    181194        {
    … …  
    277290                if (config.getProperty("cam_strobe_duration" + QString::number(i)) != QString::null)
    278291                        settings_[i].cam_strobe_duration = config.getProperty("cam_strobe_duration" + QString::number(i)).toInt();
     292
     293                //-------------------------------- Camera settings --------------------------------------------------
     294
     295                if (config.getProperty("cam_fx" + QString::number(i)) != QString::null)
     296                        settings_[i].tmp_matrix[0][0] = config.getProperty("cam_fx" + QString::number(i)).toDouble();// / settings_[i].cam_width;
     297                if (config.getProperty("cam_fy" + QString::number(i)) != QString::null)
     298                        settings_[i].tmp_matrix[1][1] = config.getProperty("cam_fy" + QString::number(i)).toDouble();// / settings_[i].cam_height;
     299                if (config.getProperty("cam_cx" + QString::number(i)) != QString::null)
     300                        settings_[i].tmp_matrix[0][2] = config.getProperty("cam_cx" + QString::number(i)).toDouble();// / settings_[i].cam_width;
     301                if (config.getProperty("cam_cy" + QString::number(i)) != QString::null)
     302                        settings_[i].tmp_matrix[1][2] = config.getProperty("cam_cy" + QString::number(i)).toDouble();// / settings_[i].cam_height;
     303                settings_[i].tmp_matrix[2][2] = 1.0;
     304
     305                for (int j = 0; j < 8; j++) // 8 max parameters for the distortion in OpenCV
     306                        if (config.getProperty("cam_distk" + QString::number(j + 1) + QString::number(i)) != QString::null)
     307                                settings_[i].distCoe[j] = config.getProperty("cam_distk" + QString::number(j + 1) + QString::number(i)).toDouble();
    279308
    280309                //-------------------------------- Recording options ------------------------------------------------
    … …  
    381410        this->settings_[indice].image_compact = 0;                              // Don't compact the image data
    382411        this->settings_[indice].save2dbt = 0;
     412
     413        for (int i = 0; i < 3; i++)
     414                for (int j = 0; j < 3; j++)
     415                        settings_[indice].tmp_matrix[i][j] = 0.0;
     416
     417        for (int i = 0; i < 8; i++) // 8 max for dist coeffs
     418                settings_[indice].distCoe[i] = 0.0;
    383419}
    384420
    … …  
    393429        this->is_running = true;
    394430
     431        // Tmp matrice used because of intelligent pointer of OpenCV and "intelligent" memory free
     432        cv::Mat_<double> tmp_cameraMatrix(3,3); // 3x3 matrix
     433        cv::Mat_<double> tmp_distCoeffs(8,1);   // 5x1 matrix
     434        cv::Mat_<double> R(3,1);            // 3x1 matrix, rotation left to right camera
     435        cv::Mat_<double> T(3,1);            // 3x1 matrix, translation left to right proj. center
     436        cv::Mat R1, R2, P1, P2, Q;
     437        std::vector<cv::Mat> map1(nbrCamera_);
     438        std::vector<cv::Mat> map2(nbrCamera_);
     439        std::vector<cv::Mat> correctedImg(nbrCamera_);
     440
     441        for (int i = 0; i < nbrCamera_; i++)
     442        {
     443                correctedImg.push_back(cv::Mat(settings_[i].cam_height, settings_[i].cam_width, CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels)));
     444                map1.push_back(cv::Mat());
     445                map2.push_back(cv::Mat());
     446
     447                tmp_cameraMatrix << settings_[i].tmp_matrix[0][0], settings_[i].tmp_matrix[0][1], settings_[i].tmp_matrix[0][2], settings_[i].tmp_matrix[1][0], settings_[i].tmp_matrix[1][1], settings_[i].tmp_matrix[1][2], settings_[i].tmp_matrix[2][0], settings_[i].tmp_matrix[2][1], settings_[i].tmp_matrix[2][2];
     448                settings_[i].matrix = tmp_cameraMatrix;
     449                LOG_INFO(settings_[i].matrix);
     450                tmp_distCoeffs << settings_[i].distCoe[0], settings_[i].distCoe[1], settings_[i].distCoe[2], settings_[i].distCoe[3], settings_[i].distCoe[4], settings_[i].distCoe[5], settings_[i].distCoe[6], settings_[i].distCoe[7];
     451                settings_[i].distCoeffs = tmp_distCoeffs;
     452                LOG_INFO(settings_[i].distCoeffs);
     453        }
     454
     455        // Used only for two cameras
     456        if (nbrCamera_ == 2)
     457        {
     458                // Initialisation des matrices pour les calculs OpenCV
     459                R << rot_[0], rot_[1], rot_[2];
     460                T << trans_[0], trans_[1], trans_[2];
     461               
     462                cv::stereoRectify(settings_[0].matrix, settings_[0].distCoeffs, settings_[1].matrix, settings_[1].distCoeffs, cv::Size(settings_[0].cam_width, settings_[0].cam_height), R, T, R1, R2, P1, P2, Q, CV_CALIB_ZERO_DISPARITY);
     463
     464                cv::initUndistortRectifyMap(settings_[0].matrix, settings_[0].distCoeffs, R1, P1, cv::Size(settings_[0].cam_width, settings_[0].cam_height), CV_16SC2, map1.at(0), map2.at(0));
     465                cv::initUndistortRectifyMap(settings_[1].matrix, settings_[1].distCoeffs, R2, P2, cv::Size(settings_[1].cam_width, settings_[1].cam_height), CV_16SC2, map1.at(1), map2.at(1));
     466        }
     467
    395468        std::vector<FlyCapture2::Camera *> cams;
     469        std::vector<FlyCapture2::Image> convertedImage;
    396470
    397471        //==================================== Camera initialization =================================================
    … …  
    414488
    415489        for (int i = 0; i < this->nbrCamera_; i++)
    416         {
     490        {       
    417491                if (this->use_shmem)
    418492                {
    … …  
    460534
    461535                // Just for visualization
    462                 printf(
     536                /*printf(
    463537                        "\n*** CAMERA INFORMATION ***\n"
    464538                        "Serial number - %u\n"
    … …  
    475549                        camInfo.sensorResolution,
    476550                        camInfo.firmwareVersion,
    477                         camInfo.firmwareBuildTime);
     551                        camInfo.firmwareBuildTime);*/
    478552           
    479553                // Query for available Format 7 modes
    … …  
    660734                const int kNumberWidth = 5;
    661735                //--------------------------------------------------------------------------------------------
     736                FlyCapture2::Image tmp_img;
     737                convertedImage.push_back(tmp_img);
    662738        }
    663739       
    664740        // Raw image
    665         FlyCapture2::Image rawImage;
    666         FlyCapture2::Image convertedImage;
     741        FlyCapture2::Image rawImage;   
    667742
    668743        //bool shmen_init = false;
     744        // Because the API doesn't work without (saw this with FlyCapture2.exe)
     745        if (settings_[masterCamera_].cam_trigger_enable)
     746        {
     747                cams[masterCamera_]->FireSoftwareTrigger();
     748                usleep(50000);
     749                cams[masterCamera_]->FireSoftwareTrigger();
     750                usleep(50000);
     751        }
    669752
    670753        road_time_t time;
    671                
     754       
    672755        while (THREAD_ALIVE)
    673756        {
    674                 cams[masterCamera_]->FireSoftwareTrigger();
    675 
    676                 time = road_time();
    677 
    678                 for (int i = 1; i < this->nbrCamera_; i++)
     757                if (settings_[masterCamera_].cam_trigger_enable)
     758                {
     759                        error = cams[masterCamera_]->FireSoftwareTrigger();
     760                        if (error != FlyCapture2::PGRERROR_OK)
     761                        {
     762                                        LOG_ERROR(error.GetDescription());
     763                                        continue;
     764                        }
     765                        time = road_time();
     766                }
     767
     768                //cout << settings_[1].timeStamp.microSeconds - settings_[0].timeStamp.microSeconds << endl;
     769
     770                for (int i = 0; i < this->nbrCamera_; i++)
    679771                {
    680772                        // Retrieve an image
    … …  
    687779
    688780                        // Image timestamp
    689                         //FlyCapture2::TimeStamp timeStamp = rawImage.GetTimeStamp();
     781                        //settings_[i].timeStamp = rawImage.GetTimeStamp();
    690782
    691783                        //time = road_time(); // Use the same time base of the other sensors
    … …  
    694786                        {
    695787                                // Convert the raw image
    696                                 error = rawImage.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &convertedImage);
     788                                error = rawImage.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &convertedImage[i]);
    697789                                if (error != FlyCapture2::PGRERROR_OK)
    698790                                {
    … …  
    704796                        {
    705797                                // Convert the raw image
    706                                 error = rawImage.Convert(FlyCapture2::PIXEL_FORMAT_BGR, &convertedImage);
     798                                error = rawImage.Convert(FlyCapture2::PIXEL_FORMAT_BGR, &convertedImage[i]);
    707799                                if (error != FlyCapture2::PGRERROR_OK)
    708800                                {
    … …  
    712804                        }
    713805
     806                        cv::remap(cv::Mat(convertedImage[i].GetRows(), convertedImage[i].GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), convertedImage[i].GetData(), (int)((double)convertedImage[i].GetReceivedDataSize()/(double)convertedImage[i].GetRows())), correctedImg[i], map1.at(i), map2.at(i), CV_INTER_LINEAR);
     807               
    714808                        if (this->use_shmem)
    715809                        {
    … …  
    720814                                // Copy images header and data to memory
    721815                                memcpy(this->settings_[i].img_mem, &settings_[i].ImageHeader, sizeof(TimestampedStructImage));
    722                                 memcpy((void*)((TimestampedStructImage*)this->settings_[i].img_mem + 1), (void*)convertedImage.GetData(), this->settings_[i].ImageHeader.image.data_size);
     816                               
     817                                if (shmem_corrected)
     818                                        memcpy((void*)((TimestampedStructImage*)this->settings_[i].img_mem + 1), (void*)correctedImg[i].data, this->settings_[i].ImageHeader.image.data_size);
     819                                else
     820                                        memcpy((void*)((TimestampedStructImage*)this->settings_[i].img_mem + 1), (void*)convertedImage[i].GetData(), this->settings_[i].ImageHeader.image.data_size);
     821                               
    723822                                this->shmem_images_[i]->write(this->settings_[i].img_mem, this->settings_[i].img_mem_size);
    724823                        }
    725824
     825                        if (this->settings_[i].showdebug)
     826                        {
     827                                cv::namedWindow(QString("Flea3Component - Current Reference Frame " + QString::number(i)).toStdString(), CV_WINDOW_NORMAL);
     828                                cv::imshow(QString("Flea3Component - Current Reference Frame " + QString::number(i)).toStdString(), cv::Mat(convertedImage[i].GetRows(), convertedImage[i].GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), convertedImage[i].GetData(), (int)((double)convertedImage[i].GetReceivedDataSize()/(double)convertedImage[i].GetRows())));
     829
     830                                cv::namedWindow(QString("Flea3Component - Current Corrected Frame " + QString::number(i)).toStdString(), CV_WINDOW_NORMAL);
     831                                cv::imshow(QString("Flea3Component - Current Corrected Frame " + QString::number(i)).toStdString(), cv::Mat(convertedImage[i].GetRows(), convertedImage[i].GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), correctedImg[i].data, (int)((double)convertedImage[i].GetReceivedDataSize()/(double)convertedImage[i].GetRows())));
     832                                cv::waitKey(1);
     833                        }
     834               
    726835                        /////////////////////
    727836                        // Write images to disk
    … …  
    833942                        //LOG_TRACE("image no. " << imageCount << " acquired successfully");
    834943                        //++imageCount;
    835 
    836                         if (this->settings_[i].showdebug)
    837                         {
    838                                 unsigned int rowBytes = (int)((double)convertedImage.GetReceivedDataSize()/(double)convertedImage.GetRows());
    839                                 cv::Mat Img_BGR = cv::Mat(convertedImage.GetRows(), convertedImage.GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), convertedImage.GetData(), rowBytes);
    840                        
    841                                 cv::namedWindow("Flea3Component - Current Reference Frame", CV_WINDOW_KEEPRATIO);
    842                                 cv::imshow("Flea3Component - Current Reference Frame", Img_BGR);
    843                                 cv::waitKey(1);
    844                         }
    845944                }
    846945        }
Note: See TracChangeset for help on using the changeset viewer.