samples/c/stereo_match.cpp

   1 /*
   2  *  stereo_match.cpp
   3  *  calibration
   4  *
   5  *  Created by Victor  Eruhimov on 1/18/10.
   6  *  Copyright 2010 Argus Corp. All rights reserved.
   7  *
   8  */
   9
  10 #include "opencv2/calib3d/calib3d.hpp"
  11 #include "opencv2/imgproc/imgproc.hpp"
  12 #include "opencv2/highgui/highgui.hpp"
  13
  14 #include <stdio.h>
  15
  16 using namespace cv;
  17
  18 void saveXYZ(const char* filename, const Mat& mat)
  19 {
  20     const double max_z = 1.0e4;
  21     FILE* fp = fopen(filename, "wt");
  22     for(int y = 0; y < mat.rows; y++)
  23     {
  24         for(int x = 0; x < mat.cols; x++)
  25         {
  26             Vec3f point = mat.at<Vec3f>(y, x);
  27             if(fabs(point[2] - max_z) < FLT_EPSILON || fabs(point[2]) > max_z) continue;
  28             fprintf(fp, "%f %f %f\n", point[0], point[1], point[2]);
  29         }
  30     }
  31     fclose(fp);
  32 }
  33
  34 void print_help()
  35 {
  36     printf("Usage: stereo_match <left_image> <right_image> [--algorithm=bm|sgbm|hh] [--blocksize=<block_size>]\n"
  37            "[--max-disparity=<max_disparity>] [-i <intrinsic_filename>] [-e <extrinsic_filename>]\n"
  38            "[--no-display] [-o <disparity_image>] [-p <point_cloud_file>]\n");
  39 }
  40
  41 int main(int argc, char** argv)
  42 {
  43     const char* algorithm_opt = "--algorithm=";
  44     const char* maxdisp_opt = "--max-disparity=";
  45     const char* blocksize_opt = "--blocksize=";
  46     const char* nodisplay_opt = "--no-display=";
  47
  48     if(argc < 3)
  49     {
  50         print_help();
  51         return 0;
  52     }
  53     const char* img1_filename = 0;
  54     const char* img2_filename = 0;
  55     const char* intrinsic_filename = 0;
  56     const char* extrinsic_filename = 0;
  57     const char* disparity_filename = 0;
  58     const char* point_cloud_filename = 0;
  59
  60     enum { STEREO_BM=0, STEREO_SGBM=1, STEREO_HH=2 };
  61     int alg = STEREO_SGBM;
  62     int SADWindowSize = 0, numberOfDisparities = 0;
  63     bool no_display = false;
  64
  65     StereoBM bm;
  66     StereoSGBM sgbm;
  67
  68     for( int i = 1; i < argc; i++ )
  69     {
  70         if( argv[i][0] != '-' )
  71         {
  72             if( !img1_filename )
  73                 img1_filename = argv[i];
  74             else
  75                 img2_filename = argv[i];
  76         }
  77         else if( strncmp(argv[i], algorithm_opt, strlen(algorithm_opt)) == 0 )
  78         {
  79             char* _alg = argv[i] + strlen(algorithm_opt);
  80             alg = strcmp(_alg, "bm") == 0 ? STEREO_BM :
  81                   strcmp(_alg, "sgbm") == 0 ? STEREO_SGBM :
  82                   strcmp(_alg, "hh") == 0 ? STEREO_HH : -1;
  83             if( alg < 0 )
  84             {
  85                 printf("Command-line parameter error: Unknown stereo algorithm\n\n");
  86                 print_help();
  87                 return -1;
  88             }
  89         }
  90         else if( strncmp(argv[i], maxdisp_opt, strlen(maxdisp_opt)) == 0 )
  91         {
  92             if( sscanf( argv[i] + strlen(maxdisp_opt), "%d", &numberOfDisparities ) != 1 ||
  93                 numberOfDisparities < 1 || numberOfDisparities % 16 != 0 )
  94             {
  95                 printf("Command-line parameter error: The max disparity (--maxdisparity=<...>) must be a positive integer divisible by 16\n");
  96                 print_help();
  97                 return -1;
  98             }
  99         }
 100         else if( strncmp(argv[i], blocksize_opt, strlen(blocksize_opt)) == 0 )
 101         {
 102             if( sscanf( argv[i] + strlen(blocksize_opt), "%d", &SADWindowSize ) != 1 ||
 103                 SADWindowSize < 1 || SADWindowSize % 2 != 1 )
 104             {
 105                 printf("Command-line parameter error: The block size (--blocksize=<...>) must be a positive odd number\n");
 106                 return -1;
 107             }
 108         }
 109         else if( strcmp(argv[i], nodisplay_opt) == 0 )
 110             no_display = true;
 111         else if( strcmp(argv[i], "-i" ) == 0 )
 112             intrinsic_filename = argv[++i];
 113         else if( strcmp(argv[i], "-e" ) == 0 )
 114             extrinsic_filename = argv[++i];
 115         else if( strcmp(argv[i], "-o" ) == 0 )
 116             disparity_filename = argv[++i];
 117         else if( strcmp(argv[i], "-p" ) == 0 )
 118             point_cloud_filename = argv[++i];
 119         else
 120         {
 121             printf("Command-line parameter error: unknown option %s\n", argv[i]);
 122             return -1;
 123         }
 124     }
 125
 126     if( !img1_filename || !img2_filename )
 127     {
 128         printf("Command-line parameter error: both left and right images must be specified\n");
 129         return -1;
 130     }
 131
 132     if( (intrinsic_filename != 0) ^ (extrinsic_filename != 0) )
 133     {
 134         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");
 135         return -1;
 136     }
 137
 138     if( extrinsic_filename == 0 && point_cloud_filename )
 139     {
 140         printf("Command-line parameter error: extrinsic and intrinsic parameters must be specified to compute the point cloud\n");
 141         return -1;
 142     }
 143
 144     int color_mode = alg == STEREO_BM ? 0 : -1;
 145     Mat img1 = imread(img1_filename, color_mode);
 146     Mat img2 = imread(img2_filename, color_mode);
 147     Size img_size = img1.size();
 148
 149     Rect roi1, roi2;
 150     Mat Q;
 151
 152     if( intrinsic_filename )
 153     {
 154         // reading intrinsic parameters
 155         FileStorage fs(intrinsic_filename, CV_STORAGE_READ);
 156         if(!fs.isOpened())
 157         {
 158             printf("Failed to open file %s\n", intrinsic_filename);
 159             return -1;
 160         }
 161
 162         Mat M1, D1, M2, D2;
 163         fs["M1"] >> M1;
 164         fs["D1"] >> D1;
 165         fs["M2"] >> M2;
 166         fs["D2"] >> D2;
 167
 168         fs.open(extrinsic_filename, CV_STORAGE_READ);
 169         if(!fs.isOpened())
 170         {
 171             printf("Failed to open file %s\n", extrinsic_filename);
 172             return -1;
 173         }
 174
 175         Mat R, T, R1, P1, R2, P2;
 176         fs["R"] >> R;
 177         fs["T"] >> T;
 178
 179         stereoRectify( M1, D1, M2, D2, img_size, R, T, R1, R2, P1, P2, Q, -1, img_size, &roi1, &roi2 );
 180
 181         Mat map11, map12, map21, map22;
 182         initUndistortRectifyMap(M1, D1, R1, P1, img_size, CV_16SC2, map11, map12);
 183         initUndistortRectifyMap(M2, D2, R2, P2, img_size, CV_16SC2, map21, map22);
 184
 185         Mat img1r, img2r;
 186         remap(img1, img1r, map11, map12, INTER_LINEAR);
 187         remap(img2, img2r, map21, map22, INTER_LINEAR);
 188
 189         img1 = img1r;
 190         img2 = img2r;
 191     }
 192
 193     numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : img_size.width/8;
 194
 195     bm.state->roi1 = roi1;
 196     bm.state->roi2 = roi2;
 197     bm.state->preFilterCap = 31;
 198     bm.state->SADWindowSize = SADWindowSize > 0 ? SADWindowSize : 9;
 199     bm.state->minDisparity = 0;
 200     bm.state->numberOfDisparities = numberOfDisparities;
 201     bm.state->textureThreshold = 10;
 202     bm.state->uniquenessRatio = 15;
 203     bm.state->speckleWindowSize = 100;
 204     bm.state->speckleRange = 32;
 205     bm.state->disp12MaxDiff = 1;
 206
 207     sgbm.preFilterCap = 63;
 208     sgbm.SADWindowSize = SADWindowSize > 0 ? SADWindowSize : 3;
 209
 210     int cn = img1.channels();
 211
 212     sgbm.P1 = 8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
 213     sgbm.P2 = 32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
 214     sgbm.minDisparity = 0;
 215     sgbm.numberOfDisparities = numberOfDisparities;
 216     sgbm.uniquenessRatio = 10;
 217     sgbm.speckleWindowSize = bm.state->speckleWindowSize;
 218     sgbm.speckleRange = bm.state->speckleRange;
 219     sgbm.disp12MaxDiff = 1;
 220     sgbm.fullDP = alg == STEREO_HH;
 221
 222     Mat disp, disp8;
 223     //Mat img1p, img2p, dispp;
 224     //copyMakeBorder(img1, img1p, 0, 0, numberOfDisparities, 0, IPL_BORDER_REPLICATE);
 225     //copyMakeBorder(img2, img2p, 0, 0, numberOfDisparities, 0, IPL_BORDER_REPLICATE);
 226
 227     int64 t = getTickCount();
 228     if( alg == STEREO_BM )
 229         bm(img1, img2, disp);
 230     else
 231         sgbm(img1, img2, disp);
 232     t = getTickCount() - t;
 233     printf("Time elapsed: %fms\n", t*1000/getTickFrequency());
 234
 235     //disp = dispp.colRange(numberOfDisparities, img1p.cols);
 236     disp.convertTo(disp8, CV_8U, 255/(numberOfDisparities*16.));
 237     if( !no_display )
 238     {
 239         namedWindow("left", 1);
 240         imshow("left", img1);
 241         namedWindow("right", 1);
 242         imshow("right", img2);
 243         namedWindow("disparity", 0);
 244         imshow("disparity", disp8);
 245         printf("press any key to continue...");
 246         fflush(stdout);
 247         waitKey();
 248         printf("\n");
 249     }
 250
 251     if(disparity_filename)
 252         imwrite(disparity_filename, disp8);
 253
 254     if(point_cloud_filename)
 255     {
 256         printf("storing the point cloud...");
 257         fflush(stdout);
 258         Mat xyz;
 259         reprojectImageTo3D(disp, xyz, Q, true);
 260         saveXYZ(point_cloud_filename, xyz);
 261         printf("\n");
 262     }
 263
 264     return 0;
 265 }