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, 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 45 #include <string> 46 #include <limits> 47 #include <vector> 48 #include <iostream> 49 #include <sstream> 50 #include <iomanip> 51 52 #include "test_chessboardgenerator.hpp" 53 54 using namespace cv; 55 using namespace std; 56 57 //template<class T> ostream& operator<<(ostream& out, const Mat_<T>& mat) 58 //{ 59 // for(Mat_<T>::const_iterator pos = mat.begin(), end = mat.end(); pos != end; ++pos) 60 // out << *pos << " "; 61 // return out; 62 //} 63 //ostream& operator<<(ostream& out, const Mat& mat) { return out << Mat_<double>(mat); } 64 65 Mat calcRvec(const vector<Point3f>& points, const Size& cornerSize) 66 { 67 Point3f p00 = points[0]; 68 Point3f p10 = points[1]; 69 Point3f p01 = points[cornerSize.width]; 70 71 Vec3d ex(p10.x - p00.x, p10.y - p00.y, p10.z - p00.z); 72 Vec3d ey(p01.x - p00.x, p01.y - p00.y, p01.z - p00.z); 73 Vec3d ez = ex.cross(ey); 74 75 Mat rot(3, 3, CV_64F); 76 *rot.ptr<Vec3d>(0) = ex; 77 *rot.ptr<Vec3d>(1) = ey; 78 *rot.ptr<Vec3d>(2) = ez * (1.0/norm(ez)); 79 80 Mat res; 81 Rodrigues(rot.t(), res); 82 return res.reshape(1, 1); 83 } 84 85 class CV_CalibrateCameraArtificialTest : public cvtest::BaseTest 86 { 87 public: 88 CV_CalibrateCameraArtificialTest() : 89 r(0) 90 { 91 } 92 ~CV_CalibrateCameraArtificialTest() {} 93 protected: 94 int r; 95 96 const static int JUST_FIND_CORNERS = 0; 97 const static int USE_CORNERS_SUBPIX = 1; 98 const static int USE_4QUAD_CORNERS = 2; 99 const static int ARTIFICIAL_CORNERS = 4; 100 101 102 bool checkErr(double a, double a0, double eps, double delta) 103 { 104 return fabs(a - a0) > eps * (fabs(a0) + delta); 105 } 106 107 void compareCameraMatrs(const Mat_<double>& camMat, const Mat& camMat_est) 108 { 109 if ( camMat_est.at<double>(0, 1) != 0 || camMat_est.at<double>(1, 0) != 0 || 110 camMat_est.at<double>(2, 0) != 0 || camMat_est.at<double>(2, 1) != 0 || 111 camMat_est.at<double>(2, 2) != 1) 112 { 113 ts->printf( cvtest::TS::LOG, "Bad shape of camera matrix returned \n"); 114 ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); 115 } 116 117 double fx_e = camMat_est.at<double>(0, 0), fy_e = camMat_est.at<double>(1, 1); 118 double cx_e = camMat_est.at<double>(0, 2), cy_e = camMat_est.at<double>(1, 2); 119 120 double fx = camMat(0, 0), fy = camMat(1, 1), cx = camMat(0, 2), cy = camMat(1, 2); 121 122 const double eps = 1e-2; 123 const double dlt = 1e-5; 124 125 bool fail = checkErr(fx_e, fx, eps, dlt) || checkErr(fy_e, fy, eps, dlt) || 126 checkErr(cx_e, cx, eps, dlt) || checkErr(cy_e, cy, eps, dlt); 127 128 if (fail) 129 { 130 ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); 131 } 132 ts->printf( cvtest::TS::LOG, "%d) Expected [Fx Fy Cx Cy] = [%.3f %.3f %.3f %.3f]\n", r, fx, fy, cx, cy); 133 ts->printf( cvtest::TS::LOG, "%d) Estimated [Fx Fy Cx Cy] = [%.3f %.3f %.3f %.3f]\n", r, fx_e, fy_e, cx_e, cy_e); 134 } 135 136 void compareDistCoeffs(const Mat_<double>& distCoeffs, const Mat& distCoeffs_est) 137 { 138 const double *dt_e = distCoeffs_est.ptr<double>(); 139 140 double k1_e = dt_e[0], k2_e = dt_e[1], k3_e = dt_e[4]; 141 double p1_e = dt_e[2], p2_e = dt_e[3]; 142 143 double k1 = distCoeffs(0, 0), k2 = distCoeffs(0, 1), k3 = distCoeffs(0, 4); 144 double p1 = distCoeffs(0, 2), p2 = distCoeffs(0, 3); 145 146 const double eps = 5e-2; 147 const double dlt = 1e-3; 148 149 const double eps_k3 = 5; 150 const double dlt_k3 = 1e-3; 151 152 bool fail = checkErr(k1_e, k1, eps, dlt) || checkErr(k2_e, k2, eps, dlt) || checkErr(k3_e, k3, eps_k3, dlt_k3) || 153 checkErr(p1_e, p1, eps, dlt) || checkErr(p2_e, p2, eps, dlt); 154 155 if (fail) 156 { 157 // commented according to vp123's recomendation. TODO - improve accuaracy 158 //ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); ss 159 } 160 ts->printf( cvtest::TS::LOG, "%d) DistCoeff exp=(%.2f, %.2f, %.4f, %.4f %.2f)\n", r, k1, k2, p1, p2, k3); 161 ts->printf( cvtest::TS::LOG, "%d) DistCoeff est=(%.2f, %.2f, %.4f, %.4f %.2f)\n", r, k1_e, k2_e, p1_e, p2_e, k3_e); 162 ts->printf( cvtest::TS::LOG, "%d) AbsError = [%.5f %.5f %.5f %.5f %.5f]\n", r, fabs(k1-k1_e), fabs(k2-k2_e), fabs(p1-p1_e), fabs(p2-p2_e), fabs(k3-k3_e)); 163 } 164 165 void compareShiftVecs(const vector<Mat>& tvecs, const vector<Mat>& tvecs_est) 166 { 167 const double eps = 1e-2; 168 const double dlt = 1e-4; 169 170 int err_count = 0; 171 const int errMsgNum = 4; 172 for(size_t i = 0; i < tvecs.size(); ++i) 173 { 174 const Point3d& tvec = *tvecs[i].ptr<Point3d>(); 175 const Point3d& tvec_est = *tvecs_est[i].ptr<Point3d>(); 176 177 if (norm(tvec_est - tvec) > eps* (norm(tvec) + dlt)) 178 { 179 if (err_count++ < errMsgNum) 180 { 181 if (err_count == errMsgNum) 182 ts->printf( cvtest::TS::LOG, "%d) ...\n", r); 183 else 184 { 185 ts->printf( cvtest::TS::LOG, "%d) Bad accuracy in returned tvecs. Index = %d\n", r, i); 186 ts->printf( cvtest::TS::LOG, "%d) norm(tvec_est - tvec) = %f, norm(tvec_exp) = %f \n", r, norm(tvec_est - tvec), norm(tvec)); 187 } 188 } 189 ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); 190 } 191 } 192 } 193 194 void compareRotationVecs(const vector<Mat>& rvecs, const vector<Mat>& rvecs_est) 195 { 196 const double eps = 2e-2; 197 const double dlt = 1e-4; 198 199 Mat rmat, rmat_est; 200 int err_count = 0; 201 const int errMsgNum = 4; 202 for(size_t i = 0; i < rvecs.size(); ++i) 203 { 204 Rodrigues(rvecs[i], rmat); 205 Rodrigues(rvecs_est[i], rmat_est); 206 207 if (cvtest::norm(rmat_est, rmat, NORM_L2) > eps* (cvtest::norm(rmat, NORM_L2) + dlt)) 208 { 209 if (err_count++ < errMsgNum) 210 { 211 if (err_count == errMsgNum) 212 ts->printf( cvtest::TS::LOG, "%d) ...\n", r); 213 else 214 { 215 ts->printf( cvtest::TS::LOG, "%d) Bad accuracy in returned rvecs (rotation matrs). Index = %d\n", r, i); 216 ts->printf( cvtest::TS::LOG, "%d) norm(rot_mat_est - rot_mat_exp) = %f, norm(rot_mat_exp) = %f \n", r, 217 cvtest::norm(rmat_est, rmat, NORM_L2), cvtest::norm(rmat, NORM_L2)); 218 219 } 220 } 221 ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); 222 } 223 } 224 } 225 226 double reprojectErrorWithoutIntrinsics(const vector<Point3f>& cb3d, const vector<Mat>& _rvecs_exp, const vector<Mat>& _tvecs_exp, 227 const vector<Mat>& rvecs_est, const vector<Mat>& tvecs_est) 228 { 229 const static Mat eye33 = Mat::eye(3, 3, CV_64F); 230 const static Mat zero15 = Mat::zeros(1, 5, CV_64F); 231 Mat _chessboard3D(cb3d); 232 vector<Point2f> uv_exp, uv_est; 233 double res = 0; 234 235 for(size_t i = 0; i < rvecs_exp.size(); ++i) 236 { 237 projectPoints(_chessboard3D, _rvecs_exp[i], _tvecs_exp[i], eye33, zero15, uv_exp); 238 projectPoints(_chessboard3D, rvecs_est[i], tvecs_est[i], eye33, zero15, uv_est); 239 for(size_t j = 0; j < cb3d.size(); ++j) 240 res += norm(uv_exp[i] - uv_est[i]); 241 } 242 return res; 243 } 244 245 Size2f sqSile; 246 247 vector<Point3f> chessboard3D; 248 vector<Mat> boards, rvecs_exp, tvecs_exp, rvecs_spnp, tvecs_spnp; 249 vector< vector<Point3f> > objectPoints; 250 vector< vector<Point2f> > imagePoints_art; 251 vector< vector<Point2f> > imagePoints_findCb; 252 253 254 void prepareForTest(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, size_t brdsNum, const ChessBoardGenerator& cbg) 255 { 256 sqSile = Size2f(1.f, 1.f); 257 Size cornersSize = cbg.cornersSize(); 258 259 chessboard3D.clear(); 260 for(int j = 0; j < cornersSize.height; ++j) 261 for(int i = 0; i < cornersSize.width; ++i) 262 chessboard3D.push_back(Point3f(sqSile.width * i, sqSile.height * j, 0)); 263 264 boards.resize(brdsNum); 265 rvecs_exp.resize(brdsNum); 266 tvecs_exp.resize(brdsNum); 267 objectPoints.clear(); 268 objectPoints.resize(brdsNum, chessboard3D); 269 imagePoints_art.clear(); 270 imagePoints_findCb.clear(); 271 272 vector<Point2f> corners_art, corners_fcb; 273 for(size_t i = 0; i < brdsNum; ++i) 274 { 275 for(;;) 276 { 277 boards[i] = cbg(bg, camMat, distCoeffs, sqSile, corners_art); 278 if(findChessboardCorners(boards[i], cornersSize, corners_fcb)) 279 break; 280 } 281 282 //cv::namedWindow("CB"); imshow("CB", boards[i]); cv::waitKey(); 283 284 imagePoints_art.push_back(corners_art); 285 imagePoints_findCb.push_back(corners_fcb); 286 287 tvecs_exp[i].create(1, 3, CV_64F); 288 *tvecs_exp[i].ptr<Point3d>() = cbg.corners3d[0]; 289 rvecs_exp[i] = calcRvec(cbg.corners3d, cbg.cornersSize()); 290 } 291 292 } 293 294 void runTest(const Size& imgSize, const Mat_<double>& camMat, const Mat_<double>& distCoeffs, size_t brdsNum, const Size& cornersSize, int flag = 0) 295 { 296 const TermCriteria tc(TermCriteria::EPS|TermCriteria::MAX_ITER, 30, 0.1); 297 298 vector< vector<Point2f> > imagePoints; 299 300 switch(flag) 301 { 302 case JUST_FIND_CORNERS: imagePoints = imagePoints_findCb; break; 303 case ARTIFICIAL_CORNERS: imagePoints = imagePoints_art; break; 304 305 case USE_CORNERS_SUBPIX: 306 for(size_t i = 0; i < brdsNum; ++i) 307 { 308 Mat gray; 309 cvtColor(boards[i], gray, COLOR_BGR2GRAY); 310 vector<Point2f> tmp = imagePoints_findCb[i]; 311 cornerSubPix(gray, tmp, Size(5, 5), Size(-1,-1), tc); 312 imagePoints.push_back(tmp); 313 } 314 break; 315 case USE_4QUAD_CORNERS: 316 for(size_t i = 0; i < brdsNum; ++i) 317 { 318 Mat gray; 319 cvtColor(boards[i], gray, COLOR_BGR2GRAY); 320 vector<Point2f> tmp = imagePoints_findCb[i]; 321 find4QuadCornerSubpix(gray, tmp, Size(5, 5)); 322 imagePoints.push_back(tmp); 323 } 324 break; 325 default: 326 throw std::exception(); 327 } 328 329 Mat camMat_est = Mat::eye(3, 3, CV_64F), distCoeffs_est = Mat::zeros(1, 5, CV_64F); 330 vector<Mat> rvecs_est, tvecs_est; 331 332 int flags = /*CALIB_FIX_K3|*/CALIB_FIX_K4|CALIB_FIX_K5|CALIB_FIX_K6; //CALIB_FIX_K3; //CALIB_FIX_ASPECT_RATIO | | CALIB_ZERO_TANGENT_DIST; 333 TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, DBL_EPSILON); 334 double rep_error = calibrateCamera(objectPoints, imagePoints, imgSize, camMat_est, distCoeffs_est, rvecs_est, tvecs_est, flags, criteria); 335 rep_error /= brdsNum * cornersSize.area(); 336 337 const double thres = 1; 338 if (rep_error > thres) 339 { 340 ts->printf( cvtest::TS::LOG, "%d) Too big reproject error = %f\n", r, rep_error); 341 ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); 342 } 343 344 compareCameraMatrs(camMat, camMat_est); 345 compareDistCoeffs(distCoeffs, distCoeffs_est); 346 compareShiftVecs(tvecs_exp, tvecs_est); 347 compareRotationVecs(rvecs_exp, rvecs_est); 348 349 double rep_errorWOI = reprojectErrorWithoutIntrinsics(chessboard3D, rvecs_exp, tvecs_exp, rvecs_est, tvecs_est); 350 rep_errorWOI /= brdsNum * cornersSize.area(); 351 352 const double thres2 = 0.01; 353 if (rep_errorWOI > thres2) 354 { 355 ts->printf( cvtest::TS::LOG, "%d) Too big reproject error without intrinsics = %f\n", r, rep_errorWOI); 356 ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY); 357 } 358 359 ts->printf( cvtest::TS::LOG, "%d) Testing solvePnP...\n", r); 360 rvecs_spnp.resize(brdsNum); 361 tvecs_spnp.resize(brdsNum); 362 for(size_t i = 0; i < brdsNum; ++i) 363 solvePnP(Mat(objectPoints[i]), Mat(imagePoints[i]), camMat, distCoeffs, rvecs_spnp[i], tvecs_spnp[i]); 364 365 compareShiftVecs(tvecs_exp, tvecs_spnp); 366 compareRotationVecs(rvecs_exp, rvecs_spnp); 367 } 368 369 void run(int) 370 { 371 372 ts->set_failed_test_info(cvtest::TS::OK); 373 RNG& rng = theRNG(); 374 375 int progress = 0; 376 int repeat_num = 3; 377 for(r = 0; r < repeat_num; ++r) 378 { 379 const int brds_num = 20; 380 381 Mat bg(Size(640, 480), CV_8UC3); 382 randu(bg, Scalar::all(32), Scalar::all(255)); 383 GaussianBlur(bg, bg, Size(5, 5), 2); 384 385 double fx = 300 + (20 * (double)rng - 10); 386 double fy = 300 + (20 * (double)rng - 10); 387 388 double cx = bg.cols/2 + (40 * (double)rng - 20); 389 double cy = bg.rows/2 + (40 * (double)rng - 20); 390 391 Mat_<double> camMat(3, 3); 392 camMat << fx, 0., cx, 0, fy, cy, 0., 0., 1.; 393 394 double k1 = 0.5 + (double)rng/5; 395 double k2 = (double)rng/5; 396 double k3 = (double)rng/5; 397 398 double p1 = 0.001 + (double)rng/10; 399 double p2 = 0.001 + (double)rng/10; 400 401 Mat_<double> distCoeffs(1, 5, 0.0); 402 distCoeffs << k1, k2, p1, p2, k3; 403 404 ChessBoardGenerator cbg(Size(9, 8)); 405 cbg.min_cos = 0.9; 406 cbg.cov = 0.8; 407 408 progress = update_progress(progress, r, repeat_num, 0); 409 ts->printf( cvtest::TS::LOG, "\n"); 410 prepareForTest(bg, camMat, distCoeffs, brds_num, cbg); 411 412 ts->printf( cvtest::TS::LOG, "artificial corners\n"); 413 runTest(bg.size(), camMat, distCoeffs, brds_num, cbg.cornersSize(), ARTIFICIAL_CORNERS); 414 progress = update_progress(progress, r, repeat_num, 0); 415 416 ts->printf( cvtest::TS::LOG, "findChessboard corners\n"); 417 runTest(bg.size(), camMat, distCoeffs, brds_num, cbg.cornersSize(), JUST_FIND_CORNERS); 418 progress = update_progress(progress, r, repeat_num, 0); 419 420 ts->printf( cvtest::TS::LOG, "cornersSubPix corners\n"); 421 runTest(bg.size(), camMat, distCoeffs, brds_num, cbg.cornersSize(), USE_CORNERS_SUBPIX); 422 progress = update_progress(progress, r, repeat_num, 0); 423 424 ts->printf( cvtest::TS::LOG, "4quad corners\n"); 425 runTest(bg.size(), camMat, distCoeffs, brds_num, cbg.cornersSize(), USE_4QUAD_CORNERS); 426 progress = update_progress(progress, r, repeat_num, 0); 427 } 428 } 429 }; 430 431 TEST(Calib3d_CalibrateCamera_CPP, DISABLED_accuracy_on_artificial_data) { CV_CalibrateCameraArtificialTest test; test.safe_run(); } 432
