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
print_help()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
saveXYZ(const char * filename,const Mat & mat)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
main(int argc,char ** argv)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