1 int main()
2 {
3 //initialize some parameters
4 bool okcalib = false;
5 Mat intrMatFirst, intrMatSec, distCoeffsFirst, distCoffesSec;
6 Mat R, T, E, F, RFirst, RSec, PFirst, PSec, Q;
7 vector<vector<Point2f>> imagePointsFirst, imagePointsSec;
8 vector<vector<Point3f>> ObjectPoints(1);
9 Rect validRoi[2];
10 Size imageSize;
11 int cameraIdFirst = 0, cameraIdSec = 1;
12 double rms = 0;
13
14 //get pictures and calibrate
15 vector<string> imageList;
16 string filename = "stereo_calib.xml";
17 bool okread = readStringList(filename, imageList);
18 if (!okread || imageList.empty())
19 {
20 cout << "can not open " << filename << " or the string list is empty" << endl;
21 return false;
22 }
23 if (imageList.size() % 2 != 0)
24 {
25 cout << "Error: the image list contains odd (non-even) number of elements\n";
26 return false;
27 }
28
29 //calibrate
30 cout << "calibrate left camera..." << endl;
31 okcalib = calibrate(intrMatFirst, distCoeffsFirst, imagePointsFirst, ObjectPoints,
32 imageSize, cameraIdFirst, imageList);
33
34 if (!okcalib)
35 {
36 cout << "fail to calibrate left camera" << endl;
37 return -1;
38 }
39 else
40 {
41 cout << "calibrate the right camera..." << endl;
42 }
43
44 okcalib = calibrate(intrMatSec, distCoffesSec, imagePointsSec, ObjectPoints,
45 imageSize, cameraIdSec, imageList);
46
47 if (!okcalib)
48 {
49 cout << "fail to calibrate the right camera" << endl;
50 return -1;
51 }
52 destroyAllWindows();
53
54 //estimate position and orientation
55 cout << "estimate position and orientation of the second camera" << endl
56 << "relative to the first camera..." << endl;
57 rms = stereoCalibrate(ObjectPoints, imagePointsFirst, imagePointsSec,
58 intrMatFirst, distCoeffsFirst, intrMatSec, distCoffesSec,
59 imageSize, R, T, E, F, CV_CALIB_FIX_INTRINSIC,
60 TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, 1e-6));
61 cout << "done with RMS error=" << rms << endl;
62
63 //stereo rectify
64 cout << "stereo rectify..." << endl;
65 stereoRectify(intrMatFirst, distCoeffsFirst, intrMatSec, distCoffesSec, imageSize, R, T, RFirst,
66 RSec, PFirst, PSec, Q, 0, 1, imageSize, &validRoi[0], &validRoi[1]);
67
68 //read pictures for 3d-reconstruction
69 namedWindow("canvas", 1);
70 cout << "read the picture for 3d-reconstruction...";
71 Mat canvas(imageSize.height, imageSize.width * 2, CV_8UC3), viewLeft, viewRight;
72 Mat canLeft = canvas(Rect(0, 0, imageSize.width, imageSize.height));
73 Mat canRight = canvas(Rect(imageSize.width, 0, imageSize.width, imageSize.height));
74 viewLeft = imread(imageList[cameraIdFirst], 1);
75 viewRight = imread(imageList[cameraIdSec], 1);
76 viewLeft.copyTo(canLeft);
77 viewRight.copyTo(canRight);
78 cout << "done" << endl;
79 imshow("canvas", canvas);
80 waitKey(50);
81
82 //stereoRectify
83 Mat rmapFirst[2], rmapSec[2], rviewFirst, rviewSec;
84 initUndistortRectifyMap(intrMatFirst, distCoeffsFirst, RFirst, PFirst,
85 imageSize, CV_16SC2, rmapFirst[0], rmapFirst[1]);
86 initUndistortRectifyMap(intrMatSec, distCoffesSec, RSec, PSec,
87 imageSize, CV_16SC2, rmapSec[0], rmapSec[1]);
88 remap(viewLeft, rviewFirst, rmapFirst[0], rmapFirst[1], INTER_LINEAR);
89 remap(viewRight, rviewSec, rmapSec[0], rmapSec[1], INTER_LINEAR);
90 rviewFirst.copyTo(canLeft);
91 rviewSec.copyTo(canRight);
92
93 rectangle(canLeft, validRoi[0], Scalar(255, 0, 0), 3, 8);
94 rectangle(canRight, validRoi[1], Scalar(255, 0, 0), 3, 8);
95 for (int j = 0; j <= canvas.rows; j += 16)
96 line(canvas, Point(0, j), Point(canvas.cols, j), Scalar(0, 255, 0), 1, 8);
97 cout << "stereo rectify done" << endl;
98 imshow("canvas", canvas);
99 waitKey(50);