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/imgcodecs.hpp" 13 #include "opencv2/highgui/highgui.hpp" 14 #include "opencv2/core/utility.hpp" 15 16 #include <stdio.h> 17 18 using namespace cv; 19 20 static void print_help() 21 { 22 printf("\nDemo stereo matching converting L and R images into disparity and point clouds\n"); 23 printf("\nUsage: stereo_match <left_image> <right_image> [--algorithm=bm|sgbm|hh] [--blocksize=<block_size>]\n" 24 "[--max-disparity=<max_disparity>] [--scale=scale_factor>] [-i <intrinsic_filename>] [-e <extrinsic_filename>]\n" 25 "[--no-display] [-o <disparity_image>] [-p <point_cloud_file>]\n"); 26 } 27 28 static void saveXYZ(const char* filename, const Mat& mat) 29 { 30 const double max_z = 1.0e4; 31 FILE* fp = fopen(filename, "wt"); 32 for(int y = 0; y < mat.rows; y++) 33 { 34 for(int x = 0; x < mat.cols; x++) 35 { 36 Vec3f point = mat.at<Vec3f>(y, x); 37 if(fabs(point[2] - max_z) < FLT_EPSILON || fabs(point[2]) > max_z) continue; 38 fprintf(fp, "%f %f %f\n", point[0], point[1], point[2]); 39 } 40 } 41 fclose(fp); 42 } 43 44 int main(int argc, char** argv) 45 { 46 const char* algorithm_opt = "--algorithm="; 47 const char* maxdisp_opt = "--max-disparity="; 48 const char* blocksize_opt = "--blocksize="; 49 const char* nodisplay_opt = "--no-display"; 50 const char* scale_opt = "--scale="; 51 52 if(argc < 3) 53 { 54 print_help(); 55 return 0; 56 } 57 const char* img1_filename = 0; 58 const char* img2_filename = 0; 59 const char* intrinsic_filename = 0; 60 const char* extrinsic_filename = 0; 61 const char* disparity_filename = 0; 62 const char* point_cloud_filename = 0; 63 64 enum { STEREO_BM=0, STEREO_SGBM=1, STEREO_HH=2, STEREO_VAR=3 }; 65 int alg = STEREO_SGBM; 66 int SADWindowSize = 0, numberOfDisparities = 0; 67 bool no_display = false; 68 float scale = 1.f; 69 70 Ptr<StereoBM> bm = StereoBM::create(16,9); 71 Ptr<StereoSGBM> sgbm = StereoSGBM::create(0,16,3); 72 73 for( int i = 1; i < argc; i++ ) 74 { 75 if( argv[i][0] != '-' ) 76 { 77 if( !img1_filename ) 78 img1_filename = argv[i]; 79 else 80 img2_filename = argv[i]; 81 } 82 else if( strncmp(argv[i], algorithm_opt, strlen(algorithm_opt)) == 0 ) 83 { 84 char* _alg = argv[i] + strlen(algorithm_opt); 85 alg = strcmp(_alg, "bm") == 0 ? STEREO_BM : 86 strcmp(_alg, "sgbm") == 0 ? STEREO_SGBM : 87 strcmp(_alg, "hh") == 0 ? STEREO_HH : 88 strcmp(_alg, "var") == 0 ? STEREO_VAR : -1; 89 if( alg < 0 ) 90 { 91 printf("Command-line parameter error: Unknown stereo algorithm\n\n"); 92 print_help(); 93 return -1; 94 } 95 } 96 else if( strncmp(argv[i], maxdisp_opt, strlen(maxdisp_opt)) == 0 ) 97 { 98 if( sscanf( argv[i] + strlen(maxdisp_opt), "%d", &numberOfDisparities ) != 1 || 99 numberOfDisparities < 1 || numberOfDisparities % 16 != 0 ) 100 { 101 printf("Command-line parameter error: The max disparity (--maxdisparity=<...>) must be a positive integer divisible by 16\n"); 102 print_help(); 103 return -1; 104 } 105 } 106 else if( strncmp(argv[i], blocksize_opt, strlen(blocksize_opt)) == 0 ) 107 { 108 if( sscanf( argv[i] + strlen(blocksize_opt), "%d", &SADWindowSize ) != 1 || 109 SADWindowSize < 1 || SADWindowSize % 2 != 1 ) 110 { 111 printf("Command-line parameter error: The block size (--blocksize=<...>) must be a positive odd number\n"); 112 return -1; 113 } 114 } 115 else if( strncmp(argv[i], scale_opt, strlen(scale_opt)) == 0 ) 116 { 117 if( sscanf( argv[i] + strlen(scale_opt), "%f", &scale ) != 1 || scale < 0 ) 118 { 119 printf("Command-line parameter error: The scale factor (--scale=<...>) must be a positive floating-point number\n"); 120 return -1; 121 } 122 } 123 else if( strcmp(argv[i], nodisplay_opt) == 0 ) 124 no_display = true; 125 else if( strcmp(argv[i], "-i" ) == 0 ) 126 intrinsic_filename = argv[++i]; 127 else if( strcmp(argv[i], "-e" ) == 0 ) 128 extrinsic_filename = argv[++i]; 129 else if( strcmp(argv[i], "-o" ) == 0 ) 130 disparity_filename = argv[++i]; 131 else if( strcmp(argv[i], "-p" ) == 0 ) 132 point_cloud_filename = argv[++i]; 133 else 134 { 135 printf("Command-line parameter error: unknown option %s\n", argv[i]); 136 return -1; 137 } 138 } 139 140 if( !img1_filename || !img2_filename ) 141 { 142 printf("Command-line parameter error: both left and right images must be specified\n"); 143 return -1; 144 } 145 146 if( (intrinsic_filename != 0) ^ (extrinsic_filename != 0) ) 147 { 148 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"); 149 return -1; 150 } 151 152 if( extrinsic_filename == 0 && point_cloud_filename ) 153 { 154 printf("Command-line parameter error: extrinsic and intrinsic parameters must be specified to compute the point cloud\n"); 155 return -1; 156 } 157 158 int color_mode = alg == STEREO_BM ? 0 : -1; 159 Mat img1 = imread(img1_filename, color_mode); 160 Mat img2 = imread(img2_filename, color_mode); 161 162 if (img1.empty()) 163 { 164 printf("Command-line parameter error: could not load the first input image file\n"); 165 return -1; 166 } 167 if (img2.empty()) 168 { 169 printf("Command-line parameter error: could not load the second input image file\n"); 170 return -1; 171 } 172 173 if (scale != 1.f) 174 { 175 Mat temp1, temp2; 176 int method = scale < 1 ? INTER_AREA : INTER_CUBIC; 177 resize(img1, temp1, Size(), scale, scale, method); 178 img1 = temp1; 179 resize(img2, temp2, Size(), scale, scale, method); 180 img2 = temp2; 181 } 182 183 Size img_size = img1.size(); 184 185 Rect roi1, roi2; 186 Mat Q; 187 188 if( intrinsic_filename ) 189 { 190 // reading intrinsic parameters 191 FileStorage fs(intrinsic_filename, FileStorage::READ); 192 if(!fs.isOpened()) 193 { 194 printf("Failed to open file %s\n", intrinsic_filename); 195 return -1; 196 } 197 198 Mat M1, D1, M2, D2; 199 fs["M1"] >> M1; 200 fs["D1"] >> D1; 201 fs["M2"] >> M2; 202 fs["D2"] >> D2; 203 204 M1 *= scale; 205 M2 *= scale; 206 207 fs.open(extrinsic_filename, FileStorage::READ); 208 if(!fs.isOpened()) 209 { 210 printf("Failed to open file %s\n", extrinsic_filename); 211 return -1; 212 } 213 214 Mat R, T, R1, P1, R2, P2; 215 fs["R"] >> R; 216 fs["T"] >> T; 217 218 stereoRectify( M1, D1, M2, D2, img_size, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, -1, img_size, &roi1, &roi2 ); 219 220 Mat map11, map12, map21, map22; 221 initUndistortRectifyMap(M1, D1, R1, P1, img_size, CV_16SC2, map11, map12); 222 initUndistortRectifyMap(M2, D2, R2, P2, img_size, CV_16SC2, map21, map22); 223 224 Mat img1r, img2r; 225 remap(img1, img1r, map11, map12, INTER_LINEAR); 226 remap(img2, img2r, map21, map22, INTER_LINEAR); 227 228 img1 = img1r; 229 img2 = img2r; 230 } 231 232 numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : ((img_size.width/8) + 15) & -16; 233 234 bm->setROI1(roi1); 235 bm->setROI2(roi2); 236 bm->setPreFilterCap(31); 237 bm->setBlockSize(SADWindowSize > 0 ? SADWindowSize : 9); 238 bm->setMinDisparity(0); 239 bm->setNumDisparities(numberOfDisparities); 240 bm->setTextureThreshold(10); 241 bm->setUniquenessRatio(15); 242 bm->setSpeckleWindowSize(100); 243 bm->setSpeckleRange(32); 244 bm->setDisp12MaxDiff(1); 245 246 sgbm->setPreFilterCap(63); 247 int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3; 248 sgbm->setBlockSize(sgbmWinSize); 249 250 int cn = img1.channels(); 251 252 sgbm->setP1(8*cn*sgbmWinSize*sgbmWinSize); 253 sgbm->setP2(32*cn*sgbmWinSize*sgbmWinSize); 254 sgbm->setMinDisparity(0); 255 sgbm->setNumDisparities(numberOfDisparities); 256 sgbm->setUniquenessRatio(10); 257 sgbm->setSpeckleWindowSize(100); 258 sgbm->setSpeckleRange(32); 259 sgbm->setDisp12MaxDiff(1); 260 sgbm->setMode(alg == STEREO_HH ? StereoSGBM::MODE_HH : StereoSGBM::MODE_SGBM); 261 262 Mat disp, disp8; 263 //Mat img1p, img2p, dispp; 264 //copyMakeBorder(img1, img1p, 0, 0, numberOfDisparities, 0, IPL_BORDER_REPLICATE); 265 //copyMakeBorder(img2, img2p, 0, 0, numberOfDisparities, 0, IPL_BORDER_REPLICATE); 266 267 int64 t = getTickCount(); 268 if( alg == STEREO_BM ) 269 bm->compute(img1, img2, disp); 270 else if( alg == STEREO_SGBM || alg == STEREO_HH ) 271 sgbm->compute(img1, img2, disp); 272 t = getTickCount() - t; 273 printf("Time elapsed: %fms\n", t*1000/getTickFrequency()); 274 275 //disp = dispp.colRange(numberOfDisparities, img1p.cols); 276 if( alg != STEREO_VAR ) 277 disp.convertTo(disp8, CV_8U, 255/(numberOfDisparities*16.)); 278 else 279 disp.convertTo(disp8, CV_8U); 280 if( !no_display ) 281 { 282 namedWindow("left", 1); 283 imshow("left", img1); 284 namedWindow("right", 1); 285 imshow("right", img2); 286 namedWindow("disparity", 0); 287 imshow("disparity", disp8); 288 printf("press any key to continue..."); 289 fflush(stdout); 290 waitKey(); 291 printf("\n"); 292 } 293 294 if(disparity_filename) 295 imwrite(disparity_filename, disp8); 296 297 if(point_cloud_filename) 298 { 299 printf("storing the point cloud..."); 300 fflush(stdout); 301 Mat xyz; 302 reprojectImageTo3D(disp, xyz, Q, true); 303 saveXYZ(point_cloud_filename, xyz); 304 printf("\n"); 305 } 306 307 return 0; 308 } 309
