Changeset 53 in pacpussensors for trunk/PtGreyCameras
- Timestamp:
- Jun 20, 2014, 5:36:27 PM (10 years ago)
- Location:
- trunk/PtGreyCameras
- Files:
-
- 2 edited
-
Flea3Grabber.cpp (modified) (15 diffs)
-
Flea3Grabber.h (modified) (4 diffs)
Legend:
- Unmodified
- Added
- Removed
-
trunk/PtGreyCameras/Flea3Grabber.cpp
r51 r53 15 15 16 16 #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> 22 18 23 19 #include "Pacpus/kernel/DbiteException.h" … … 176 172 177 173 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 180 193 for (int i = 0; i < nbrCamera_; i++) 181 194 { … … 277 290 if (config.getProperty("cam_strobe_duration" + QString::number(i)) != QString::null) 278 291 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(); 279 308 280 309 //-------------------------------- Recording options ------------------------------------------------ … … 381 410 this->settings_[indice].image_compact = 0; // Don't compact the image data 382 411 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; 383 419 } 384 420 … … 393 429 this->is_running = true; 394 430 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 395 468 std::vector<FlyCapture2::Camera *> cams; 469 std::vector<FlyCapture2::Image> convertedImage; 396 470 397 471 //==================================== Camera initialization ================================================= … … 414 488 415 489 for (int i = 0; i < this->nbrCamera_; i++) 416 { 490 { 417 491 if (this->use_shmem) 418 492 { … … 460 534 461 535 // Just for visualization 462 printf(536 /*printf( 463 537 "\n*** CAMERA INFORMATION ***\n" 464 538 "Serial number - %u\n" … … 475 549 camInfo.sensorResolution, 476 550 camInfo.firmwareVersion, 477 camInfo.firmwareBuildTime); 551 camInfo.firmwareBuildTime);*/ 478 552 479 553 // Query for available Format 7 modes … … 660 734 const int kNumberWidth = 5; 661 735 //-------------------------------------------------------------------------------------------- 736 FlyCapture2::Image tmp_img; 737 convertedImage.push_back(tmp_img); 662 738 } 663 739 664 740 // Raw image 665 FlyCapture2::Image rawImage; 666 FlyCapture2::Image convertedImage; 741 FlyCapture2::Image rawImage; 667 742 668 743 //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 } 669 752 670 753 road_time_t time; 671 754 672 755 while (THREAD_ALIVE) 673 756 { 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++) 679 771 { 680 772 // Retrieve an image … … 687 779 688 780 // Image timestamp 689 // FlyCapture2::TimeStamptimeStamp = rawImage.GetTimeStamp();781 //settings_[i].timeStamp = rawImage.GetTimeStamp(); 690 782 691 783 //time = road_time(); // Use the same time base of the other sensors … … 694 786 { 695 787 // Convert the raw image 696 error = rawImage.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &convertedImage );788 error = rawImage.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &convertedImage[i]); 697 789 if (error != FlyCapture2::PGRERROR_OK) 698 790 { … … 704 796 { 705 797 // Convert the raw image 706 error = rawImage.Convert(FlyCapture2::PIXEL_FORMAT_BGR, &convertedImage );798 error = rawImage.Convert(FlyCapture2::PIXEL_FORMAT_BGR, &convertedImage[i]); 707 799 if (error != FlyCapture2::PGRERROR_OK) 708 800 { … … 712 804 } 713 805 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 714 808 if (this->use_shmem) 715 809 { … … 720 814 // Copy images header and data to memory 721 815 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 723 822 this->shmem_images_[i]->write(this->settings_[i].img_mem, this->settings_[i].img_mem_size); 724 823 } 725 824 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 726 835 ///////////////////// 727 836 // Write images to disk … … 833 942 //LOG_TRACE("image no. " << imageCount << " acquired successfully"); 834 943 //++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 }845 944 } 846 945 } -
trunk/PtGreyCameras/Flea3Grabber.h
r51 r53 22 22 #include "Pacpus/PacpusTools/ShMem.h" 23 23 #include "PtGreyCamerasExp.h" 24 25 #include "opencv2/objdetect/objdetect.hpp" 26 #include "opencv2/calib3d/calib3d.hpp" 27 #include "opencv2/imgproc/imgproc.hpp" 28 #include "opencv2/core/core.hpp" 29 #include "opencv2/highgui/highgui.hpp" 24 30 25 31 #include "FlyCapture2.h" … … 62 68 float cam_strobe_delay; 63 69 float cam_strobe_duration; 64 70 FlyCapture2::TimeStamp timeStamp; 71 double distCoe[8]; 72 cv::Mat_<double> distCoeffs; 73 double tmp_matrix[3][3]; 74 cv::Mat_<double> matrix; 65 75 int cam_ColorProcessingAlgorithm; /** 66 76 * Color processing algorithms. Please refer to our knowledge base at … … 110 120 DbiteFile mDbtImage; 111 121 bool use_shmem; // If is to use shared memory 122 bool shmem_corrected; 112 123 bool THREAD_ALIVE; 113 124 … … 122 133 int nbrCamera_; 123 134 int masterCamera_; 135 float rot_[3], trans_[3]; 124 136 125 137 std::vector<camSetting> settings_;
Note:
See TracChangeset
for help on using the changeset viewer.
