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