双摄像头立体成像(三)-畸变矫正与立体校正

 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);

 1 bool calibrate(Mat& intrMat, Mat& distCoeffs, vector<vector<Point2f>>& imagePoints,
 2     vector<vector<Point3f>>& ObjectPoints, Size& imageSize,const int cameraId , 
 3     vector<string> imageList)
 4 {
 5     int w = 6;
 6     int h = 9;
 7     double rms = 0;
 8 
 9     Size boardSize;
10     boardSize.width = w;
11     boardSize.height = h;
12     vector<Point2f> pointBuf;
13     float squareSize = 1.f;
14     vector<Mat> rvecs, tvecs;
15     bool ok = false;
16 
17     int nImages = (int)imageList.size() / 2;
18     namedWindow("View", 1);
19     for (int i = 0; i<nImages ; i++)
20     {
21         Mat view, viewGray;
22         view = imread(imageList[i*2+cameraId], 1);
23         imageSize = view.size();
24         cvtColor(view, viewGray, COLOR_BGR2GRAY);
25 
26         bool found = findChessboardCorners(view, boardSize, pointBuf,
27             CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
28 
29         if (found)
30         {
31             cornerSubPix(viewGray, pointBuf, Size(11, 11),
32                 Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
33             drawChessboardCorners(view, boardSize, Mat(pointBuf), found);
34             bitwise_not(view, view);
35             imagePoints.push_back(pointBuf);
36             cout << ‘.‘;
37         }
38         imshow("View", view);
39         waitKey(50);
40     }
41     //calculate chessboardCorners
42     calcChessboardCorners(boardSize, squareSize, ObjectPoints[0]);
43     ObjectPoints.resize(imagePoints.size(), ObjectPoints[0]);
44 
45     rms = calibrateCamera(ObjectPoints, imagePoints, imageSize, intrMat, distCoeffs,
46         rvecs, tvecs);
47     ok = checkRange(intrMat) && checkRange(distCoeffs);
48 
49     if (ok)
50     {
51         cout << "done with RMS error=" << rms << endl;
52         return true;
53     }
54     else
55         return false;
56 }