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>
45 #include "../src/fisheye.hpp"
46
47 class fisheyeTest : public ::testing::Test {
48
49 protected:
50 const static cv::Size imageSize;
51 const static cv::Matx33d K;
52 const static cv::Vec4d D;
53 const static cv::Matx33d R;
54 const static cv::Vec3d T;
55 std::string datasets_repository_path;
56
SetUp()57 virtual void SetUp() {
58 datasets_repository_path = combine(cvtest::TS::ptr()->get_data_path(), "cv/cameracalibration/fisheye");
59 }
60
61 protected:
62 std::string combine(const std::string& _item1, const std::string& _item2);
63 cv::Mat mergeRectification(const cv::Mat& l, const cv::Mat& r);
64 };
65
66 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
67 /// TESTS::
68
TEST_F(fisheyeTest,projectPoints)69 TEST_F(fisheyeTest, projectPoints)
70 {
71 double cols = this->imageSize.width,
72 rows = this->imageSize.height;
73
74 const int N = 20;
75 cv::Mat distorted0(1, N*N, CV_64FC2), undist1, undist2, distorted1, distorted2;
76 undist2.create(distorted0.size(), CV_MAKETYPE(distorted0.depth(), 3));
77 cv::Vec2d* pts = distorted0.ptr<cv::Vec2d>();
78
79 cv::Vec2d c(this->K(0, 2), this->K(1, 2));
80 for(int y = 0, k = 0; y < N; ++y)
81 for(int x = 0; x < N; ++x)
82 {
83 cv::Vec2d point(x*cols/(N-1.f), y*rows/(N-1.f));
84 pts[k++] = (point - c) * 0.85 + c;
85 }
86
87 cv::fisheye::undistortPoints(distorted0, undist1, this->K, this->D);
88
89 cv::Vec2d* u1 = undist1.ptr<cv::Vec2d>();
90 cv::Vec3d* u2 = undist2.ptr<cv::Vec3d>();
91 for(int i = 0; i < (int)distorted0.total(); ++i)
92 u2[i] = cv::Vec3d(u1[i][0], u1[i][1], 1.0);
93
94 cv::fisheye::distortPoints(undist1, distorted1, this->K, this->D);
95 cv::fisheye::projectPoints(undist2, distorted2, cv::Vec3d::all(0), cv::Vec3d::all(0), this->K, this->D);
96
97 EXPECT_MAT_NEAR(distorted0, distorted1, 1e-10);
98 EXPECT_MAT_NEAR(distorted0, distorted2, 1e-10);
99 }
100
TEST_F(fisheyeTest,DISABLED_undistortImage)101 TEST_F(fisheyeTest, DISABLED_undistortImage)
102 {
103 cv::Matx33d K = this->K;
104 cv::Mat D = cv::Mat(this->D);
105 std::string file = combine(datasets_repository_path, "/calib-3_stereo_from_JY/left/stereo_pair_014.jpg");
106 cv::Matx33d newK = K;
107 cv::Mat distorted = cv::imread(file), undistorted;
108 {
109 newK(0, 0) = 100;
110 newK(1, 1) = 100;
111 cv::fisheye::undistortImage(distorted, undistorted, K, D, newK);
112 cv::Mat correct = cv::imread(combine(datasets_repository_path, "new_f_100.png"));
113 if (correct.empty())
114 CV_Assert(cv::imwrite(combine(datasets_repository_path, "new_f_100.png"), undistorted));
115 else
116 EXPECT_MAT_NEAR(correct, undistorted, 1e-10);
117 }
118 {
119 double balance = 1.0;
120 cv::fisheye::estimateNewCameraMatrixForUndistortRectify(K, D, distorted.size(), cv::noArray(), newK, balance);
121 cv::fisheye::undistortImage(distorted, undistorted, K, D, newK);
122 cv::Mat correct = cv::imread(combine(datasets_repository_path, "balance_1.0.png"));
123 if (correct.empty())
124 CV_Assert(cv::imwrite(combine(datasets_repository_path, "balance_1.0.png"), undistorted));
125 else
126 EXPECT_MAT_NEAR(correct, undistorted, 1e-10);
127 }
128
129 {
130 double balance = 0.0;
131 cv::fisheye::estimateNewCameraMatrixForUndistortRectify(K, D, distorted.size(), cv::noArray(), newK, balance);
132 cv::fisheye::undistortImage(distorted, undistorted, K, D, newK);
133 cv::Mat correct = cv::imread(combine(datasets_repository_path, "balance_0.0.png"));
134 if (correct.empty())
135 CV_Assert(cv::imwrite(combine(datasets_repository_path, "balance_0.0.png"), undistorted));
136 else
137 EXPECT_MAT_NEAR(correct, undistorted, 1e-10);
138 }
139 }
140
TEST_F(fisheyeTest,jacobians)141 TEST_F(fisheyeTest, jacobians)
142 {
143 int n = 10;
144 cv::Mat X(1, n, CV_64FC3);
145 cv::Mat om(3, 1, CV_64F), T(3, 1, CV_64F);
146 cv::Mat f(2, 1, CV_64F), c(2, 1, CV_64F);
147 cv::Mat k(4, 1, CV_64F);
148 double alpha;
149
150 cv::RNG r;
151
152 r.fill(X, cv::RNG::NORMAL, 2, 1);
153 X = cv::abs(X) * 10;
154
155 r.fill(om, cv::RNG::NORMAL, 0, 1);
156 om = cv::abs(om);
157
158 r.fill(T, cv::RNG::NORMAL, 0, 1);
159 T = cv::abs(T); T.at<double>(2) = 4; T *= 10;
160
161 r.fill(f, cv::RNG::NORMAL, 0, 1);
162 f = cv::abs(f) * 1000;
163
164 r.fill(c, cv::RNG::NORMAL, 0, 1);
165 c = cv::abs(c) * 1000;
166
167 r.fill(k, cv::RNG::NORMAL, 0, 1);
168 k*= 0.5;
169
170 alpha = 0.01*r.gaussian(1);
171
172 cv::Mat x1, x2, xpred;
173 cv::Matx33d K(f.at<double>(0), alpha * f.at<double>(0), c.at<double>(0),
174 0, f.at<double>(1), c.at<double>(1),
175 0, 0, 1);
176
177 cv::Mat jacobians;
178 cv::fisheye::projectPoints(X, x1, om, T, K, k, alpha, jacobians);
179
180 //test on T:
181 cv::Mat dT(3, 1, CV_64FC1);
182 r.fill(dT, cv::RNG::NORMAL, 0, 1);
183 dT *= 1e-9*cv::norm(T);
184 cv::Mat T2 = T + dT;
185 cv::fisheye::projectPoints(X, x2, om, T2, K, k, alpha, cv::noArray());
186 xpred = x1 + cv::Mat(jacobians.colRange(11,14) * dT).reshape(2, 1);
187 CV_Assert (cv::norm(x2 - xpred) < 1e-10);
188
189 //test on om:
190 cv::Mat dom(3, 1, CV_64FC1);
191 r.fill(dom, cv::RNG::NORMAL, 0, 1);
192 dom *= 1e-9*cv::norm(om);
193 cv::Mat om2 = om + dom;
194 cv::fisheye::projectPoints(X, x2, om2, T, K, k, alpha, cv::noArray());
195 xpred = x1 + cv::Mat(jacobians.colRange(8,11) * dom).reshape(2, 1);
196 CV_Assert (cv::norm(x2 - xpred) < 1e-10);
197
198 //test on f:
199 cv::Mat df(2, 1, CV_64FC1);
200 r.fill(df, cv::RNG::NORMAL, 0, 1);
201 df *= 1e-9*cv::norm(f);
202 cv::Matx33d K2 = K + cv::Matx33d(df.at<double>(0), df.at<double>(0) * alpha, 0, 0, df.at<double>(1), 0, 0, 0, 0);
203 cv::fisheye::projectPoints(X, x2, om, T, K2, k, alpha, cv::noArray());
204 xpred = x1 + cv::Mat(jacobians.colRange(0,2) * df).reshape(2, 1);
205 CV_Assert (cv::norm(x2 - xpred) < 1e-10);
206
207 //test on c:
208 cv::Mat dc(2, 1, CV_64FC1);
209 r.fill(dc, cv::RNG::NORMAL, 0, 1);
210 dc *= 1e-9*cv::norm(c);
211 K2 = K + cv::Matx33d(0, 0, dc.at<double>(0), 0, 0, dc.at<double>(1), 0, 0, 0);
212 cv::fisheye::projectPoints(X, x2, om, T, K2, k, alpha, cv::noArray());
213 xpred = x1 + cv::Mat(jacobians.colRange(2,4) * dc).reshape(2, 1);
214 CV_Assert (cv::norm(x2 - xpred) < 1e-10);
215
216 //test on k:
217 cv::Mat dk(4, 1, CV_64FC1);
218 r.fill(dk, cv::RNG::NORMAL, 0, 1);
219 dk *= 1e-9*cv::norm(k);
220 cv::Mat k2 = k + dk;
221 cv::fisheye::projectPoints(X, x2, om, T, K, k2, alpha, cv::noArray());
222 xpred = x1 + cv::Mat(jacobians.colRange(4,8) * dk).reshape(2, 1);
223 CV_Assert (cv::norm(x2 - xpred) < 1e-10);
224
225 //test on alpha:
226 cv::Mat dalpha(1, 1, CV_64FC1);
227 r.fill(dalpha, cv::RNG::NORMAL, 0, 1);
228 dalpha *= 1e-9*cv::norm(f);
229 double alpha2 = alpha + dalpha.at<double>(0);
230 K2 = K + cv::Matx33d(0, f.at<double>(0) * dalpha.at<double>(0), 0, 0, 0, 0, 0, 0, 0);
231 cv::fisheye::projectPoints(X, x2, om, T, K, k, alpha2, cv::noArray());
232 xpred = x1 + cv::Mat(jacobians.col(14) * dalpha).reshape(2, 1);
233 CV_Assert (cv::norm(x2 - xpred) < 1e-10);
234 }
235
TEST_F(fisheyeTest,Calibration)236 TEST_F(fisheyeTest, Calibration)
237 {
238 const int n_images = 34;
239
240 std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
241 std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
242
243 const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
244 cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
245 CV_Assert(fs_left.isOpened());
246 for(int i = 0; i < n_images; ++i)
247 fs_left[cv::format("image_%d", i )] >> imagePoints[i];
248 fs_left.release();
249
250 cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
251 CV_Assert(fs_object.isOpened());
252 for(int i = 0; i < n_images; ++i)
253 fs_object[cv::format("image_%d", i )] >> objectPoints[i];
254 fs_object.release();
255
256 int flag = 0;
257 flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
258 flag |= cv::fisheye::CALIB_CHECK_COND;
259 flag |= cv::fisheye::CALIB_FIX_SKEW;
260
261 cv::Matx33d K;
262 cv::Vec4d D;
263
264 cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, K, D,
265 cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6));
266
267 EXPECT_MAT_NEAR(K, this->K, 1e-10);
268 EXPECT_MAT_NEAR(D, this->D, 1e-10);
269 }
270
TEST_F(fisheyeTest,Homography)271 TEST_F(fisheyeTest, Homography)
272 {
273 const int n_images = 1;
274
275 std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
276 std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
277
278 const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
279 cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
280 CV_Assert(fs_left.isOpened());
281 for(int i = 0; i < n_images; ++i)
282 fs_left[cv::format("image_%d", i )] >> imagePoints[i];
283 fs_left.release();
284
285 cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
286 CV_Assert(fs_object.isOpened());
287 for(int i = 0; i < n_images; ++i)
288 fs_object[cv::format("image_%d", i )] >> objectPoints[i];
289 fs_object.release();
290
291 cv::internal::IntrinsicParams param;
292 param.Init(cv::Vec2d(cv::max(imageSize.width, imageSize.height) / CV_PI, cv::max(imageSize.width, imageSize.height) / CV_PI),
293 cv::Vec2d(imageSize.width / 2.0 - 0.5, imageSize.height / 2.0 - 0.5));
294
295 cv::Mat _imagePoints (imagePoints[0]);
296 cv::Mat _objectPoints(objectPoints[0]);
297
298 cv::Mat imagePointsNormalized = NormalizePixels(_imagePoints, param).reshape(1).t();
299 _objectPoints = _objectPoints.reshape(1).t();
300 cv::Mat objectPointsMean, covObjectPoints;
301
302 int Np = imagePointsNormalized.cols;
303 cv::calcCovarMatrix(_objectPoints, covObjectPoints, objectPointsMean, cv::COVAR_NORMAL | cv::COVAR_COLS);
304 cv::SVD svd(covObjectPoints);
305 cv::Mat R(svd.vt);
306
307 if (cv::norm(R(cv::Rect(2, 0, 1, 2))) < 1e-6)
308 R = cv::Mat::eye(3,3, CV_64FC1);
309 if (cv::determinant(R) < 0)
310 R = -R;
311
312 cv::Mat T = -R * objectPointsMean;
313 cv::Mat X_new = R * _objectPoints + T * cv::Mat::ones(1, Np, CV_64FC1);
314 cv::Mat H = cv::internal::ComputeHomography(imagePointsNormalized, X_new.rowRange(0, 2));
315
316 cv::Mat M = cv::Mat::ones(3, X_new.cols, CV_64FC1);
317 X_new.rowRange(0, 2).copyTo(M.rowRange(0, 2));
318 cv::Mat mrep = H * M;
319
320 cv::divide(mrep, cv::Mat::ones(3,1, CV_64FC1) * mrep.row(2).clone(), mrep);
321
322 cv::Mat merr = (mrep.rowRange(0, 2) - imagePointsNormalized).t();
323
324 cv::Vec2d std_err;
325 cv::meanStdDev(merr.reshape(2), cv::noArray(), std_err);
326 std_err *= sqrt((double)merr.reshape(2).total() / (merr.reshape(2).total() - 1));
327
328 cv::Vec2d correct_std_err(0.00516740156010384, 0.00644205331553901);
329 EXPECT_MAT_NEAR(std_err, correct_std_err, 1e-12);
330 }
331
TEST_F(fisheyeTest,EtimateUncertainties)332 TEST_F(fisheyeTest, EtimateUncertainties)
333 {
334 const int n_images = 34;
335
336 std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
337 std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
338
339 const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
340 cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
341 CV_Assert(fs_left.isOpened());
342 for(int i = 0; i < n_images; ++i)
343 fs_left[cv::format("image_%d", i )] >> imagePoints[i];
344 fs_left.release();
345
346 cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
347 CV_Assert(fs_object.isOpened());
348 for(int i = 0; i < n_images; ++i)
349 fs_object[cv::format("image_%d", i )] >> objectPoints[i];
350 fs_object.release();
351
352 int flag = 0;
353 flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
354 flag |= cv::fisheye::CALIB_CHECK_COND;
355 flag |= cv::fisheye::CALIB_FIX_SKEW;
356
357 cv::Matx33d K;
358 cv::Vec4d D;
359 std::vector<cv::Vec3d> rvec;
360 std::vector<cv::Vec3d> tvec;
361
362 cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, K, D,
363 rvec, tvec, flag, cv::TermCriteria(3, 20, 1e-6));
364
365 cv::internal::IntrinsicParams param, errors;
366 cv::Vec2d err_std;
367 double thresh_cond = 1e6;
368 int check_cond = 1;
369 param.Init(cv::Vec2d(K(0,0), K(1,1)), cv::Vec2d(K(0,2), K(1, 2)), D);
370 param.isEstimate = std::vector<int>(9, 1);
371 param.isEstimate[4] = 0;
372
373 errors.isEstimate = param.isEstimate;
374
375 double rms;
376
377 cv::internal::EstimateUncertainties(objectPoints, imagePoints, param, rvec, tvec,
378 errors, err_std, thresh_cond, check_cond, rms);
379
380 EXPECT_MAT_NEAR(errors.f, cv::Vec2d(1.29837104202046, 1.31565641071524), 1e-10);
381 EXPECT_MAT_NEAR(errors.c, cv::Vec2d(0.890439368129246, 0.816096854937896), 1e-10);
382 EXPECT_MAT_NEAR(errors.k, cv::Vec4d(0.00516248605191506, 0.0168181467500934, 0.0213118690274604, 0.00916010877545648), 1e-10);
383 EXPECT_MAT_NEAR(err_std, cv::Vec2d(0.187475975266883, 0.185678953263995), 1e-10);
384 CV_Assert(fabs(rms - 0.263782587133546) < 1e-10);
385 CV_Assert(errors.alpha == 0);
386 }
387
388 #ifdef HAVE_TEGRA_OPTIMIZATION
389 // not passing accuracy constrains
TEST_F(fisheyeTest,DISABLED_rectify)390 TEST_F(fisheyeTest, DISABLED_rectify)
391 #else
392 TEST_F(fisheyeTest, rectify)
393 #endif
394 {
395 const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
396
397 cv::Size calibration_size = this->imageSize, requested_size = calibration_size;
398 cv::Matx33d K1 = this->K, K2 = K1;
399 cv::Mat D1 = cv::Mat(this->D), D2 = D1;
400
401 cv::Vec3d T = this->T;
402 cv::Matx33d R = this->R;
403
404 double balance = 0.0, fov_scale = 1.1;
405 cv::Mat R1, R2, P1, P2, Q;
406 cv::fisheye::stereoRectify(K1, D1, K2, D2, calibration_size, R, T, R1, R2, P1, P2, Q,
407 cv::CALIB_ZERO_DISPARITY, requested_size, balance, fov_scale);
408
409 cv::Mat lmapx, lmapy, rmapx, rmapy;
410 //rewrite for fisheye
411 cv::fisheye::initUndistortRectifyMap(K1, D1, R1, P1, requested_size, CV_32F, lmapx, lmapy);
412 cv::fisheye::initUndistortRectifyMap(K2, D2, R2, P2, requested_size, CV_32F, rmapx, rmapy);
413
414 cv::Mat l, r, lundist, rundist;
415 cv::VideoCapture lcap(combine(folder, "left/stereo_pair_%03d.jpg")),
416 rcap(combine(folder, "right/stereo_pair_%03d.jpg"));
417
418 for(int i = 0;; ++i)
419 {
420 lcap >> l; rcap >> r;
421 if (l.empty() || r.empty())
422 break;
423
424 int ndisp = 128;
425 cv::rectangle(l, cv::Rect(255, 0, 829, l.rows-1), cv::Scalar(0, 0, 255));
426 cv::rectangle(r, cv::Rect(255, 0, 829, l.rows-1), cv::Scalar(0, 0, 255));
427 cv::rectangle(r, cv::Rect(255-ndisp, 0, 829+ndisp ,l.rows-1), cv::Scalar(0, 0, 255));
428 cv::remap(l, lundist, lmapx, lmapy, cv::INTER_LINEAR);
429 cv::remap(r, rundist, rmapx, rmapy, cv::INTER_LINEAR);
430
431 cv::Mat rectification = mergeRectification(lundist, rundist);
432
433 cv::Mat correct = cv::imread(combine(datasets_repository_path, cv::format("rectification_AB_%03d.png", i)));
434
435 if (correct.empty())
436 cv::imwrite(combine(datasets_repository_path, cv::format("rectification_AB_%03d.png", i)), rectification);
437 else
438 EXPECT_MAT_NEAR(correct, rectification, 1e-10);
439 }
440 }
441
TEST_F(fisheyeTest,stereoCalibrate)442 TEST_F(fisheyeTest, stereoCalibrate)
443 {
444 const int n_images = 34;
445
446 const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
447
448 std::vector<std::vector<cv::Point2d> > leftPoints(n_images);
449 std::vector<std::vector<cv::Point2d> > rightPoints(n_images);
450 std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
451
452 cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
453 CV_Assert(fs_left.isOpened());
454 for(int i = 0; i < n_images; ++i)
455 fs_left[cv::format("image_%d", i )] >> leftPoints[i];
456 fs_left.release();
457
458 cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ);
459 CV_Assert(fs_right.isOpened());
460 for(int i = 0; i < n_images; ++i)
461 fs_right[cv::format("image_%d", i )] >> rightPoints[i];
462 fs_right.release();
463
464 cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
465 CV_Assert(fs_object.isOpened());
466 for(int i = 0; i < n_images; ++i)
467 fs_object[cv::format("image_%d", i )] >> objectPoints[i];
468 fs_object.release();
469
470 cv::Matx33d K1, K2, R;
471 cv::Vec3d T;
472 cv::Vec4d D1, D2;
473
474 int flag = 0;
475 flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
476 flag |= cv::fisheye::CALIB_CHECK_COND;
477 flag |= cv::fisheye::CALIB_FIX_SKEW;
478 // flag |= cv::fisheye::CALIB_FIX_INTRINSIC;
479
480 cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints,
481 K1, D1, K2, D2, imageSize, R, T, flag,
482 cv::TermCriteria(3, 12, 0));
483
484 cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523,
485 -0.06956823121068059, 0.9975601387249519, 0.005833595226966235,
486 -0.006071257768382089, -0.006271040135405457, 0.9999619062167968);
487 cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699);
488 cv::Matx33d K1_correct (561.195925927249, 0, 621.282400272412,
489 0, 562.849402029712, 380.555455380889,
490 0, 0, 1);
491
492 cv::Matx33d K2_correct (560.395452535348, 0, 678.971652040359,
493 0, 561.90171021422, 380.401340535339,
494 0, 0, 1);
495
496 cv::Vec4d D1_correct (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771);
497 cv::Vec4d D2_correct (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222);
498
499 EXPECT_MAT_NEAR(R, R_correct, 1e-10);
500 EXPECT_MAT_NEAR(T, T_correct, 1e-10);
501
502 EXPECT_MAT_NEAR(K1, K1_correct, 1e-10);
503 EXPECT_MAT_NEAR(K2, K2_correct, 1e-10);
504
505 EXPECT_MAT_NEAR(D1, D1_correct, 1e-10);
506 EXPECT_MAT_NEAR(D2, D2_correct, 1e-10);
507
508 }
509
TEST_F(fisheyeTest,stereoCalibrateFixIntrinsic)510 TEST_F(fisheyeTest, stereoCalibrateFixIntrinsic)
511 {
512 const int n_images = 34;
513
514 const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
515
516 std::vector<std::vector<cv::Point2d> > leftPoints(n_images);
517 std::vector<std::vector<cv::Point2d> > rightPoints(n_images);
518 std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
519
520 cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
521 CV_Assert(fs_left.isOpened());
522 for(int i = 0; i < n_images; ++i)
523 fs_left[cv::format("image_%d", i )] >> leftPoints[i];
524 fs_left.release();
525
526 cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ);
527 CV_Assert(fs_right.isOpened());
528 for(int i = 0; i < n_images; ++i)
529 fs_right[cv::format("image_%d", i )] >> rightPoints[i];
530 fs_right.release();
531
532 cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
533 CV_Assert(fs_object.isOpened());
534 for(int i = 0; i < n_images; ++i)
535 fs_object[cv::format("image_%d", i )] >> objectPoints[i];
536 fs_object.release();
537
538 cv::Matx33d R;
539 cv::Vec3d T;
540
541 int flag = 0;
542 flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
543 flag |= cv::fisheye::CALIB_CHECK_COND;
544 flag |= cv::fisheye::CALIB_FIX_SKEW;
545 flag |= cv::fisheye::CALIB_FIX_INTRINSIC;
546
547 cv::Matx33d K1 (561.195925927249, 0, 621.282400272412,
548 0, 562.849402029712, 380.555455380889,
549 0, 0, 1);
550
551 cv::Matx33d K2 (560.395452535348, 0, 678.971652040359,
552 0, 561.90171021422, 380.401340535339,
553 0, 0, 1);
554
555 cv::Vec4d D1 (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771);
556 cv::Vec4d D2 (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222);
557
558 cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints,
559 K1, D1, K2, D2, imageSize, R, T, flag,
560 cv::TermCriteria(3, 12, 0));
561
562 cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523,
563 -0.06956823121068059, 0.9975601387249519, 0.005833595226966235,
564 -0.006071257768382089, -0.006271040135405457, 0.9999619062167968);
565 cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699);
566
567
568 EXPECT_MAT_NEAR(R, R_correct, 1e-10);
569 EXPECT_MAT_NEAR(T, T_correct, 1e-10);
570 }
571
572 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
573 /// fisheyeTest::
574
575 const cv::Size fisheyeTest::imageSize(1280, 800);
576
577 const cv::Matx33d fisheyeTest::K(558.478087865323, 0, 620.458515360843,
578 0, 560.506767351568, 381.939424848348,
579 0, 0, 1);
580
581 const cv::Vec4d fisheyeTest::D(-0.0014613319981768, -0.00329861110580401, 0.00605760088590183, -0.00374209380722371);
582
583 const cv::Matx33d fisheyeTest::R ( 9.9756700084424932e-01, 6.9698277640183867e-02, 1.4929569991321144e-03,
584 -6.9711825162322980e-02, 9.9748249845531767e-01, 1.2997180766418455e-02,
585 -5.8331736398316541e-04,-1.3069635393884985e-02, 9.9991441852366736e-01);
586
587 const cv::Vec3d fisheyeTest::T(-9.9217369356044638e-02, 3.1741831972356663e-03, 1.8551007952921010e-04);
588
combine(const std::string & _item1,const std::string & _item2)589 std::string fisheyeTest::combine(const std::string& _item1, const std::string& _item2)
590 {
591 std::string item1 = _item1, item2 = _item2;
592 std::replace(item1.begin(), item1.end(), '\\', '/');
593 std::replace(item2.begin(), item2.end(), '\\', '/');
594
595 if (item1.empty())
596 return item2;
597
598 if (item2.empty())
599 return item1;
600
601 char last = item1[item1.size()-1];
602 return item1 + (last != '/' ? "/" : "") + item2;
603 }
604
mergeRectification(const cv::Mat & l,const cv::Mat & r)605 cv::Mat fisheyeTest::mergeRectification(const cv::Mat& l, const cv::Mat& r)
606 {
607 CV_Assert(l.type() == r.type() && l.size() == r.size());
608 cv::Mat merged(l.rows, l.cols * 2, l.type());
609 cv::Mat lpart = merged.colRange(0, l.cols);
610 cv::Mat rpart = merged.colRange(l.cols, merged.cols);
611 l.copyTo(lpart);
612 r.copyTo(rpart);
613
614 for(int i = 0; i < l.rows; i+=20)
615 cv::line(merged, cv::Point(0, i), cv::Point(merged.cols, i), cv::Scalar(0, 255, 0));
616
617 return merged;
618 }
619
