Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include "opencv2/calib3d/calib3d.hpp"
- #include "opencv2/imgproc.hpp"
- #include "opencv2/imgcodecs.hpp"
- #include "opencv2/highgui.hpp"
- #include "opencv2/core/utility.hpp"
- #include <iostream>
- #include <stdio.h>
- #include <string>
- using namespace cv;
- void testfunc()
- {
- using namespace std;
- using namespace cv;
- Mat large = imread("E:/work/working_folder/2hRcBDV.jpg");
- Mat rgb;
- if (large.rows > 2500 || large.cols > 1250)
- {
- pyrDown(large, rgb);
- }
- else
- {
- rgb = large.clone();
- }
- cv::Mat smallx;
- cvtColor(rgb, smallx, CV_BGR2GRAY);
- Mat grad, connected, bw;
- Mat morphKernel = getStructuringElement(MORPH_ELLIPSE, Size(3, 3));
- cv::morphologyEx(smallx, grad, MORPH_GRADIENT, morphKernel);
- cv::threshold(grad, bw, 100, 255, THRESH_BINARY + THRESH_OTSU);
- morphKernel = getStructuringElement(MORPH_RECT, Size(9, 1));
- cv::morphologyEx(bw, connected, MORPH_CLOSE, morphKernel);
- Mat mask = Mat::zeros(bw.size(), CV_8UC1);
- vector<vector<Point>> contours;
- vector<Vec4i> hierarchy;
- imshow("connected", connected);
- cv::waitKey();
- cv::findContours(connected, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));
- }
- static void print_help()
- {
- printf("\nDemo stereo matching converting L and R images into disparity and point clouds\n");
- printf("\nUsage: stereo_match <left_image> <right_image> [--algorithm=bm|sgbm|hh|sgbm3way] [--blocksize=<block_size>]\n"
- "[--max-disparity=<max_disparity>] [--scale=scale_factor>] [-i=<intrinsic_filename>] [-e=<extrinsic_filename>]\n"
- "[--no-display] [-o=<disparity_image>] [-p=<point_cloud_file>]\n");
- }
- static void saveXYZ(const char* filename, const Mat& mat)
- {
- const double max_z = 1e4;
- FILE* fp = fopen(filename, "wt");
- for (int y = 0; y < mat.rows; y++)
- {
- for (int x = 0; x < mat.cols; x++)
- {
- Vec3f point = mat.at<Vec3f>(y, x);
- if (fabs(point[2] - max_z) < FLT_EPSILON || fabs(point[2]) > max_z) continue;
- fprintf(fp, "%f %f %f\n", point[0], point[1], point[2]);
- }
- }
- fclose(fp);
- }
- int main(int argc, char** argv)
- {
- //testfunc();
- std::string img1_filename = "E:/work/working_folder/cam_1_01.bmp";
- std::string img2_filename = "E:/work/working_folder/cam_2_01.bmp";
- std::string intrinsic_filename = "E:/work/working_folder/intrinsics.yml";
- std::string extrinsic_filename = "E:/work/working_folder/extrinsics.yml";
- std::string disparity_filename = "E:/work/working_folder/SBM_sample.png";
- std::string point_cloud_filename = "E:/work/working_folder/xyz.txt";
- enum { STEREO_BM = 0, STEREO_SGBM = 1, STEREO_HH = 2, STEREO_VAR = 3, STEREO_3WAY = 4 };
- int alg = STEREO_BM;
- int SADWindowSize, numberOfDisparities;
- bool no_display;
- float scale;
- Ptr<StereoBM> bm = StereoBM::create(16, 9);
- Ptr<StereoSGBM> sgbm = StereoSGBM::create(0, 16, 3);
- /*cv::CommandLineParser parser(argc, argv,
- "{@arg1||}{@arg2||}{help h||}{algorithm||}{max-disparity|0|}{blocksize|0|}{no-display||}{scale|1|}{i||}{e||}{o||}{p||}");
- if (parser.has("help"))
- {
- print_help();
- return 0;
- }
- img1_filename = parser.get<std::string>(0);
- img2_filename = parser.get<std::string>(1);
- if (parser.has("algorithm"))
- {
- std::string _alg = parser.get<std::string>("algorithm");
- alg = _alg == "bm" ? STEREO_BM :
- _alg == "sgbm" ? STEREO_SGBM :
- _alg == "hh" ? STEREO_HH :
- _alg == "var" ? STEREO_VAR :
- _alg == "sgbm3way" ? STEREO_3WAY : -1;
- }
- numberOfDisparities = parser.get<int>("max-disparity");
- SADWindowSize = parser.get<int>("blocksize");
- scale = parser.get<float>("scale");
- no_display = parser.has("no-display");
- if (parser.has("i"))
- intrinsic_filename = parser.get<std::string>("i");
- if (parser.has("e"))
- extrinsic_filename = parser.get<std::string>("e");
- if (parser.has("o"))
- disparity_filename = parser.get<std::string>("o");
- if (parser.has("p"))
- point_cloud_filename = parser.get<std::string>("p");
- if (!parser.check())
- {
- parser.printErrors();
- return 1;
- }
- if (alg < 0)
- {
- printf("Command-line parameter error: Unknown stereo algorithm\n\n");
- print_help();
- return -1;
- }
- if (numberOfDisparities < 1 || numberOfDisparities % 16 != 0)
- {
- printf("Command-line parameter error: The max disparity (--maxdisparity=<...>) must be a positive integer divisible by 16\n");
- print_help();
- return -1;
- }
- if (scale < 0)
- {
- printf("Command-line parameter error: The scale factor (--scale=<...>) must be a positive floating-point number\n");
- return -1;
- }
- if (SADWindowSize < 1 || SADWindowSize % 2 != 1)
- {
- printf("Command-line parameter error: The block size (--blocksize=<...>) must be a positive odd number\n");
- return -1;
- }
- if (img1_filename.empty() || img2_filename.empty())
- {
- printf("Command-line parameter error: both left and right images must be specified\n");
- return -1;
- }
- if ((!intrinsic_filename.empty()) ^ (!extrinsic_filename.empty()))
- {
- 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");
- return -1;
- }
- if (extrinsic_filename.empty() && !point_cloud_filename.empty())
- {
- printf("Command-line parameter error: extrinsic and intrinsic parameters must be specified to compute the point cloud\n");
- return -1;
- }
- int color_mode = alg == STEREO_BM ? 0 : -1;
- */
- Mat img1 = imread(img1_filename, IMREAD_GRAYSCALE);
- Mat img2 = imread(img2_filename, IMREAD_GRAYSCALE);
- //cv::resize(img1, img1, cv::Size(), 0.25, 0.25);
- //cv::resize(img2, img2, cv::Size(), 0.25, 0.25);
- if (img1.empty())
- {
- printf("Command-line parameter error: could not load the first input image file\n");
- return -1;
- }
- if (img2.empty())
- {
- printf("Command-line parameter error: could not load the second input image file\n");
- return -1;
- }
- scale = 1;
- if (scale != 1.f)
- {
- Mat temp1, temp2;
- int method = scale < 1 ? INTER_AREA : INTER_CUBIC;
- resize(img1, temp1, Size(), scale, scale, method);
- img1 = temp1;
- resize(img2, temp2, Size(), scale, scale, method);
- img2 = temp2;
- }
- Size img_size = img1.size();
- Rect roi1, roi2;
- Mat Q;
- if (!intrinsic_filename.empty())
- {
- // reading intrinsic parameters
- FileStorage fs(intrinsic_filename, FileStorage::READ);
- if (!fs.isOpened())
- {
- printf("Failed to open file %s\n", intrinsic_filename.c_str());
- return -1;
- }
- Mat M1, D1, M2, D2;
- fs["M1"] >> M1;
- fs["D1"] >> D1;
- fs["M2"] >> M2;
- fs["D2"] >> D2;
- M1 *= scale;
- M2 *= scale;
- fs.open(extrinsic_filename, FileStorage::READ);
- if (!fs.isOpened())
- {
- printf("Failed to open file %s\n", extrinsic_filename.c_str());
- return -1;
- }
- std::cout << "<M1 : " << std::endl;
- std::cout << M1.size() << std::endl;
- std::cout << M1.type() << std::endl;
- for (int i = 0; i < 3; i++)
- {
- for (int j = 0; j < 3; j++)
- {
- std::cout << M1.at<double>(i, j) << " ";
- }
- std::cout << std::endl;
- }
- std::cout << "<M2 : " << std::endl;
- std::cout << M2.size() << std::endl;
- std::cout << M2.type() << std::endl;
- for (int i = 0; i < 3; i++)
- {
- for (int j = 0; j < 3; j++)
- {
- std::cout << M2.at<double>(i, j) << " ";
- }
- std::cout << std::endl;
- }
- Mat R, T, R1, P1, R2, P2;
- fs["R"] >> R;
- fs["T"] >> T;
- fs["R1"] >> R1;
- fs["P1"] >> P1;
- fs["R2"] >> R2;
- fs["P2"] >> P2;
- fs["Q"] >> Q;
- stereoRectify(M1, D1, M2, D2, img_size, R, T, R1, R2, P1, P2, Q, CV_CALIB_ZERO_DISPARITY, -1, img_size, &roi1, &roi2);
- Mat map11, map12, map21, map22;
- initUndistortRectifyMap(M1, D1, R1, P1, img_size, CV_16SC2, map11, map12);
- initUndistortRectifyMap(M2, D2, R2, P2, img_size, CV_16SC2, map21, map22);
- Mat img1r, img2r;
- remap(img1, img1r, map11, map12, INTER_LINEAR);
- remap(img2, img2r, map21, map22, INTER_LINEAR);
- img1 = img1r;
- img2 = img2r; // images lay together, so there is no disparity at all
- }
- numberOfDisparities = 16 * 10;
- SADWindowSize = 15;
- numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : ((img_size.width / 8) + 15) & -16;
- bm->setROI1(roi1);
- bm->setROI2(roi2);
- bm->setPreFilterCap(31);
- bm->setBlockSize(SADWindowSize > 0 ? SADWindowSize : 9);
- bm->setMinDisparity(0);
- bm->setNumDisparities(numberOfDisparities);
- bm->setTextureThreshold(10);
- bm->setUniquenessRatio(15);
- bm->setSpeckleWindowSize(100);
- bm->setSpeckleRange(32);
- bm->setDisp12MaxDiff(1);
- sgbm->setPreFilterCap(63);
- int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3;
- sgbm->setBlockSize(sgbmWinSize);
- int cn = img1.channels();
- sgbm->setP1(8 * cn*sgbmWinSize*sgbmWinSize);
- sgbm->setP2(32 * cn*sgbmWinSize*sgbmWinSize);
- sgbm->setMinDisparity(0);
- sgbm->setNumDisparities(numberOfDisparities);
- sgbm->setUniquenessRatio(10);
- sgbm->setSpeckleWindowSize(50);
- sgbm->setSpeckleRange(32);
- sgbm->setDisp12MaxDiff(1);
- if (alg == STEREO_HH)
- sgbm->setMode(StereoSGBM::MODE_HH);
- else if (alg == STEREO_SGBM)
- sgbm->setMode(StereoSGBM::MODE_SGBM);
- else if (alg == STEREO_3WAY)
- sgbm->setMode(StereoSGBM::MODE_SGBM_3WAY);
- Mat disp, disp8;
- //Mat img1p, img2p, dispp;
- //copyMakeBorder(img1, img1p, 0, 0, numberOfDisparities, 0, IPL_BORDER_REPLICATE);
- //copyMakeBorder(img2, img2p, 0, 0, numberOfDisparities, 0, IPL_BORDER_REPLICATE);
- int64 t = getTickCount();
- if (alg == STEREO_BM)
- bm->compute(img1, img2, disp);
- else if (alg == STEREO_SGBM || alg == STEREO_HH || alg == STEREO_3WAY)
- sgbm->compute(img1, img2, disp);
- t = getTickCount() - t;
- printf("Time elapsed: %fms\n", t * 1000 / getTickFrequency());
- //disp = dispp.colRange(numberOfDisparities, img1p.cols);
- if (alg != STEREO_VAR)
- disp.convertTo(disp8, CV_8U, 255 / (numberOfDisparities*16.));
- else
- disp.convertTo(disp8, CV_8U);
- no_display = false;
- if (!no_display)
- {
- namedWindow("left", 1);
- imshow("left", img1);
- namedWindow("right", 1);
- imshow("right", img2);
- namedWindow("disparity", 0);
- imshow("disparity", disp8);
- printf("press any key to continue...");
- fflush(stdout);
- waitKey(5000);
- printf("\n");
- }
- if (!disparity_filename.empty())
- imwrite(disparity_filename, disp8);
- if (!point_cloud_filename.empty())
- {
- printf("storing the point cloud...");
- fflush(stdout);
- Mat xyz;
- //disp.convertTo(disp, CV_32F);
- disp.convertTo(disp, CV_32F, 1 / 16.);
- //cv::normalize(disp, disp, 0, 16*10, NORM_MINMAX, CV_32F);
- std::cout << disp.type() << std::endl;
- std::cout << Q.type();
- Q.convertTo(Q, CV_32FC1);
- /*
- cv::Mat_<cv::Vec3f> XYZ(disp.rows, disp.cols); // Output point cloud
- cv::Mat_<float> vec_tmp(4, 1);
- for (int y = 0; y<disp.rows; ++y) {
- for (int x = 0; x<disp.cols; ++x) {
- vec_tmp(0) = y; vec_tmp(1) = x; vec_tmp(2) = disp.at<float>(y, x); vec_tmp(3) = 1;
- vec_tmp = Q * vec_tmp;
- vec_tmp /= vec_tmp(3);
- cv::Vec3f &point = XYZ.at<cv::Vec3f>(y, x);
- point[0] = vec_tmp(0);
- point[1] = vec_tmp(1);
- point[2] = vec_tmp(2)/2;
- }
- }
- */
- reprojectImageTo3D(disp, xyz, Q, true, CV_32F);
- patchNaNs(xyz, 0);
- saveXYZ(point_cloud_filename.c_str(), xyz);
- printf("\n");
- }
- return 0;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement