SHARE
TWEET

Untitled

hagor May 11th, 2017 153 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include "opencv2/calib3d/calib3d.hpp"
  2. #include "opencv2/imgproc.hpp"
  3. #include "opencv2/imgcodecs.hpp"
  4. #include "opencv2/highgui.hpp"
  5. #include "opencv2/core/utility.hpp"
  6. #include <iostream>
  7. #include <stdio.h>
  8. #include <string>
  9. using namespace cv;
  10.  
  11. void testfunc()
  12. {  
  13.     using namespace std;
  14.     using namespace cv;
  15.     Mat large = imread("E:/work/working_folder/2hRcBDV.jpg");
  16.     Mat rgb;
  17.     if (large.rows > 2500 || large.cols > 1250)
  18.     {
  19.         pyrDown(large, rgb);
  20.     }
  21.     else
  22.     {
  23.         rgb = large.clone();
  24.     }
  25.     cv::Mat smallx;
  26.     cvtColor(rgb, smallx, CV_BGR2GRAY);
  27.     Mat grad, connected, bw;
  28.  
  29.     Mat morphKernel = getStructuringElement(MORPH_ELLIPSE, Size(3, 3));
  30.     cv::morphologyEx(smallx, grad, MORPH_GRADIENT, morphKernel);
  31.     cv::threshold(grad, bw, 100, 255, THRESH_BINARY + THRESH_OTSU);
  32.     morphKernel = getStructuringElement(MORPH_RECT, Size(9, 1));
  33.     cv::morphologyEx(bw, connected, MORPH_CLOSE, morphKernel);
  34.  
  35.     Mat mask = Mat::zeros(bw.size(), CV_8UC1);
  36.     vector<vector<Point>> contours;
  37.     vector<Vec4i> hierarchy;
  38.     imshow("connected", connected);
  39.     cv::waitKey();
  40.  
  41.     cv::findContours(connected, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));
  42. }
  43.  
  44. static void print_help()
  45. {
  46.     printf("\nDemo stereo matching converting L and R images into disparity and point clouds\n");
  47.     printf("\nUsage: stereo_match <left_image> <right_image> [--algorithm=bm|sgbm|hh|sgbm3way] [--blocksize=<block_size>]\n"
  48.         "[--max-disparity=<max_disparity>] [--scale=scale_factor>] [-i=<intrinsic_filename>] [-e=<extrinsic_filename>]\n"
  49.         "[--no-display] [-o=<disparity_image>] [-p=<point_cloud_file>]\n");
  50. }
  51.  
  52. static void saveXYZ(const char* filename, const Mat& mat)
  53. {
  54.     const double max_z = 1e4;
  55.     FILE* fp = fopen(filename, "wt");
  56.     for (int y = 0; y < mat.rows; y++)
  57.     {
  58.         for (int x = 0; x < mat.cols; x++)
  59.         {
  60.             Vec3f point = mat.at<Vec3f>(y, x);
  61.             if (fabs(point[2] - max_z) < FLT_EPSILON || fabs(point[2]) > max_z) continue;
  62.             fprintf(fp, "%f %f %f\n", point[0], point[1], point[2]);
  63.         }
  64.     }
  65.     fclose(fp);
  66. }
  67.  
  68. int main(int argc, char** argv)
  69. {
  70.  
  71.     //testfunc();
  72.     std::string img1_filename = "E:/work/working_folder/cam_1_01.bmp";
  73.     std::string img2_filename = "E:/work/working_folder/cam_2_01.bmp";
  74.     std::string intrinsic_filename = "E:/work/working_folder/intrinsics.yml";
  75.     std::string extrinsic_filename = "E:/work/working_folder/extrinsics.yml";
  76.     std::string disparity_filename = "E:/work/working_folder/SBM_sample.png";
  77.     std::string point_cloud_filename = "E:/work/working_folder/xyz.txt";
  78.  
  79.     enum { STEREO_BM = 0, STEREO_SGBM = 1, STEREO_HH = 2, STEREO_VAR = 3, STEREO_3WAY = 4 };
  80.     int alg = STEREO_BM;
  81.     int SADWindowSize, numberOfDisparities;
  82.     bool no_display;
  83.     float scale;
  84.  
  85.     Ptr<StereoBM> bm = StereoBM::create(16, 9);
  86.     Ptr<StereoSGBM> sgbm = StereoSGBM::create(0, 16, 3);
  87.     /*cv::CommandLineParser parser(argc, argv,
  88.         "{@arg1||}{@arg2||}{help h||}{algorithm||}{max-disparity|0|}{blocksize|0|}{no-display||}{scale|1|}{i||}{e||}{o||}{p||}");
  89.     if (parser.has("help"))
  90.     {
  91.         print_help();
  92.         return 0;
  93.     }
  94.     img1_filename = parser.get<std::string>(0);
  95.     img2_filename = parser.get<std::string>(1);
  96.     if (parser.has("algorithm"))
  97.     {
  98.         std::string _alg = parser.get<std::string>("algorithm");
  99.         alg = _alg == "bm" ? STEREO_BM :
  100.             _alg == "sgbm" ? STEREO_SGBM :
  101.             _alg == "hh" ? STEREO_HH :
  102.             _alg == "var" ? STEREO_VAR :
  103.             _alg == "sgbm3way" ? STEREO_3WAY : -1;
  104.     }
  105.     numberOfDisparities = parser.get<int>("max-disparity");
  106.     SADWindowSize = parser.get<int>("blocksize");
  107.     scale = parser.get<float>("scale");
  108.     no_display = parser.has("no-display");
  109.     if (parser.has("i"))
  110.         intrinsic_filename = parser.get<std::string>("i");
  111.     if (parser.has("e"))
  112.         extrinsic_filename = parser.get<std::string>("e");
  113.     if (parser.has("o"))
  114.         disparity_filename = parser.get<std::string>("o");
  115.     if (parser.has("p"))
  116.         point_cloud_filename = parser.get<std::string>("p");
  117.     if (!parser.check())
  118.     {
  119.         parser.printErrors();
  120.         return 1;
  121.     }
  122.     if (alg < 0)
  123.     {
  124.         printf("Command-line parameter error: Unknown stereo algorithm\n\n");
  125.         print_help();
  126.         return -1;
  127.     }
  128.     if (numberOfDisparities < 1 || numberOfDisparities % 16 != 0)
  129.     {
  130.         printf("Command-line parameter error: The max disparity (--maxdisparity=<...>) must be a positive integer divisible by 16\n");
  131.         print_help();
  132.         return -1;
  133.     }
  134.     if (scale < 0)
  135.     {
  136.         printf("Command-line parameter error: The scale factor (--scale=<...>) must be a positive floating-point number\n");
  137.         return -1;
  138.     }
  139.     if (SADWindowSize < 1 || SADWindowSize % 2 != 1)
  140.     {
  141.         printf("Command-line parameter error: The block size (--blocksize=<...>) must be a positive odd number\n");
  142.         return -1;
  143.     }
  144.     if (img1_filename.empty() || img2_filename.empty())
  145.     {
  146.         printf("Command-line parameter error: both left and right images must be specified\n");
  147.         return -1;
  148.     }
  149.     if ((!intrinsic_filename.empty()) ^ (!extrinsic_filename.empty()))
  150.     {
  151.         printf("Command-line parameter error: either both intrinsic and extrinsic parameters must be specified, or none of them (when the stereo pair is already rectified)\n");
  152.         return -1;
  153.     }
  154.  
  155.     if (extrinsic_filename.empty() && !point_cloud_filename.empty())
  156.     {
  157.         printf("Command-line parameter error: extrinsic and intrinsic parameters must be specified to compute the point cloud\n");
  158.         return -1;
  159.     }
  160.  
  161.     int color_mode = alg == STEREO_BM ? 0 : -1;
  162.     */
  163.  
  164.     Mat img1 = imread(img1_filename, IMREAD_GRAYSCALE);
  165.     Mat img2 = imread(img2_filename, IMREAD_GRAYSCALE);
  166.     //cv::resize(img1, img1, cv::Size(), 0.25, 0.25);
  167.     //cv::resize(img2, img2, cv::Size(), 0.25, 0.25);
  168.  
  169.     if (img1.empty())
  170.     {
  171.         printf("Command-line parameter error: could not load the first input image file\n");
  172.         return -1;
  173.     }
  174.     if (img2.empty())
  175.     {
  176.         printf("Command-line parameter error: could not load the second input image file\n");
  177.         return -1;
  178.     }
  179.     scale = 1;
  180.     if (scale != 1.f)
  181.     {
  182.         Mat temp1, temp2;
  183.         int method = scale < 1 ? INTER_AREA : INTER_CUBIC;
  184.         resize(img1, temp1, Size(), scale, scale, method);
  185.         img1 = temp1;
  186.         resize(img2, temp2, Size(), scale, scale, method);
  187.         img2 = temp2;
  188.     }
  189.  
  190.     Size img_size = img1.size();
  191.  
  192.     Rect roi1, roi2;
  193.  
  194.     Mat Q;
  195.  
  196.     if (!intrinsic_filename.empty())
  197.     {
  198.         // reading intrinsic parameters
  199.         FileStorage fs(intrinsic_filename, FileStorage::READ);
  200.         if (!fs.isOpened())
  201.         {
  202.             printf("Failed to open file %s\n", intrinsic_filename.c_str());
  203.             return -1;
  204.         }
  205.  
  206.         Mat M1, D1, M2, D2;
  207.         fs["M1"] >> M1;
  208.         fs["D1"] >> D1;
  209.         fs["M2"] >> M2;
  210.         fs["D2"] >> D2;
  211.        
  212.         M1 *= scale;
  213.         M2 *= scale;
  214.  
  215.         fs.open(extrinsic_filename, FileStorage::READ);
  216.         if (!fs.isOpened())
  217.         {
  218.             printf("Failed to open file %s\n", extrinsic_filename.c_str());
  219.             return -1;
  220.         }
  221.  
  222.         std::cout << "<M1 : " << std::endl;
  223.         std::cout << M1.size() << std::endl;
  224.         std::cout << M1.type() << std::endl;
  225.         for (int i = 0; i < 3; i++)
  226.         {
  227.             for (int j = 0; j < 3; j++)
  228.             {
  229.                 std::cout << M1.at<double>(i, j) << " ";
  230.             }
  231.             std::cout << std::endl;
  232.         }
  233.         std::cout << "<M2 : " << std::endl;
  234.         std::cout << M2.size() << std::endl;
  235.         std::cout << M2.type() << std::endl;
  236.  
  237.         for (int i = 0; i < 3; i++)
  238.         {
  239.             for (int j = 0; j < 3; j++)
  240.             {
  241.                 std::cout << M2.at<double>(i, j) << " ";
  242.             }
  243.             std::cout << std::endl;
  244.         }
  245.         Mat R, T, R1, P1, R2, P2;
  246.         fs["R"] >> R;
  247.         fs["T"] >> T;
  248.         fs["R1"] >> R1;
  249.         fs["P1"] >> P1;
  250.         fs["R2"] >> R2;
  251.         fs["P2"] >> P2;
  252.         fs["Q"] >> Q;
  253.         stereoRectify(M1, D1, M2, D2, img_size, R, T, R1, R2, P1, P2, Q, CV_CALIB_ZERO_DISPARITY, -1, img_size, &roi1, &roi2);
  254.         Mat map11, map12, map21, map22;
  255.         initUndistortRectifyMap(M1, D1, R1, P1, img_size, CV_16SC2, map11, map12);
  256.         initUndistortRectifyMap(M2, D2, R2, P2, img_size, CV_16SC2, map21, map22);
  257.  
  258.         Mat img1r, img2r;
  259.         remap(img1, img1r, map11, map12, INTER_LINEAR);
  260.         remap(img2, img2r, map21, map22, INTER_LINEAR);
  261.         img1 = img1r;
  262.         img2 = img2r; // images lay together, so there is no disparity at all
  263.     }
  264.  
  265.     numberOfDisparities = 16 * 10;
  266.     SADWindowSize = 15;
  267.     numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : ((img_size.width / 8) + 15) & -16;
  268.  
  269.     bm->setROI1(roi1);
  270.     bm->setROI2(roi2);
  271.     bm->setPreFilterCap(31);
  272.     bm->setBlockSize(SADWindowSize > 0 ? SADWindowSize : 9);
  273.     bm->setMinDisparity(0);
  274.     bm->setNumDisparities(numberOfDisparities);
  275.     bm->setTextureThreshold(10);
  276.     bm->setUniquenessRatio(15);
  277.     bm->setSpeckleWindowSize(100);
  278.     bm->setSpeckleRange(32);
  279.     bm->setDisp12MaxDiff(1);
  280.  
  281.  
  282.  
  283.     sgbm->setPreFilterCap(63);
  284.     int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3;
  285.     sgbm->setBlockSize(sgbmWinSize);
  286.  
  287.     int cn = img1.channels();
  288.     sgbm->setP1(8 * cn*sgbmWinSize*sgbmWinSize);
  289.     sgbm->setP2(32 * cn*sgbmWinSize*sgbmWinSize);
  290.     sgbm->setMinDisparity(0);
  291.     sgbm->setNumDisparities(numberOfDisparities);
  292.     sgbm->setUniquenessRatio(10);
  293.     sgbm->setSpeckleWindowSize(50);
  294.     sgbm->setSpeckleRange(32);
  295.     sgbm->setDisp12MaxDiff(1);
  296.     if (alg == STEREO_HH)
  297.         sgbm->setMode(StereoSGBM::MODE_HH);
  298.     else if (alg == STEREO_SGBM)
  299.         sgbm->setMode(StereoSGBM::MODE_SGBM);
  300.     else if (alg == STEREO_3WAY)
  301.         sgbm->setMode(StereoSGBM::MODE_SGBM_3WAY);
  302.  
  303.     Mat disp, disp8;
  304.     //Mat img1p, img2p, dispp;
  305.     //copyMakeBorder(img1, img1p, 0, 0, numberOfDisparities, 0, IPL_BORDER_REPLICATE);
  306.     //copyMakeBorder(img2, img2p, 0, 0, numberOfDisparities, 0, IPL_BORDER_REPLICATE);
  307.  
  308.     int64 t = getTickCount();
  309.     if (alg == STEREO_BM)
  310.         bm->compute(img1, img2, disp);
  311.     else if (alg == STEREO_SGBM || alg == STEREO_HH || alg == STEREO_3WAY)
  312.         sgbm->compute(img1, img2, disp);
  313.     t = getTickCount() - t;
  314.     printf("Time elapsed: %fms\n", t * 1000 / getTickFrequency());
  315.  
  316.     //disp = dispp.colRange(numberOfDisparities, img1p.cols);
  317.     if (alg != STEREO_VAR)
  318.         disp.convertTo(disp8, CV_8U, 255 / (numberOfDisparities*16.));
  319.     else
  320.         disp.convertTo(disp8, CV_8U);
  321.     no_display = false;
  322.     if (!no_display)
  323.     {
  324.         namedWindow("left", 1);
  325.         imshow("left", img1);
  326.         namedWindow("right", 1);
  327.         imshow("right", img2);
  328.         namedWindow("disparity", 0);
  329.         imshow("disparity", disp8);
  330.         printf("press any key to continue...");
  331.         fflush(stdout);
  332.         waitKey(5000);
  333.         printf("\n");
  334.     }
  335.  
  336.     if (!disparity_filename.empty())
  337.         imwrite(disparity_filename, disp8);
  338.  
  339.     if (!point_cloud_filename.empty())
  340.     {
  341.         printf("storing the point cloud...");
  342.         fflush(stdout);
  343.         Mat xyz;
  344.         //disp.convertTo(disp, CV_32F);
  345.         disp.convertTo(disp, CV_32F, 1 / 16.);
  346.         //cv::normalize(disp, disp, 0, 16*10, NORM_MINMAX, CV_32F);
  347.        
  348.         std::cout << disp.type() << std::endl;
  349.         std::cout << Q.type();
  350.         Q.convertTo(Q, CV_32FC1);
  351.         /*
  352.         cv::Mat_<cv::Vec3f> XYZ(disp.rows, disp.cols);   // Output point cloud
  353.         cv::Mat_<float> vec_tmp(4, 1);
  354.         for (int y = 0; y<disp.rows; ++y) {
  355.             for (int x = 0; x<disp.cols; ++x) {
  356.                 vec_tmp(0) = y; vec_tmp(1) = x; vec_tmp(2) = disp.at<float>(y, x); vec_tmp(3) = 1;
  357.                 vec_tmp = Q * vec_tmp;
  358.                 vec_tmp /= vec_tmp(3);
  359.                 cv::Vec3f &point = XYZ.at<cv::Vec3f>(y, x);
  360.                 point[0] = vec_tmp(0);
  361.                 point[1] = vec_tmp(1);
  362.                 point[2] = vec_tmp(2)/2;
  363.             }
  364.         }
  365.         */
  366.         reprojectImageTo3D(disp, xyz, Q, true, CV_32F);
  367.         patchNaNs(xyz, 0);
  368.         saveXYZ(point_cloud_filename.c_str(), xyz);
  369.  
  370.         printf("\n");
  371.     }
  372.  
  373.     return 0;
  374. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Top