Skip to content

Commit a99bf92

Browse files
garybradskigarybradski
committed
added example 19-03
1 parent c41b7bc commit a99bf92

File tree

3 files changed

+335
-2
lines changed

3 files changed

+335
-2
lines changed

‎CMakeLists.txt

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,7 @@ add_executable( example_18-01 example_18-01.cpp )
6666
add_executable( example_18-01_from_disk example_18-01_from_disk.cpp )
6767
add_executable( example_19-01 example_19-01.cpp )
6868
add_executable( example_19-02 example_19-02.cpp )
69-
69+
add_executable( example_19-03 example_19-03.cpp )
7070
add_executable( example_19-04 example_19-04.cpp )
7171
add_executable( example_20-01 example_20-01.cpp )
7272
add_executable( example_20-02 example_20-02.cpp )
@@ -130,7 +130,7 @@ target_link_libraries( example_18-01 ${OpenCV_LIBS} )
130130
target_link_libraries( example_18-01_from_disk ${OpenCV_LIBS} )
131131
target_link_libraries( example_19-01 ${OpenCV_LIBS} )
132132
target_link_libraries( example_19-02 ${OpenCV_LIBS} )
133-
133+
target_link_libraries( example_19-03 ${OpenCV_LIBS} )
134134
target_link_libraries( example_19-04 ${OpenCV_LIBS} )
135135
target_link_libraries( example_20-01 ${OpenCV_LIBS} )
136136
target_link_libraries( example_20-02 ${OpenCV_LIBS} )

‎example_19-03.cpp

