1 /*
2 * 3calibration.cpp -- Calibrate 3 cameras in a horizontal line together.
3 */
4
5 #include "opencv2/calib3d/calib3d.hpp"
6 #include "opencv2/imgproc/imgproc.hpp"
7 #include "opencv2/imgcodecs/imgcodecs.hpp"
8 #include "opencv2/highgui/highgui.hpp"
9
10 #include <stdio.h>
11 #include <string.h>
12 #include <time.h>
13
14 using namespace cv;
15 using namespace std;
16
17 enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 };
18
help()19 static void help()
20 {
21 printf( "\nThis is a camera calibration sample that calibrates 3 horizontally placed cameras together.\n"
22 "Usage: 3calibration\n"
23 " -w <board_width> # the number of inner corners per one of board dimension\n"
24 " -h <board_height> # the number of inner corners per another board dimension\n"
25 " [-s <squareSize>] # square size in some user-defined units (1 by default)\n"
26 " [-o <out_camera_params>] # the output filename for intrinsic [and extrinsic] parameters\n"
27 " [-zt] # assume zero tangential distortion\n"
28 " [-a <aspectRatio>] # fix aspect ratio (fx/fy)\n"
29 " [-p] # fix the principal point at the center\n"
30 " [input_data] # input data - text file with a list of the images of the board\n"
31 "\n" );
32
33 }
34
calcChessboardCorners(Size boardSize,float squareSize,vector<Point3f> & corners)35 static void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners)
36 {
37 corners.resize(0);
38
39 for( int i = 0; i < boardSize.height; i++ )
40 for( int j = 0; j < boardSize.width; j++ )
41 corners.push_back(Point3f(float(j*squareSize),
42 float(i*squareSize), 0));
43 }
44
run3Calibration(vector<vector<Point2f>> imagePoints1,vector<vector<Point2f>> imagePoints2,vector<vector<Point2f>> imagePoints3,Size imageSize,Size boardSize,float squareSize,float aspectRatio,int flags,Mat & cameraMatrix1,Mat & distCoeffs1,Mat & cameraMatrix2,Mat & distCoeffs2,Mat & cameraMatrix3,Mat & distCoeffs3,Mat & R12,Mat & T12,Mat & R13,Mat & T13)45 static bool run3Calibration( vector<vector<Point2f> > imagePoints1,
46 vector<vector<Point2f> > imagePoints2,
47 vector<vector<Point2f> > imagePoints3,
48 Size imageSize, Size boardSize,
49 float squareSize, float aspectRatio,
50 int flags,
51 Mat& cameraMatrix1, Mat& distCoeffs1,
52 Mat& cameraMatrix2, Mat& distCoeffs2,
53 Mat& cameraMatrix3, Mat& distCoeffs3,
54 Mat& R12, Mat& T12, Mat& R13, Mat& T13)
55 {
56 int c, i;
57
58 // step 1: calibrate each camera individually
59 vector<vector<Point3f> > objpt(1);
60 vector<vector<Point2f> > imgpt;
61 calcChessboardCorners(boardSize, squareSize, objpt[0]);
62 vector<Mat> rvecs, tvecs;
63
64 for( c = 1; c <= 3; c++ )
65 {
66 const vector<vector<Point2f> >& imgpt0 = c == 1 ? imagePoints1 : c == 2 ? imagePoints2 : imagePoints3;
67 imgpt.clear();
68 int N = 0;
69 for( i = 0; i < (int)imgpt0.size(); i++ )
70 if( !imgpt0[i].empty() )
71 {
72 imgpt.push_back(imgpt0[i]);
73 N += (int)imgpt0[i].size();
74 }
75
76 if( imgpt.size() < 3 )
77 {
78 printf("Error: not enough views for camera %d\n", c);
79 return false;
80 }
81
82 objpt.resize(imgpt.size(),objpt[0]);
83
84 Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
85 if( flags & CALIB_FIX_ASPECT_RATIO )
86 cameraMatrix.at<double>(0,0) = aspectRatio;
87
88 Mat distCoeffs = Mat::zeros(5, 1, CV_64F);
89
90 double err = calibrateCamera(objpt, imgpt, imageSize, cameraMatrix,
91 distCoeffs, rvecs, tvecs,
92 flags|CALIB_FIX_K3/*|CALIB_FIX_K4|CALIB_FIX_K5|CALIB_FIX_K6*/);
93 bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);
94 if(!ok)
95 {
96 printf("Error: camera %d was not calibrated\n", c);
97 return false;
98 }
99 printf("Camera %d calibration reprojection error = %g\n", c, sqrt(err/N));
100
101 if( c == 1 )
102 cameraMatrix1 = cameraMatrix, distCoeffs1 = distCoeffs;
103 else if( c == 2 )
104 cameraMatrix2 = cameraMatrix, distCoeffs2 = distCoeffs;
105 else
106 cameraMatrix3 = cameraMatrix, distCoeffs3 = distCoeffs;
107 }
108
109 vector<vector<Point2f> > imgpt_right;
110
111 // step 2: calibrate (1,2) and (3,2) pairs
112 for( c = 2; c <= 3; c++ )
113 {
114 const vector<vector<Point2f> >& imgpt0 = c == 2 ? imagePoints2 : imagePoints3;
115
116 imgpt.clear();
117 imgpt_right.clear();
118 int N = 0;
119
120 for( i = 0; i < (int)std::min(imagePoints1.size(), imgpt0.size()); i++ )
121 if( !imagePoints1.empty() && !imgpt0[i].empty() )
122 {
123 imgpt.push_back(imagePoints1[i]);
124 imgpt_right.push_back(imgpt0[i]);
125 N += (int)imgpt0[i].size();
126 }
127
128 if( imgpt.size() < 3 )
129 {
130 printf("Error: not enough shared views for cameras 1 and %d\n", c);
131 return false;
132 }
133
134 objpt.resize(imgpt.size(),objpt[0]);
135 Mat cameraMatrix = c == 2 ? cameraMatrix2 : cameraMatrix3;
136 Mat distCoeffs = c == 2 ? distCoeffs2 : distCoeffs3;
137 Mat R, T, E, F;
138 double err = stereoCalibrate(objpt, imgpt, imgpt_right, cameraMatrix1, distCoeffs1,
139 cameraMatrix, distCoeffs,
140 imageSize, R, T, E, F,
141 CALIB_FIX_INTRINSIC,
142 TermCriteria(TermCriteria::COUNT, 30, 0));
143 printf("Pair (1,%d) calibration reprojection error = %g\n", c, sqrt(err/(N*2)));
144 if( c == 2 )
145 {
146 cameraMatrix2 = cameraMatrix;
147 distCoeffs2 = distCoeffs;
148 R12 = R; T12 = T;
149 }
150 else
151 {
152 R13 = R; T13 = T;
153 }
154 }
155
156 return true;
157 }
158
readStringList(const string & filename,vector<string> & l)159 static bool readStringList( const string& filename, vector<string>& l )
160 {
161 l.resize(0);
162 FileStorage fs(filename, FileStorage::READ);
163 if( !fs.isOpened() )
164 return false;
165 FileNode n = fs.getFirstTopLevelNode();
166 if( n.type() != FileNode::SEQ )
167 return false;
168 FileNodeIterator it = n.begin(), it_end = n.end();
169 for( ; it != it_end; ++it )
170 l.push_back((string)*it);
171 return true;
172 }
173
174
main(int argc,char ** argv)175 int main( int argc, char** argv )
176 {
177 int i, k;
178 int flags = 0;
179 Size boardSize, imageSize;
180 float squareSize = 1.f, aspectRatio = 1.f;
181 const char* outputFilename = "out_camera_data.yml";
182 const char* inputFilename = 0;
183
184 vector<vector<Point2f> > imgpt[3];
185 vector<string> imageList;
186
187 if(argc < 2)
188 {
189 help();
190 return 1;
191 }
192
193
194 for( i = 1; i < argc; i++ )
195 {
196 const char* s = argv[i];
197 if( strcmp( s, "-w" ) == 0 )
198 {
199 if( sscanf( argv[++i], "%u", &boardSize.width ) != 1 || boardSize.width <= 0 )
200 return fprintf( stderr, "Invalid board width\n" ), -1;
201 }
202 else if( strcmp( s, "-h" ) == 0 )
203 {
204 if( sscanf( argv[++i], "%u", &boardSize.height ) != 1 || boardSize.height <= 0 )
205 return fprintf( stderr, "Invalid board height\n" ), -1;
206 }
207 else if( strcmp( s, "-s" ) == 0 )
208 {
209 if( sscanf( argv[++i], "%f", &squareSize ) != 1 || squareSize <= 0 )
210 return fprintf( stderr, "Invalid board square width\n" ), -1;
211 }
212 else if( strcmp( s, "-a" ) == 0 )
213 {
214 if( sscanf( argv[++i], "%f", &aspectRatio ) != 1 || aspectRatio <= 0 )
215 return printf("Invalid aspect ratio\n" ), -1;
216 flags |= CALIB_FIX_ASPECT_RATIO;
217 }
218 else if( strcmp( s, "-zt" ) == 0 )
219 {
220 flags |= CALIB_ZERO_TANGENT_DIST;
221 }
222 else if( strcmp( s, "-p" ) == 0 )
223 {
224 flags |= CALIB_FIX_PRINCIPAL_POINT;
225 }
226 else if( strcmp( s, "-o" ) == 0 )
227 {
228 outputFilename = argv[++i];
229 }
230 else if( s[0] != '-' )
231 {
232 inputFilename = s;
233 }
234 else
235 return fprintf( stderr, "Unknown option %s", s ), -1;
236 }
237
238 if( !inputFilename ||
239 !readStringList(inputFilename, imageList) ||
240 imageList.size() == 0 || imageList.size() % 3 != 0 )
241 {
242 printf("Error: the input image list is not specified, or can not be read, or the number of files is not divisible by 3\n");
243 return -1;
244 }
245
246 Mat view, viewGray;
247 Mat cameraMatrix[3], distCoeffs[3], R[3], P[3], R12, T12;
248 for( k = 0; k < 3; k++ )
249 {
250 cameraMatrix[k] = Mat_<double>::eye(3,3);
251 cameraMatrix[k].at<double>(0,0) = aspectRatio;
252 cameraMatrix[k].at<double>(1,1) = 1;
253 distCoeffs[k] = Mat_<double>::zeros(5,1);
254 }
255 Mat R13=Mat_<double>::eye(3,3), T13=Mat_<double>::zeros(3,1);
256
257 FileStorage fs;
258 namedWindow( "Image View", 0 );
259
260 for( k = 0; k < 3; k++ )
261 imgpt[k].resize(imageList.size()/3);
262
263 for( i = 0; i < (int)(imageList.size()/3); i++ )
264 {
265 for( k = 0; k < 3; k++ )
266 {
267 int k1 = k == 0 ? 2 : k == 1 ? 0 : 1;
268 printf("%s\n", imageList[i*3+k].c_str());
269 view = imread(imageList[i*3+k], 1);
270
271 if(!view.empty())
272 {
273 vector<Point2f> ptvec;
274 imageSize = view.size();
275 cvtColor(view, viewGray, COLOR_BGR2GRAY);
276 bool found = findChessboardCorners( view, boardSize, ptvec, CALIB_CB_ADAPTIVE_THRESH );
277
278 drawChessboardCorners( view, boardSize, Mat(ptvec), found );
279 if( found )
280 {
281 imgpt[k1][i].resize(ptvec.size());
282 std::copy(ptvec.begin(), ptvec.end(), imgpt[k1][i].begin());
283 }
284 //imshow("view", view);
285 //int c = waitKey(0) & 255;
286 //if( c == 27 || c == 'q' || c == 'Q' )
287 // return -1;
288 }
289 }
290 }
291
292 printf("Running calibration ...\n");
293
294 run3Calibration(imgpt[0], imgpt[1], imgpt[2], imageSize,
295 boardSize, squareSize, aspectRatio, flags|CALIB_FIX_K4|CALIB_FIX_K5,
296 cameraMatrix[0], distCoeffs[0],
297 cameraMatrix[1], distCoeffs[1],
298 cameraMatrix[2], distCoeffs[2],
299 R12, T12, R13, T13);
300
301 fs.open(outputFilename, FileStorage::WRITE);
302
303 fs << "cameraMatrix1" << cameraMatrix[0];
304 fs << "cameraMatrix2" << cameraMatrix[1];
305 fs << "cameraMatrix3" << cameraMatrix[2];
306
307 fs << "distCoeffs1" << distCoeffs[0];
308 fs << "distCoeffs2" << distCoeffs[1];
309 fs << "distCoeffs3" << distCoeffs[2];
310
311 fs << "R12" << R12;
312 fs << "T12" << T12;
313 fs << "R13" << R13;
314 fs << "T13" << T13;
315
316 fs << "imageWidth" << imageSize.width;
317 fs << "imageHeight" << imageSize.height;
318
319 Mat Q;
320
321 // step 3: find rectification transforms
322 double ratio = rectify3Collinear(cameraMatrix[0], distCoeffs[0], cameraMatrix[1],
323 distCoeffs[1], cameraMatrix[2], distCoeffs[2],
324 imgpt[0], imgpt[2],
325 imageSize, R12, T12, R13, T13,
326 R[0], R[1], R[2], P[0], P[1], P[2], Q, -1.,
327 imageSize, 0, 0, CALIB_ZERO_DISPARITY);
328 Mat map1[3], map2[3];
329
330 fs << "R1" << R[0];
331 fs << "R2" << R[1];
332 fs << "R3" << R[2];
333
334 fs << "P1" << P[0];
335 fs << "P2" << P[1];
336 fs << "P3" << P[2];
337
338 fs << "disparityRatio" << ratio;
339 fs.release();
340
341 printf("Disparity ratio = %g\n", ratio);
342
343 for( k = 0; k < 3; k++ )
344 initUndistortRectifyMap(cameraMatrix[k], distCoeffs[k], R[k], P[k], imageSize, CV_16SC2, map1[k], map2[k]);
345
346 Mat canvas(imageSize.height, imageSize.width*3, CV_8UC3), small_canvas;
347 destroyWindow("view");
348 canvas = Scalar::all(0);
349
350 for( i = 0; i < (int)(imageList.size()/3); i++ )
351 {
352 canvas = Scalar::all(0);
353 for( k = 0; k < 3; k++ )
354 {
355 int k1 = k == 0 ? 2 : k == 1 ? 0 : 1;
356 int k2 = k == 0 ? 1 : k == 1 ? 0 : 2;
357 view = imread(imageList[i*3+k], 1);
358
359 if(view.empty())
360 continue;
361
362 Mat rview = canvas.colRange(k2*imageSize.width, (k2+1)*imageSize.width);
363 remap(view, rview, map1[k1], map2[k1], INTER_LINEAR);
364 }
365 printf("%s %s %s\n", imageList[i*3].c_str(), imageList[i*3+1].c_str(), imageList[i*3+2].c_str());
366 resize( canvas, small_canvas, Size(1500, 1500/3) );
367 for( k = 0; k < small_canvas.rows; k += 16 )
368 line(small_canvas, Point(0, k), Point(small_canvas.cols, k), Scalar(0,255,0), 1);
369 imshow("rectified", small_canvas);
370 int c = waitKey(0);
371 if( c == 27 || c == 'q' || c == 'Q' )
372 break;
373 }
374
375 return 0;
376 }
377