1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
8 //
9 //
10 // License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
14 // Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved.
15 // Third party copyrights are property of their respective owners.
16 //
17 // Redistribution and use in source and binary forms, with or without modification,
18 // are permitted provided that the following conditions are met:
19 //
20 // * Redistribution's of source code must retain the above copyright notice,
21 // this list of conditions and the following disclaimer.
22 //
23 // * Redistribution's in binary form must reproduce the above copyright notice,
24 // this list of conditions and the following disclaimer in the documentation
25 // and/or other materials provided with the distribution.
26 //
27 // * The name of the copyright holders may not be used to endorse or promote products
28 // derived from this software without specific prior written permission.
29 //
30 // This software is provided by the copyright holders and contributors "as is" and
31 // any express or implied warranties, including, but not limited to, the implied
32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
33 // In no event shall the Intel Corporation or contributors be liable for any direct,
34 // indirect, incidental, special, exemplary, or consequential damages
35 // (including, but not limited to, procurement of substitute goods or services;
36 // loss of use, data, or profits; or business interruption) however caused
37 // and on any theory of liability, whether in contract, strict liability,
38 // or tort (including negligence or otherwise) arising in any way out of
39 // the use of this software, even if advised of the possibility of such damage.
40 //
41 //M*/
42
43 #include "test_precomp.hpp"
44 #include <opencv2/ts/cuda_test.hpp> // EXPECT_MAT_NEAR
45 #include "../src/fisheye.hpp"
46 #include "opencv2/videoio.hpp"
47
48 namespace opencv_test { namespace {
49
50 class fisheyeTest : public ::testing::Test {
51
52 protected:
53 const static cv::Size imageSize;
54 const static cv::Matx33d K;
55 const static cv::Vec4d D;
56 const static cv::Matx33d R;
57 const static cv::Vec3d T;
58 std::string datasets_repository_path;
59
SetUp()60 virtual void SetUp() {
61 datasets_repository_path = combine(cvtest::TS::ptr()->get_data_path(), "cv/cameracalibration/fisheye");
62 }
63
64 protected:
65 std::string combine(const std::string& _item1, const std::string& _item2);
66 static void merge4(const cv::Mat& tl, const cv::Mat& tr, const cv::Mat& bl, const cv::Mat& br, cv::Mat& merged);
67 };
68
69 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
70 /// TESTS::
71
TEST_F(fisheyeTest,projectPoints)72 TEST_F(fisheyeTest, projectPoints)
73 {
74 double cols = this->imageSize.width,
75 rows = this->imageSize.height;
76
77 const int N = 20;
78 cv::Mat distorted0(1, N*N, CV_64FC2), undist1, undist2, distorted1, distorted2;
79 undist2.create(distorted0.size(), CV_MAKETYPE(distorted0.depth(), 3));
80 cv::Vec2d* pts = distorted0.ptr<cv::Vec2d>();
81
82 cv::Vec2d c(this->K(0, 2), this->K(1, 2));
83 for(int y = 0, k = 0; y < N; ++y)
84 for(int x = 0; x < N; ++x)
85 {
86 cv::Vec2d point(x*cols/(N-1.f), y*rows/(N-1.f));
87 pts[k++] = (point - c) * 0.85 + c;
88 }
89
90 cv::fisheye::undistortPoints(distorted0, undist1, this->K, this->D);
91
92 cv::Vec2d* u1 = undist1.ptr<cv::Vec2d>();
93 cv::Vec3d* u2 = undist2.ptr<cv::Vec3d>();
94 for(int i = 0; i < (int)distorted0.total(); ++i)
95 u2[i] = cv::Vec3d(u1[i][0], u1[i][1], 1.0);
96
97 cv::fisheye::distortPoints(undist1, distorted1, this->K, this->D);
98 cv::fisheye::projectPoints(undist2, distorted2, cv::Vec3d::all(0), cv::Vec3d::all(0), this->K, this->D);
99
100 EXPECT_MAT_NEAR(distorted0, distorted1, 1e-10);
101 EXPECT_MAT_NEAR(distorted0, distorted2, 1e-10);
102 }
103
TEST_F(fisheyeTest,undistortImage)104 TEST_F(fisheyeTest, undistortImage)
105 {
106 cv::Matx33d theK = this->K;
107 cv::Mat theD = cv::Mat(this->D);
108 std::string file = combine(datasets_repository_path, "/calib-3_stereo_from_JY/left/stereo_pair_014.jpg");
109 cv::Matx33d newK = theK;
110 cv::Mat distorted = cv::imread(file), undistorted;
111 {
112 newK(0, 0) = 100;
113 newK(1, 1) = 100;
114 cv::fisheye::undistortImage(distorted, undistorted, theK, theD, newK);
115 cv::Mat correct = cv::imread(combine(datasets_repository_path, "new_f_100.png"));
116 if (correct.empty())
117 CV_Assert(cv::imwrite(combine(datasets_repository_path, "new_f_100.png"), undistorted));
118 else
119 EXPECT_MAT_NEAR(correct, undistorted, 1e-10);
120 }
121 {
122 double balance = 1.0;
123 cv::fisheye::estimateNewCameraMatrixForUndistortRectify(theK, theD, distorted.size(), cv::noArray(), newK, balance);
124 cv::fisheye::undistortImage(distorted, undistorted, theK, theD, newK);
125 cv::Mat correct = cv::imread(combine(datasets_repository_path, "balance_1.0.png"));
126 if (correct.empty())
127 CV_Assert(cv::imwrite(combine(datasets_repository_path, "balance_1.0.png"), undistorted));
128 else
129 EXPECT_MAT_NEAR(correct, undistorted, 1e-10);
130 }
131
132 {
133 double balance = 0.0;
134 cv::fisheye::estimateNewCameraMatrixForUndistortRectify(theK, theD, distorted.size(), cv::noArray(), newK, balance);
135 cv::fisheye::undistortImage(distorted, undistorted, theK, theD, newK);
136 cv::Mat correct = cv::imread(combine(datasets_repository_path, "balance_0.0.png"));
137 if (correct.empty())
138 CV_Assert(cv::imwrite(combine(datasets_repository_path, "balance_0.0.png"), undistorted));
139 else
140 EXPECT_MAT_NEAR(correct, undistorted, 1e-10);
141 }
142 }
143
TEST_F(fisheyeTest,undistortAndDistortImage)144 TEST_F(fisheyeTest, undistortAndDistortImage)
145 {
146 cv::Matx33d K_src = this->K;
147 cv::Mat D_src = cv::Mat(this->D);
148 std::string file = combine(datasets_repository_path, "/calib-3_stereo_from_JY/left/stereo_pair_014.jpg");
149 cv::Matx33d K_dst = K_src;
150 cv::Mat image = cv::imread(file), image_projected;
151 cv::Vec4d D_dst_vec (-1.0, 0.0, 0.0, 0.0);
152 cv::Mat D_dst = cv::Mat(D_dst_vec);
153
154 int imageWidth = (int)this->imageSize.width;
155 int imageHeight = (int)this->imageSize.height;
156
157 cv::Mat imagePoints(imageHeight, imageWidth, CV_32FC2), undPoints, distPoints;
158 cv::Vec2f* pts = imagePoints.ptr<cv::Vec2f>();
159
160 for(int y = 0, k = 0; y < imageHeight; ++y)
161 {
162 for(int x = 0; x < imageWidth; ++x)
163 {
164 cv::Vec2f point((float)x, (float)y);
165 pts[k++] = point;
166 }
167 }
168
169 cv::fisheye::undistortPoints(imagePoints, undPoints, K_dst, D_dst);
170 cv::fisheye::distortPoints(undPoints, distPoints, K_src, D_src);
171 cv::remap(image, image_projected, distPoints, cv::noArray(), cv::INTER_LINEAR);
172
173 float dx, dy, r_sq;
174 float R_MAX = 250;
175 float imageCenterX = (float)imageWidth / 2;
176 float imageCenterY = (float)imageHeight / 2;
177
178 cv::Mat undPointsGt(imageHeight, imageWidth, CV_32FC2);
179 cv::Mat imageGt(imageHeight, imageWidth, CV_8UC3);
180
181 for(int y = 0, k = 0; y < imageHeight; ++y)
182 {
183 for(int x = 0; x < imageWidth; ++x)
184 {
185 dx = x - imageCenterX;
186 dy = y - imageCenterY;
187 r_sq = dy * dy + dx * dx;
188
189 Vec2f & und_vec = undPoints.at<Vec2f>(y,x);
190 Vec3b & pixel = image_projected.at<Vec3b>(y,x);
191
192 Vec2f & undist_vec_gt = undPointsGt.at<Vec2f>(y,x);
193 Vec3b & pixel_gt = imageGt.at<Vec3b>(y,x);
194
195 if (r_sq > R_MAX * R_MAX)
196 {
197
198 undist_vec_gt[0] = -1e6;
199 undist_vec_gt[1] = -1e6;
200
201 pixel_gt[0] = 0;
202 pixel_gt[1] = 0;
203 pixel_gt[2] = 0;
204 }
205 else
206 {
207 undist_vec_gt[0] = und_vec[0];
208 undist_vec_gt[1] = und_vec[1];
209
210 pixel_gt[0] = pixel[0];
211 pixel_gt[1] = pixel[1];
212 pixel_gt[2] = pixel[2];
213 }
214
215 k++;
216 }
217 }
218
219 EXPECT_MAT_NEAR(undPoints, undPointsGt, 1e-10);
220 EXPECT_MAT_NEAR(image_projected, imageGt, 1e-10);
221
222 Vec2f dist_point_1 = distPoints.at<Vec2f>(400, 640);
223 Vec2f dist_point_1_gt(640.044f, 400.041f);
224
225 Vec2f dist_point_2 = distPoints.at<Vec2f>(400, 440);
226 Vec2f dist_point_2_gt(409.731f, 403.029f);
227
228 Vec2f dist_point_3 = distPoints.at<Vec2f>(200, 640);
229 Vec2f dist_point_3_gt(643.341f, 168.896f);
230
231 Vec2f dist_point_4 = distPoints.at<Vec2f>(300, 480);
232 Vec2f dist_point_4_gt(463.402f, 290.317f);
233
234 Vec2f dist_point_5 = distPoints.at<Vec2f>(550, 750);
235 Vec2f dist_point_5_gt(797.51f, 611.637f);
236
237 EXPECT_MAT_NEAR(dist_point_1, dist_point_1_gt, 1e-2);
238 EXPECT_MAT_NEAR(dist_point_2, dist_point_2_gt, 1e-2);
239 EXPECT_MAT_NEAR(dist_point_3, dist_point_3_gt, 1e-2);
240 EXPECT_MAT_NEAR(dist_point_4, dist_point_4_gt, 1e-2);
241 EXPECT_MAT_NEAR(dist_point_5, dist_point_5_gt, 1e-2);
242
243 CV_Assert(cv::imwrite(combine(datasets_repository_path, "new_distortion.png"), image_projected));
244 }
245
TEST_F(fisheyeTest,jacobians)246 TEST_F(fisheyeTest, jacobians)
247 {
248 int n = 10;
249 cv::Mat X(1, n, CV_64FC3);
250 cv::Mat om(3, 1, CV_64F), theT(3, 1, CV_64F);
251 cv::Mat f(2, 1, CV_64F), c(2, 1, CV_64F);
252 cv::Mat k(4, 1, CV_64F);
253 double alpha;
254
255 cv::RNG r;
256
257 r.fill(X, cv::RNG::NORMAL, 2, 1);
258 X = cv::abs(X) * 10;
259
260 r.fill(om, cv::RNG::NORMAL, 0, 1);
261 om = cv::abs(om);
262
263 r.fill(theT, cv::RNG::NORMAL, 0, 1);
264 theT = cv::abs(theT); theT.at<double>(2) = 4; theT *= 10;
265
266 r.fill(f, cv::RNG::NORMAL, 0, 1);
267 f = cv::abs(f) * 1000;
268
269 r.fill(c, cv::RNG::NORMAL, 0, 1);
270 c = cv::abs(c) * 1000;
271
272 r.fill(k, cv::RNG::NORMAL, 0, 1);
273 k*= 0.5;
274
275 alpha = 0.01*r.gaussian(1);
276
277 cv::Mat x1, x2, xpred;
278 cv::Matx33d theK(f.at<double>(0), alpha * f.at<double>(0), c.at<double>(0),
279 0, f.at<double>(1), c.at<double>(1),
280 0, 0, 1);
281
282 cv::Mat jacobians;
283 cv::fisheye::projectPoints(X, x1, om, theT, theK, k, alpha, jacobians);
284
285 //test on T:
286 cv::Mat dT(3, 1, CV_64FC1);
287 r.fill(dT, cv::RNG::NORMAL, 0, 1);
288 dT *= 1e-9*cv::norm(theT);
289 cv::Mat T2 = theT + dT;
290 cv::fisheye::projectPoints(X, x2, om, T2, theK, k, alpha, cv::noArray());
291 xpred = x1 + cv::Mat(jacobians.colRange(11,14) * dT).reshape(2, 1);
292 CV_Assert (cv::norm(x2 - xpred) < 1e-10);
293
294 //test on om:
295 cv::Mat dom(3, 1, CV_64FC1);
296 r.fill(dom, cv::RNG::NORMAL, 0, 1);
297 dom *= 1e-9*cv::norm(om);
298 cv::Mat om2 = om + dom;
299 cv::fisheye::projectPoints(X, x2, om2, theT, theK, k, alpha, cv::noArray());
300 xpred = x1 + cv::Mat(jacobians.colRange(8,11) * dom).reshape(2, 1);
301 CV_Assert (cv::norm(x2 - xpred) < 1e-10);
302
303 //test on f:
304 cv::Mat df(2, 1, CV_64FC1);
305 r.fill(df, cv::RNG::NORMAL, 0, 1);
306 df *= 1e-9*cv::norm(f);
307 cv::Matx33d K2 = theK + cv::Matx33d(df.at<double>(0), df.at<double>(0) * alpha, 0, 0, df.at<double>(1), 0, 0, 0, 0);
308 cv::fisheye::projectPoints(X, x2, om, theT, K2, k, alpha, cv::noArray());
309 xpred = x1 + cv::Mat(jacobians.colRange(0,2) * df).reshape(2, 1);
310 CV_Assert (cv::norm(x2 - xpred) < 1e-10);
311
312 //test on c:
313 cv::Mat dc(2, 1, CV_64FC1);
314 r.fill(dc, cv::RNG::NORMAL, 0, 1);
315 dc *= 1e-9*cv::norm(c);
316 K2 = theK + cv::Matx33d(0, 0, dc.at<double>(0), 0, 0, dc.at<double>(1), 0, 0, 0);
317 cv::fisheye::projectPoints(X, x2, om, theT, K2, k, alpha, cv::noArray());
318 xpred = x1 + cv::Mat(jacobians.colRange(2,4) * dc).reshape(2, 1);
319 CV_Assert (cv::norm(x2 - xpred) < 1e-10);
320
321 //test on k:
322 cv::Mat dk(4, 1, CV_64FC1);
323 r.fill(dk, cv::RNG::NORMAL, 0, 1);
324 dk *= 1e-9*cv::norm(k);
325 cv::Mat k2 = k + dk;
326 cv::fisheye::projectPoints(X, x2, om, theT, theK, k2, alpha, cv::noArray());
327 xpred = x1 + cv::Mat(jacobians.colRange(4,8) * dk).reshape(2, 1);
328 CV_Assert (cv::norm(x2 - xpred) < 1e-10);
329
330 //test on alpha:
331 cv::Mat dalpha(1, 1, CV_64FC1);
332 r.fill(dalpha, cv::RNG::NORMAL, 0, 1);
333 dalpha *= 1e-9*cv::norm(f);
334 double alpha2 = alpha + dalpha.at<double>(0);
335 K2 = theK + cv::Matx33d(0, f.at<double>(0) * dalpha.at<double>(0), 0, 0, 0, 0, 0, 0, 0);
336 cv::fisheye::projectPoints(X, x2, om, theT, theK, k, alpha2, cv::noArray());
337 xpred = x1 + cv::Mat(jacobians.col(14) * dalpha).reshape(2, 1);
338 CV_Assert (cv::norm(x2 - xpred) < 1e-10);
339 }
340
TEST_F(fisheyeTest,Calibration)341 TEST_F(fisheyeTest, Calibration)
342 {
343 const int n_images = 34;
344
345 std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
346 std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
347
348 const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY");
349 cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
350 CV_Assert(fs_left.isOpened());
351 for(int i = 0; i < n_images; ++i)
352 fs_left[cv::format("image_%d", i )] >> imagePoints[i];
353 fs_left.release();
354
355 cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
356 CV_Assert(fs_object.isOpened());
357 for(int i = 0; i < n_images; ++i)
358 fs_object[cv::format("image_%d", i )] >> objectPoints[i];
359 fs_object.release();
360
361 int flag = 0;
362 flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
363 flag |= cv::fisheye::CALIB_CHECK_COND;
364 flag |= cv::fisheye::CALIB_FIX_SKEW;
365
366 cv::Matx33d theK;
367 cv::Vec4d theD;
368
369 cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, theK, theD,
370 cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6));
371
372 EXPECT_MAT_NEAR(theK, this->K, 1e-10);
373 EXPECT_MAT_NEAR(theD, this->D, 1e-10);
374 }
375
TEST_F(fisheyeTest,CalibrationWithFixedFocalLength)376 TEST_F(fisheyeTest, CalibrationWithFixedFocalLength)
377 {
378 const int n_images = 34;
379
380 std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
381 std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
382
383 const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
384 cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
385 CV_Assert(fs_left.isOpened());
386 for(int i = 0; i < n_images; ++i)
387 fs_left[cv::format("image_%d", i )] >> imagePoints[i];
388 fs_left.release();
389
390 cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
391 CV_Assert(fs_object.isOpened());
392 for(int i = 0; i < n_images; ++i)
393 fs_object[cv::format("image_%d", i )] >> objectPoints[i];
394 fs_object.release();
395
396 int flag = 0;
397 flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
398 flag |= cv::fisheye::CALIB_CHECK_COND;
399 flag |= cv::fisheye::CALIB_FIX_SKEW;
400 flag |= cv::fisheye::CALIB_FIX_FOCAL_LENGTH;
401 flag |= cv::fisheye::CALIB_USE_INTRINSIC_GUESS;
402
403 cv::Matx33d theK = this->K;
404 const cv::Matx33d newK(
405 558.478088, 0.000000, 620.458461,
406 0.000000, 560.506767, 381.939362,
407 0.000000, 0.000000, 1.000000);
408
409 cv::Vec4d theD;
410 const cv::Vec4d newD(-0.001461, -0.003298, 0.006057, -0.003742);
411
412 cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, theK, theD,
413 cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6));
414
415 // ensure that CALIB_FIX_FOCAL_LENGTH works and focal lenght has not changed
416 EXPECT_EQ(theK(0,0), K(0,0));
417 EXPECT_EQ(theK(1,1), K(1,1));
418
419 EXPECT_MAT_NEAR(theK, newK, 1e-6);
420 EXPECT_MAT_NEAR(theD, newD, 1e-6);
421 }
422
TEST_F(fisheyeTest,Homography)423 TEST_F(fisheyeTest, Homography)
424 {
425 const int n_images = 1;
426
427 std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
428 std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
429
430 const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY");
431 cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
432 CV_Assert(fs_left.isOpened());
433 for(int i = 0; i < n_images; ++i)
434 fs_left[cv::format("image_%d", i )] >> imagePoints[i];
435 fs_left.release();
436
437 cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
438 CV_Assert(fs_object.isOpened());
439 for(int i = 0; i < n_images; ++i)
440 fs_object[cv::format("image_%d", i )] >> objectPoints[i];
441 fs_object.release();
442
443 cv::internal::IntrinsicParams param;
444 param.Init(cv::Vec2d(cv::max(imageSize.width, imageSize.height) / CV_PI, cv::max(imageSize.width, imageSize.height) / CV_PI),
445 cv::Vec2d(imageSize.width / 2.0 - 0.5, imageSize.height / 2.0 - 0.5));
446
447 cv::Mat _imagePoints (imagePoints[0]);
448 cv::Mat _objectPoints(objectPoints[0]);
449
450 cv::Mat imagePointsNormalized = NormalizePixels(_imagePoints, param).reshape(1).t();
451 _objectPoints = _objectPoints.reshape(1).t();
452 cv::Mat objectPointsMean, covObjectPoints;
453
454 int Np = imagePointsNormalized.cols;
455 cv::calcCovarMatrix(_objectPoints, covObjectPoints, objectPointsMean, cv::COVAR_NORMAL | cv::COVAR_COLS);
456 cv::SVD svd(covObjectPoints);
457 cv::Mat theR(svd.vt);
458
459 if (cv::norm(theR(cv::Rect(2, 0, 1, 2))) < 1e-6)
460 theR = cv::Mat::eye(3,3, CV_64FC1);
461 if (cv::determinant(theR) < 0)
462 theR = -theR;
463
464 cv::Mat theT = -theR * objectPointsMean;
465 cv::Mat X_new = theR * _objectPoints + theT * cv::Mat::ones(1, Np, CV_64FC1);
466 cv::Mat H = cv::internal::ComputeHomography(imagePointsNormalized, X_new.rowRange(0, 2));
467
468 cv::Mat M = cv::Mat::ones(3, X_new.cols, CV_64FC1);
469 X_new.rowRange(0, 2).copyTo(M.rowRange(0, 2));
470 cv::Mat mrep = H * M;
471
472 cv::divide(mrep, cv::Mat::ones(3,1, CV_64FC1) * mrep.row(2).clone(), mrep);
473
474 cv::Mat merr = (mrep.rowRange(0, 2) - imagePointsNormalized).t();
475
476 cv::Vec2d std_err;
477 cv::meanStdDev(merr.reshape(2), cv::noArray(), std_err);
478 std_err *= sqrt((double)merr.reshape(2).total() / (merr.reshape(2).total() - 1));
479
480 cv::Vec2d correct_std_err(0.00516740156010384, 0.00644205331553901);
481 EXPECT_MAT_NEAR(std_err, correct_std_err, 1e-12);
482 }
483
TEST_F(fisheyeTest,EstimateUncertainties)484 TEST_F(fisheyeTest, EstimateUncertainties)
485 {
486 const int n_images = 34;
487
488 std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
489 std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
490
491 const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
492 cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
493 CV_Assert(fs_left.isOpened());
494 for(int i = 0; i < n_images; ++i)
495 fs_left[cv::format("image_%d", i )] >> imagePoints[i];
496 fs_left.release();
497
498 cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
499 CV_Assert(fs_object.isOpened());
500 for(int i = 0; i < n_images; ++i)
501 fs_object[cv::format("image_%d", i )] >> objectPoints[i];
502 fs_object.release();
503
504 int flag = 0;
505 flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
506 flag |= cv::fisheye::CALIB_CHECK_COND;
507 flag |= cv::fisheye::CALIB_FIX_SKEW;
508
509 cv::Matx33d theK;
510 cv::Vec4d theD;
511 std::vector<cv::Vec3d> rvec;
512 std::vector<cv::Vec3d> tvec;
513
514 cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, theK, theD,
515 rvec, tvec, flag, cv::TermCriteria(3, 20, 1e-6));
516
517 cv::internal::IntrinsicParams param, errors;
518 cv::Vec2d err_std;
519 double thresh_cond = 1e6;
520 int check_cond = 1;
521 param.Init(cv::Vec2d(theK(0,0), theK(1,1)), cv::Vec2d(theK(0,2), theK(1, 2)), theD);
522 param.isEstimate = std::vector<uchar>(9, 1);
523 param.isEstimate[4] = 0;
524
525 errors.isEstimate = param.isEstimate;
526
527 double rms;
528
529 cv::internal::EstimateUncertainties(objectPoints, imagePoints, param, rvec, tvec,
530 errors, err_std, thresh_cond, check_cond, rms);
531
532 EXPECT_MAT_NEAR(errors.f, cv::Vec2d(1.29837104202046, 1.31565641071524), 1e-10);
533 EXPECT_MAT_NEAR(errors.c, cv::Vec2d(0.890439368129246, 0.816096854937896), 1e-10);
534 EXPECT_MAT_NEAR(errors.k, cv::Vec4d(0.00516248605191506, 0.0168181467500934, 0.0213118690274604, 0.00916010877545648), 1e-10);
535 EXPECT_MAT_NEAR(err_std, cv::Vec2d(0.187475975266883, 0.185678953263995), 1e-10);
536 CV_Assert(fabs(rms - 0.263782587133546) < 1e-10);
537 CV_Assert(errors.alpha == 0);
538 }
539
TEST_F(fisheyeTest,stereoRectify)540 TEST_F(fisheyeTest, stereoRectify)
541 {
542 // For consistency purposes
543 CV_StaticAssert(
544 static_cast<int>(cv::CALIB_ZERO_DISPARITY) == static_cast<int>(cv::fisheye::CALIB_ZERO_DISPARITY),
545 "For the purpose of continuity the following should be true: cv::CALIB_ZERO_DISPARITY == cv::fisheye::CALIB_ZERO_DISPARITY"
546 );
547
548 const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY");
549
550 cv::Size calibration_size = this->imageSize, requested_size = calibration_size;
551 cv::Matx33d K1 = this->K, K2 = K1;
552 cv::Mat D1 = cv::Mat(this->D), D2 = D1;
553
554 cv::Vec3d theT = this->T;
555 cv::Matx33d theR = this->R;
556
557 double balance = 0.0, fov_scale = 1.1;
558 cv::Mat R1, R2, P1, P2, Q;
559 cv::fisheye::stereoRectify(K1, D1, K2, D2, calibration_size, theR, theT, R1, R2, P1, P2, Q,
560 cv::fisheye::CALIB_ZERO_DISPARITY, requested_size, balance, fov_scale);
561
562 // Collected with these CMake flags: -DWITH_IPP=OFF -DCV_ENABLE_INTRINSICS=OFF -DCV_DISABLE_OPTIMIZATION=ON -DCMAKE_BUILD_TYPE=Debug
563 cv::Matx33d R1_ref(
564 0.9992853269091279, 0.03779164101000276, -0.0007920188690205426,
565 -0.03778569762983931, 0.9992646472015868, 0.006511981857667881,
566 0.001037534936357442, -0.006477400933964018, 0.9999784831677112
567 );
568 cv::Matx33d R2_ref(
569 0.9994868963898833, -0.03197579751378937, -0.001868774538573449,
570 0.03196298186616116, 0.9994677442608699, -0.0065265589947392,
571 0.002076471801477729, 0.006463478587068991, 0.9999769555891836
572 );
573 cv::Matx34d P1_ref(
574 420.8551870450913, 0, 586.501617798451, 0,
575 0, 420.8551870450913, 374.7667511986098, 0,
576 0, 0, 1, 0
577 );
578 cv::Matx34d P2_ref(
579 420.8551870450913, 0, 586.501617798451, -41.77758076597302,
580 0, 420.8551870450913, 374.7667511986098, 0,
581 0, 0, 1, 0
582 );
583 cv::Matx44d Q_ref(
584 1, 0, 0, -586.501617798451,
585 0, 1, 0, -374.7667511986098,
586 0, 0, 0, 420.8551870450913,
587 0, 0, 10.07370889670733, -0
588 );
589
590 const double eps = 1e-10;
591 EXPECT_MAT_NEAR(R1_ref, R1, eps);
592 EXPECT_MAT_NEAR(R2_ref, R2, eps);
593 EXPECT_MAT_NEAR(P1_ref, P1, eps);
594 EXPECT_MAT_NEAR(P2_ref, P2, eps);
595 EXPECT_MAT_NEAR(Q_ref, Q, eps);
596
597 if (::testing::Test::HasFailure())
598 {
599 std::cout << "Actual values are:" << std::endl
600 << "R1 =" << std::endl << R1 << std::endl
601 << "R2 =" << std::endl << R2 << std::endl
602 << "P1 =" << std::endl << P1 << std::endl
603 << "P2 =" << std::endl << P2 << std::endl
604 << "Q =" << std::endl << Q << std::endl;
605 }
606
607 if (cvtest::debugLevel == 0)
608 return;
609 // DEBUG code is below
610
611 cv::Mat lmapx, lmapy, rmapx, rmapy;
612 //rewrite for fisheye
613 cv::fisheye::initUndistortRectifyMap(K1, D1, R1, P1, requested_size, CV_32F, lmapx, lmapy);
614 cv::fisheye::initUndistortRectifyMap(K2, D2, R2, P2, requested_size, CV_32F, rmapx, rmapy);
615
616 cv::Mat l, r, lundist, rundist;
617 for (int i = 0; i < 34; ++i)
618 {
619 SCOPED_TRACE(cv::format("image %d", i));
620 l = imread(combine(folder, cv::format("left/stereo_pair_%03d.jpg", i)), cv::IMREAD_COLOR);
621 r = imread(combine(folder, cv::format("right/stereo_pair_%03d.jpg", i)), cv::IMREAD_COLOR);
622 ASSERT_FALSE(l.empty());
623 ASSERT_FALSE(r.empty());
624
625 int ndisp = 128;
626 cv::rectangle(l, cv::Rect(255, 0, 829, l.rows-1), cv::Scalar(0, 0, 255));
627 cv::rectangle(r, cv::Rect(255, 0, 829, l.rows-1), cv::Scalar(0, 0, 255));
628 cv::rectangle(r, cv::Rect(255-ndisp, 0, 829+ndisp ,l.rows-1), cv::Scalar(0, 0, 255));
629 cv::remap(l, lundist, lmapx, lmapy, cv::INTER_LINEAR);
630 cv::remap(r, rundist, rmapx, rmapy, cv::INTER_LINEAR);
631
632 for (int ii = 0; ii < lundist.rows; ii += 20)
633 {
634 cv::line(lundist, cv::Point(0, ii), cv::Point(lundist.cols, ii), cv::Scalar(0, 255, 0));
635 cv::line(rundist, cv::Point(0, ii), cv::Point(lundist.cols, ii), cv::Scalar(0, 255, 0));
636 }
637
638 cv::Mat rectification;
639 merge4(l, r, lundist, rundist, rectification);
640
641 cv::imwrite(cv::format("fisheye_rectification_AB_%03d.png", i), rectification);
642 }
643 }
644
TEST_F(fisheyeTest,stereoCalibrate)645 TEST_F(fisheyeTest, stereoCalibrate)
646 {
647 const int n_images = 34;
648
649 const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY");
650
651 std::vector<std::vector<cv::Point2d> > leftPoints(n_images);
652 std::vector<std::vector<cv::Point2d> > rightPoints(n_images);
653 std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
654
655 cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
656 CV_Assert(fs_left.isOpened());
657 for(int i = 0; i < n_images; ++i)
658 fs_left[cv::format("image_%d", i )] >> leftPoints[i];
659 fs_left.release();
660
661 cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ);
662 CV_Assert(fs_right.isOpened());
663 for(int i = 0; i < n_images; ++i)
664 fs_right[cv::format("image_%d", i )] >> rightPoints[i];
665 fs_right.release();
666
667 cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
668 CV_Assert(fs_object.isOpened());
669 for(int i = 0; i < n_images; ++i)
670 fs_object[cv::format("image_%d", i )] >> objectPoints[i];
671 fs_object.release();
672
673 cv::Matx33d K1, K2, theR;
674 cv::Vec3d theT;
675 cv::Vec4d D1, D2;
676
677 int flag = 0;
678 flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
679 flag |= cv::fisheye::CALIB_CHECK_COND;
680 flag |= cv::fisheye::CALIB_FIX_SKEW;
681
682 cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints,
683 K1, D1, K2, D2, imageSize, theR, theT, flag,
684 cv::TermCriteria(3, 12, 0));
685
686 cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523,
687 -0.06956823121068059, 0.9975601387249519, 0.005833595226966235,
688 -0.006071257768382089, -0.006271040135405457, 0.9999619062167968);
689 cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699);
690 cv::Matx33d K1_correct (561.195925927249, 0, 621.282400272412,
691 0, 562.849402029712, 380.555455380889,
692 0, 0, 1);
693
694 cv::Matx33d K2_correct (560.395452535348, 0, 678.971652040359,
695 0, 561.90171021422, 380.401340535339,
696 0, 0, 1);
697
698 cv::Vec4d D1_correct (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771);
699 cv::Vec4d D2_correct (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222);
700
701 EXPECT_MAT_NEAR(theR, R_correct, 1e-10);
702 EXPECT_MAT_NEAR(theT, T_correct, 1e-10);
703
704 EXPECT_MAT_NEAR(K1, K1_correct, 1e-10);
705 EXPECT_MAT_NEAR(K2, K2_correct, 1e-10);
706
707 EXPECT_MAT_NEAR(D1, D1_correct, 1e-10);
708 EXPECT_MAT_NEAR(D2, D2_correct, 1e-10);
709
710 }
711
TEST_F(fisheyeTest,stereoCalibrateFixIntrinsic)712 TEST_F(fisheyeTest, stereoCalibrateFixIntrinsic)
713 {
714 const int n_images = 34;
715
716 const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY");
717
718 std::vector<std::vector<cv::Point2d> > leftPoints(n_images);
719 std::vector<std::vector<cv::Point2d> > rightPoints(n_images);
720 std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
721
722 cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
723 CV_Assert(fs_left.isOpened());
724 for(int i = 0; i < n_images; ++i)
725 fs_left[cv::format("image_%d", i )] >> leftPoints[i];
726 fs_left.release();
727
728 cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ);
729 CV_Assert(fs_right.isOpened());
730 for(int i = 0; i < n_images; ++i)
731 fs_right[cv::format("image_%d", i )] >> rightPoints[i];
732 fs_right.release();
733
734 cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
735 CV_Assert(fs_object.isOpened());
736 for(int i = 0; i < n_images; ++i)
737 fs_object[cv::format("image_%d", i )] >> objectPoints[i];
738 fs_object.release();
739
740 cv::Matx33d theR;
741 cv::Vec3d theT;
742
743 int flag = 0;
744 flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
745 flag |= cv::fisheye::CALIB_CHECK_COND;
746 flag |= cv::fisheye::CALIB_FIX_SKEW;
747 flag |= cv::fisheye::CALIB_FIX_INTRINSIC;
748
749 cv::Matx33d K1 (561.195925927249, 0, 621.282400272412,
750 0, 562.849402029712, 380.555455380889,
751 0, 0, 1);
752
753 cv::Matx33d K2 (560.395452535348, 0, 678.971652040359,
754 0, 561.90171021422, 380.401340535339,
755 0, 0, 1);
756
757 cv::Vec4d D1 (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771);
758 cv::Vec4d D2 (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222);
759
760 cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints,
761 K1, D1, K2, D2, imageSize, theR, theT, flag,
762 cv::TermCriteria(3, 12, 0));
763
764 cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523,
765 -0.06956823121068059, 0.9975601387249519, 0.005833595226966235,
766 -0.006071257768382089, -0.006271040135405457, 0.9999619062167968);
767 cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699);
768
769
770 EXPECT_MAT_NEAR(theR, R_correct, 1e-10);
771 EXPECT_MAT_NEAR(theT, T_correct, 1e-10);
772 }
773
TEST_F(fisheyeTest,CalibrationWithDifferentPointsNumber)774 TEST_F(fisheyeTest, CalibrationWithDifferentPointsNumber)
775 {
776 const int n_images = 2;
777
778 std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
779 std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
780
781 std::vector<cv::Point2d> imgPoints1(10);
782 std::vector<cv::Point2d> imgPoints2(15);
783
784 std::vector<cv::Point3d> objectPoints1(imgPoints1.size());
785 std::vector<cv::Point3d> objectPoints2(imgPoints2.size());
786
787 for (size_t i = 0; i < imgPoints1.size(); i++)
788 {
789 imgPoints1[i] = cv::Point2d((double)i, (double)i);
790 objectPoints1[i] = cv::Point3d((double)i, (double)i, 10.0);
791 }
792
793 for (size_t i = 0; i < imgPoints2.size(); i++)
794 {
795 imgPoints2[i] = cv::Point2d(i + 0.5, i + 0.5);
796 objectPoints2[i] = cv::Point3d(i + 0.5, i + 0.5, 10.0);
797 }
798
799 imagePoints[0] = imgPoints1;
800 imagePoints[1] = imgPoints2;
801 objectPoints[0] = objectPoints1;
802 objectPoints[1] = objectPoints2;
803
804 cv::Matx33d theK = cv::Matx33d::eye();
805 cv::Vec4d theD;
806
807 int flag = 0;
808 flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
809 flag |= cv::fisheye::CALIB_USE_INTRINSIC_GUESS;
810 flag |= cv::fisheye::CALIB_FIX_SKEW;
811
812 cv::fisheye::calibrate(objectPoints, imagePoints, cv::Size(100, 100), theK, theD,
813 cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6));
814 }
815
TEST_F(fisheyeTest,estimateNewCameraMatrixForUndistortRectify)816 TEST_F(fisheyeTest, estimateNewCameraMatrixForUndistortRectify)
817 {
818 cv::Size size(1920, 1080);
819
820 cv::Mat K_fullhd(3, 3, cv::DataType<double>::type);
821 K_fullhd.at<double>(0, 0) = 600.44477382;
822 K_fullhd.at<double>(0, 1) = 0.0;
823 K_fullhd.at<double>(0, 2) = 992.06425788;
824
825 K_fullhd.at<double>(1, 0) = 0.0;
826 K_fullhd.at<double>(1, 1) = 578.99298055;
827 K_fullhd.at<double>(1, 2) = 549.26826242;
828
829 K_fullhd.at<double>(2, 0) = 0.0;
830 K_fullhd.at<double>(2, 1) = 0.0;
831 K_fullhd.at<double>(2, 2) = 1.0;
832
833 cv::Mat K_new_truth(3, 3, cv::DataType<double>::type);
834
835 K_new_truth.at<double>(0, 0) = 387.4809086880343;
836 K_new_truth.at<double>(0, 1) = 0.0;
837 K_new_truth.at<double>(0, 2) = 1036.669802754649;
838
839 K_new_truth.at<double>(1, 0) = 0.0;
840 K_new_truth.at<double>(1, 1) = 373.6375700303157;
841 K_new_truth.at<double>(1, 2) = 538.8373261247601;
842
843 K_new_truth.at<double>(2, 0) = 0.0;
844 K_new_truth.at<double>(2, 1) = 0.0;
845 K_new_truth.at<double>(2, 2) = 1.0;
846
847 cv::Mat D_fullhd(4, 1, cv::DataType<double>::type);
848 D_fullhd.at<double>(0, 0) = -0.05090103223466704;
849 D_fullhd.at<double>(1, 0) = 0.030944413642173308;
850 D_fullhd.at<double>(2, 0) = -0.021509225493198905;
851 D_fullhd.at<double>(3, 0) = 0.0043378096628297145;
852 cv::Mat E = cv::Mat::eye(3, 3, cv::DataType<double>::type);
853
854 cv::Mat K_new(3, 3, cv::DataType<double>::type);
855
856 cv::fisheye::estimateNewCameraMatrixForUndistortRectify(K_fullhd, D_fullhd, size, E, K_new, 0.0, size);
857
858 EXPECT_MAT_NEAR(K_new, K_new_truth, 1e-6);
859 }
860
861 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
862 /// fisheyeTest::
863
864 const cv::Size fisheyeTest::imageSize(1280, 800);
865
866 const cv::Matx33d fisheyeTest::K(558.478087865323, 0, 620.458515360843,
867 0, 560.506767351568, 381.939424848348,
868 0, 0, 1);
869
870 const cv::Vec4d fisheyeTest::D(-0.0014613319981768, -0.00329861110580401, 0.00605760088590183, -0.00374209380722371);
871
872
873 const cv::Matx33d fisheyeTest::R ( 9.9756700084424932e-01, 6.9698277640183867e-02, 1.4929569991321144e-03,
874 -6.9711825162322980e-02, 9.9748249845531767e-01, 1.2997180766418455e-02,
875 -5.8331736398316541e-04,-1.3069635393884985e-02, 9.9991441852366736e-01);
876
877 const cv::Vec3d fisheyeTest::T(-9.9217369356044638e-02, 3.1741831972356663e-03, 1.8551007952921010e-04);
878
combine(const std::string & _item1,const std::string & _item2)879 std::string fisheyeTest::combine(const std::string& _item1, const std::string& _item2)
880 {
881 std::string item1 = _item1, item2 = _item2;
882 std::replace(item1.begin(), item1.end(), '\\', '/');
883 std::replace(item2.begin(), item2.end(), '\\', '/');
884
885 if (item1.empty())
886 return item2;
887
888 if (item2.empty())
889 return item1;
890
891 char last = item1[item1.size()-1];
892 return item1 + (last != '/' ? "/" : "") + item2;
893 }
894
merge4(const cv::Mat & tl,const cv::Mat & tr,const cv::Mat & bl,const cv::Mat & br,cv::Mat & merged)895 void fisheyeTest::merge4(const cv::Mat& tl, const cv::Mat& tr, const cv::Mat& bl, const cv::Mat& br, cv::Mat& merged)
896 {
897 int type = tl.type();
898 cv::Size sz = tl.size();
899 ASSERT_EQ(type, tr.type()); ASSERT_EQ(type, bl.type()); ASSERT_EQ(type, br.type());
900 ASSERT_EQ(sz.width, tr.cols); ASSERT_EQ(sz.width, bl.cols); ASSERT_EQ(sz.width, br.cols);
901 ASSERT_EQ(sz.height, tr.rows); ASSERT_EQ(sz.height, bl.rows); ASSERT_EQ(sz.height, br.rows);
902
903 merged.create(cv::Size(sz.width * 2, sz.height * 2), type);
904 tl.copyTo(merged(cv::Rect(0, 0, sz.width, sz.height)));
905 tr.copyTo(merged(cv::Rect(sz.width, 0, sz.width, sz.height)));
906 bl.copyTo(merged(cv::Rect(0, sz.height, sz.width, sz.height)));
907 br.copyTo(merged(cv::Rect(sz.width, sz.height, sz.width, sz.height)));
908 }
909
910 }} // namespace
911