Lines changed: 305 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,305 @@
1+
// Example 19-3. Stereo calibration, rectification, and correspondence
2+
#pragma warning(disable : 4996)
3+
#include <opencv2/opencv.hpp>
4+
#include <iostream>
5+
#include <string.h>
6+
#include <stdlib.h>
7+
#include <stdio.h>
8+
#include <math.h>
9+
10+
using namespace std;
11+
12+
void help(char *argv[]) {
13+
cout
14+
<< "\n\nExample 19-3. Stereo calibration, rectification, and "
15+
"correspondence"
16+
<< "\n Reads in list of locations of a sequence of checkerboard "
17+
"calibration"
18+
<< "\n objects from a left,right stereo camera pair. Calibrates, "
19+
"rectifies and then"
20+
<< "\n does stereo correspondence."
21+
<< "\n"
22+
<< "\n This program will run on default parameters assuming you "
23+
"created a build directory"
24+
<< "\n directly below the Learning-OpenCV-3 directory and are "
25+
"running programs there. NOTE: the list_of_stereo_pairs> must"
26+
<< "\n give the full path name to the left right images, in "
27+
"alternating"
28+
<< "\n lines: left image, right image, one path/filename per line, see"
29+
<< "\n stereoData/example_19-03_list.txt file, you can comment out "
30+
"lines"
31+
<< "\n there by starting them with #."
32+
<< "\n"
33+
<< "\nDefault Call (with parameters: board_w = 9, board_h = 6, list = "
34+
"../stereoData_19-03_list.txt):"
35+
<< "\n" << argv[0] << "\n"
36+
<< "\nManual call:"
37+
<< "\n" << argv[0] << " [<board_w> <board_h> <path/list_of_stereo_pairs>]"
38+
<< "\n\n PRESS ANY KEY TO STEP THROUGH RESULTS AT EACH STAGE."
39+
<< "\n" << endl;
40+
}
41+
42+
static void StereoCalib(const char *imageList, int nx, int ny,
43+
bool useUncalibrated) {
44+
bool displayCorners = true;
45+
bool showUndistorted = true;
46+
bool isVerticalStereo = false; // horiz or vert cams
47+
const int maxScale = 1;
48+
const float squareSize = 1.f;
49+
50+
// actual square size
51+
FILE *f = fopen(imageList, "rt");
52+
int i, j, lr;
53+
int N = nx * ny;
54+
cv::Size board_sz = cv::Size(nx, ny);
55+
vector<string> imageNames[2];
56+
vector<cv::Point3f> boardModel;
57+
vector<vector<cv::Point3f> > objectPoints;
58+
vector<vector<cv::Point2f> > points[2];
59+
vector<cv::Point2f> corners[2];
60+
bool found[2] = {false, false};
61+
cv::Size imageSize;
62+
63+
// READ IN THE LIST OF CIRCLE GRIDS:
64+
//
65+
if (!f) {
66+
cout << "Cannot open file " << imageList << endl;
67+
return;
68+
}
69+
for (i = 0; i < ny; i++)
70+
for (j = 0; j < nx; j++)
71+
boardModel.push_back(
72+
cv::Point3f((float)(i * squareSize), (float)(j * squareSize), 0.f));
73+
i = 0;
74+
for (;;) {
75+
char buf[1024];
76+
lr = i % 2;
77+
if (lr == 0)
78+
found[0] = found[1] = false;
79+
if (!fgets(buf, sizeof(buf) - 3, f))
80+
break;
81+
size_t len = strlen(buf);
82+
while (len > 0 && isspace(buf[len - 1]))
83+
buf[--len] = '\0';
84+
if (buf[0] == '#')
85+
continue;
86+
cv::Mat img = cv::imread(buf, 0);
87+
if (img.empty())
88+
break;
89+
imageSize = img.size();
90+
imageNames[lr].push_back(buf);
91+
i++;
92+
93+
// If we did not find board on the left image,
94+
// it does not make sense to find it on the right.
95+
//
96+
if (lr == 1 && !found[0])
97+
continue;
98+
99+
// Find circle grids and centers therein:
100+
for (int s = 1; s <= maxScale; s++) {
101+
cv::Mat timg = img;
102+
if (s > 1)
103+
resize(img, timg, cv::Size(), s, s, cv::INTER_CUBIC);
104+
// Just as example, this would be the call if you had circle calibration
105+
// boards ...
106+
// found[lr] = cv::findCirclesGrid(timg, cv::Size(nx, ny),
107+
// corners[lr],
108+
// cv::CALIB_CB_ASYMMETRIC_GRID |
109+
// cv::CALIB_CB_CLUSTERING);
110+
//...but we have chessboards in our images
111+
found[lr] = cv::findChessboardCorners(timg, board_sz, corners[lr]);
112+
113+
if (found[lr] || s == maxScale) {
114+
cv::Mat mcorners(corners[lr]);
115+
mcorners *= (1. / s);
116+
}
117+
if (found[lr])
118+
break;
119+
}
120+
if (displayCorners) {
121+
cout << buf << endl;
122+
cv::Mat cimg;
123+
cv::cvtColor(img, cimg, cv::COLOR_GRAY2BGR);
124+
125+
// draw chessboard corners works for circle grids too
126+
cv::drawChessboardCorners(cimg, cv::Size(nx, ny), corners[lr], found[lr]);
127+
cv::imshow("Corners", cimg);
128+
if ((cv::waitKey(0) & 255) == 27) // Allow ESC to quit
129+
exit(-1);
130+
} else
131+
cout << '.';
132+
if (lr == 1 && found[0] && found[1]) {
133+
objectPoints.push_back(boardModel);
134+
points[0].push_back(corners[0]);
135+
points[1].push_back(corners[1]);
136+
}
137+
}
138+
fclose(f);
139+
140+
// CALIBRATE THE STEREO CAMERAS
141+
cv::Mat M1 = cv::Mat::eye(3, 3, CV_64F);
142+
cv::Mat M2 = cv::Mat::eye(3, 3, CV_64F);
143+
cv::Mat D1, D2, R, T, E, F;
144+
cout << "\nRunning stereo calibration ...\n";
145+
cv::stereoCalibrate(
146+
objectPoints, points[0], points[1], M1, D1, M2, D2, imageSize, R, T, E, F,
147+
cv::CALIB_FIX_ASPECT_RATIO | cv::CALIB_ZERO_TANGENT_DIST |
148+
cv::CALIB_SAME_FOCAL_LENGTH,
149+
cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 100,
150+
1e-5));
151+
cout << "Done! Press any key to step through images, ESC to exit\n\n";
152+
153+
// CALIBRATION QUALITY CHECK
154+
// because the output fundamental matrix implicitly
155+
// includes all the output information,
156+
// we can check the quality of calibration using the
157+
// epipolar geometry constraint: m2^t*F*m1=0
158+
vector<cv::Point3f> lines[2];
159+
double avgErr = 0;
160+
int nframes = (int)objectPoints.size();
161+
for (i = 0; i < nframes; i++) {
162+
vector<cv::Point2f> &pt0 = points[0][i];
163+
vector<cv::Point2f> &pt1 = points[1][i];
164+
cv::undistortPoints(pt0, pt0, M1, D1, cv::Mat(), M1);
165+
cv::undistortPoints(pt1, pt1, M2, D2, cv::Mat(), M2);
166+
cv::computeCorrespondEpilines(pt0, 1, F, lines[0]);
167+
cv::computeCorrespondEpilines(pt1, 2, F, lines[1]);
168+
169+
for (j = 0; j < N; j++) {
170+
double err = fabs(pt0[j].x * lines[1][j].x + pt0[j].y * lines[1][j].y +
171+
lines[1][j].z) +
172+
fabs(pt1[j].x * lines[0][j].x + pt1[j].y * lines[0][j].y +
173+
lines[0][j].z);
174+
avgErr += err;
175+
}
176+
}
177+
cout << "avg err = " << avgErr / (nframes * N) << endl;
178+
179+
// COMPUTE AND DISPLAY RECTIFICATION
180+
//
181+
if (showUndistorted) {
182+
cv::Mat R1, R2, P1, P2, map11, map12, map21, map22;
183+
184+
// IF BY CALIBRATED (BOUGUET'S METHOD)
185+
//
186+
if (!useUncalibrated) {
187+
stereoRectify(M1, D1, M2, D2, imageSize, R, T, R1, R2, P1, P2,
188+
cv::noArray(), 0);
189+
isVerticalStereo = fabs(P2.at<double>(1, 3)) > fabs(P2.at<double>(0, 3));
190+
// Precompute maps for cvRemap()
191+
initUndistortRectifyMap(M1, D1, R1, P1, imageSize, CV_16SC2, map11,
192+
map12);
193+
initUndistortRectifyMap(M2, D2, R2, P2, imageSize, CV_16SC2, map21,
194+
map22);
195+
}
196+
197+
// OR ELSE HARTLEY'S METHOD
198+
//
199+
else {
200+
201+
// use intrinsic parameters of each camera, but
202+
// compute the rectification transformation directly
203+
// from the fundamental matrix
204+
vector<cv::Point2f> allpoints[2];
205+
for (i = 0; i < nframes; i++) {
206+
copy(points[0][i].begin(), points[0][i].end(),
207+
back_inserter(allpoints[0]));
208+
copy(points[1][i].begin(), points[1][i].end(),
209+
back_inserter(allpoints[1]));
210+
}
211+
cv::Mat F = findFundamentalMat(allpoints[0], allpoints[1], cv::FM_8POINT);
212+
cv::Mat H1, H2;
213+
cv::stereoRectifyUncalibrated(allpoints[0], allpoints[1], F, imageSize,
214+
H1, H2, 3);
215+
R1 = M1.inv() * H1 * M1;
216+
R2 = M2.inv() * H2 * M2;
217+
218+
// Precompute map for cvRemap()
219+
//
220+
cv::initUndistortRectifyMap(M1, D1, R1, P1, imageSize, CV_16SC2, map11,
221+
map12);
222+
cv::initUndistortRectifyMap(M2, D2, R2, P2, imageSize, CV_16SC2, map21,
223+
map22);
224+
}
225+
226+
// RECTIFY THE IMAGES AND FIND DISPARITY MAPS
227+
//
228+
cv::Mat pair;
229+
if (!isVerticalStereo)
230+
pair.create(imageSize.height, imageSize.width * 2, CV_8UC3);
231+
else
232+
pair.create(imageSize.height * 2, imageSize.width, CV_8UC3);
233+
234+
// Setup for finding stereo corrrespondences
235+
//
236+
cv::Ptr<cv::StereoSGBM> stereo = cv::StereoSGBM::create(
237+
-64, 128, 11, 100, 1000, 32, 0, 15, 1000, 16, cv::StereoSGBM::MODE_HH);
238+
239+
for (i = 0; i < nframes; i++) {
240+
cv::Mat img1 = cv::imread(imageNames[0][i].c_str(), 0);
241+
cv::Mat img2 = cv::imread(imageNames[1][i].c_str(), 0);
242+
cv::Mat img1r, img2r, disp, vdisp;
243+
if (img1.empty() || img2.empty())
244+
continue;
245+
cv::remap(img1, img1r, map11, map12, cv::INTER_LINEAR);
246+
cv::remap(img2, img2r, map21, map22, cv::INTER_LINEAR);
247+
if (!isVerticalStereo || !useUncalibrated) {
248+
249+
// When the stereo camera is oriented vertically,
250+
// Hartley method does not transpose the
251+
// image, so the epipolar lines in the rectified
252+
// images are vertical. Stereo correspondence
253+
// function does not support such a case.
254+
stereo->compute(img1r, img2r, disp);
255+
cv::normalize(disp, vdisp, 0, 256, cv::NORM_MINMAX, CV_8U);
256+
cv::imshow("disparity", vdisp);
257+
}
258+
if (!isVerticalStereo) {
259+
cv::Mat part = pair.colRange(0, imageSize.width);
260+
cvtColor(img1r, part, cv::COLOR_GRAY2BGR);
261+
part = pair.colRange(imageSize.width, imageSize.width * 2);
262+
cvtColor(img2r, part, cv::COLOR_GRAY2BGR);
263+
for (j = 0; j < imageSize.height; j += 16)
264+
cv::line(pair, cv::Point(0, j), cv::Point(imageSize.width * 2, j),
265+
cv::Scalar(0, 255, 0));
266+
} else {
267+
cv::Mat part = pair.rowRange(0, imageSize.height);
268+
cv::cvtColor(img1r, part, cv::COLOR_GRAY2BGR);
269+
part = pair.rowRange(imageSize.height, imageSize.height * 2);
270+
cv::cvtColor(img2r, part, cv::COLOR_GRAY2BGR);
271+
for (j = 0; j < imageSize.width; j += 16)
272+
line(pair, cv::Point(j, 0), cv::Point(j, imageSize.height * 2),
273+
cv::Scalar(0, 255, 0));
274+
}
275+
cv::imshow("rectified", pair);
276+
if ((cv::waitKey() & 255) == 27)
277+
break;
278+
}
279+
}
280+
}
281+
282+
//
283+
//Default Call (with parameters: board_w = 9, board_h = 6, list =
284+
// ../stereoData_19-03_list.txt):
285+
//./example_19-03
286+
//
287+
//Manual call:
288+
//./example_19-03 [<board_w> <board_h> <path/list_of_stereo_pairs>]
289+
//
290+
// Press any key to step through results, ESC to exit
291+
//
292+
293+
294+
int main(int argc, char **argv) {
295+
help(argv);
296+
int board_w = 9, board_h = 6;
297+
const char *board_list = "../stereoData/example_19-03_list.txt";
298+
if (argc == 4) {
299+
board_list = argv[1];
300+
board_w = atoi(argv[2]);
301+
board_h = atoi(argv[3]);
302+
}
303+
StereoCalib(board_list, board_w, board_h, true);
304+
return 0;
305+
}

‎stereoData/example_19-03_list.txt

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
../stereoData/left01.jpg
2+
../stereoData/right01.jpg
3+
../stereoData/left02.jpg
4+
../stereoData/right02.jpg
5+
../stereoData/left03.jpg
6+
../stereoData/right03.jpg
7+
../stereoData/left04.jpg
8+
../stereoData/right04.jpg
9+
../stereoData/left05.jpg
10+
../stereoData/right05.jpg
11+
../stereoData/left06.jpg
12+
../stereoData/right06.jpg
13+
../stereoData/left07.jpg
14+
../stereoData/right07.jpg
15+
../stereoData/left08.jpg
16+
../stereoData/right08.jpg
17+
../stereoData/left09.jpg
18+
../stereoData/right09.jpg
19+
#../stereoData/left10.jpg
20+
#../stereoData/right10.jpg
21+
../stereoData/left11.jpg
22+
../stereoData/right11.jpg
23+
../stereoData/left12.jpg
24+
../stereoData/right12.jpg
25+
../stereoData/left13.jpg
26+
../stereoData/right13.jpg
27+
../stereoData/left14.jpg
28+
../stereoData/right14.jpg

0 commit comments

Comments
 (0)