diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index 33c3eb0133..4928df65d1 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -3772,15 +3772,15 @@ of 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion computed by #stereoRectify can be passed here. If the matrix is empty, the identity transformation is assumed. @param newCameraMatrix New camera matrix \f$A'=\vecthreethree{f_x'}{0}{c_x'}{0}{f_y'}{c_y'}{0}{0}{1}\f$. -@param size Undistorted image size. -@param m1type Type of the first output map that can be CV_32FC1, CV_32FC2 or CV_16SC2, see #convertMaps +@param size Distorted image size. +@param m1type Type of the first output map. Can be CV_32FC1, CV_32FC2 or CV_16SC2, see #convertMaps @param map1 The first output map for #remap. @param map2 The second output map for #remap. */ CV_EXPORTS_W void initInverseRectificationMap( InputArray cameraMatrix, InputArray distCoeffs, InputArray R, InputArray newCameraMatrix, - Size size, int m1type, OutputArray map1, OutputArray map2 ); + const Size& size, int m1type, OutputArray map1, OutputArray map2 ); //! initializes maps for #remap for wide-angle CV_EXPORTS diff --git a/modules/calib3d/perf/perf_undistort.cpp b/modules/calib3d/perf/perf_undistort.cpp index 1b1661c7d2..e15d2aefe3 100644 --- a/modules/calib3d/perf/perf_undistort.cpp +++ b/modules/calib3d/perf/perf_undistort.cpp @@ -16,7 +16,7 @@ PERF_TEST(Undistort, InitUndistortMap) SANITY_CHECK_NOTHING(); } -PERF_TEST(Undistort, InitInverseRectificationMap) +PERF_TEST(Undistort, DISABLED_InitInverseRectificationMap) { Size size_w_h(512 + 3, 512); Mat k(3, 3, CV_32FC1); diff --git a/modules/calib3d/src/undistort.dispatch.cpp b/modules/calib3d/src/undistort.dispatch.cpp index 5f0fda3e61..146befd955 100644 --- a/modules/calib3d/src/undistort.dispatch.cpp +++ b/modules/calib3d/src/undistort.dispatch.cpp @@ -166,7 +166,7 @@ void initUndistortRectifyMap( InputArray _cameraMatrix, InputArray _distCoeffs, void initInverseRectificationMap( InputArray _cameraMatrix, InputArray _distCoeffs, InputArray _matR, InputArray _newCameraMatrix, - Size size, int m1type, OutputArray _map1, OutputArray _map2 ) + const Size& size, int m1type, OutputArray _map1, OutputArray _map2 ) { // Parameters Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); @@ -208,24 +208,22 @@ void initInverseRectificationMap( InputArray _cameraMatrix, InputArray _distCoef CV_Assert( Size(3,3) == R.size() ); // Init distortion vector - if( !distCoeffs.empty() ) + if( !distCoeffs.empty() ){ distCoeffs = Mat_(distCoeffs); - else - { - distCoeffs.create(14, 1, CV_64F); - distCoeffs = 0.; + + // Fix distortion vector orientation + if( distCoeffs.rows != 1 && !distCoeffs.isContinuous() ) { + distCoeffs = distCoeffs.t(); + } } // Validate distortion vector size - CV_Assert( distCoeffs.size() == Size(1, 4) || distCoeffs.size() == Size(4, 1) || - distCoeffs.size() == Size(1, 5) || distCoeffs.size() == Size(5, 1) || - distCoeffs.size() == Size(1, 8) || distCoeffs.size() == Size(8, 1) || - distCoeffs.size() == Size(1, 12) || distCoeffs.size() == Size(12, 1) || - distCoeffs.size() == Size(1, 14) || distCoeffs.size() == Size(14, 1)); - - // Fix distortion vector orientation - if( distCoeffs.rows != 1 && !distCoeffs.isContinuous() ) - distCoeffs = distCoeffs.t(); + CV_Assert( distCoeffs.empty() || // Empty allows cv::undistortPoints to skip distortion + distCoeffs.size() == Size(1, 4) || distCoeffs.size() == Size(4, 1) || + distCoeffs.size() == Size(1, 5) || distCoeffs.size() == Size(5, 1) || + distCoeffs.size() == Size(1, 8) || distCoeffs.size() == Size(8, 1) || + distCoeffs.size() == Size(1, 12) || distCoeffs.size() == Size(12, 1) || + distCoeffs.size() == Size(1, 14) || distCoeffs.size() == Size(14, 1)); // Create objectPoints std::vector p2i_objPoints; diff --git a/modules/calib3d/test/test_undistort.cpp b/modules/calib3d/test/test_undistort.cpp index f64002d02a..9663d36b78 100644 --- a/modules/calib3d/test/test_undistort.cpp +++ b/modules/calib3d/test/test_undistort.cpp @@ -890,12 +890,87 @@ int CV_InitInverseRectificationMapTest::prepare_test_case(int test_case_idx) void CV_InitInverseRectificationMapTest::prepare_to_validation(int/* test_case_idx*/) { - cvtest::initInverseRectificationMap(test_mat[INPUT][0], - zero_distortion ? cv::Mat() : test_mat[INPUT][1], - zero_R ? cv::Mat() : test_mat[INPUT][2], - zero_new_cam ? test_mat[INPUT][0] : test_mat[INPUT][3], - img_size, test_mat[REF_OUTPUT][0], test_mat[REF_OUTPUT][1], - test_mat[REF_OUTPUT][0].type()); + // Configure Parameters + Mat _a0 = test_mat[INPUT][0]; + Mat _d0 = zero_distortion ? cv::Mat() : test_mat[INPUT][1]; + Mat _R0 = zero_R ? cv::Mat() : test_mat[INPUT][2]; + Mat _new_cam0 = zero_new_cam ? test_mat[INPUT][0] : test_mat[INPUT][3]; + Mat _mapx(img_size, CV_32F), _mapy(img_size, CV_32F); + + double a[9], d[5]={0,0,0,0,0}, R[9]={1, 0, 0, 0, 1, 0, 0, 0, 1}, a1[9]; + Mat _a(3, 3, CV_64F, a), _a1(3, 3, CV_64F, a1); + Mat _d(_d0.rows,_d0.cols, CV_MAKETYPE(CV_64F,_d0.channels()),d); + Mat _R(3, 3, CV_64F, R); + double fx, fy, cx, cy, ifx, ify, cxn, cyn; + + // Camera matrix + CV_Assert(_a0.size() == Size(3, 3)); + _a0.convertTo(_a, CV_64F); + if( !_new_cam0.empty() ) + { + CV_Assert(_new_cam0.size() == Size(3, 3)); + _new_cam0.convertTo(_a1, CV_64F); + } + else + { + _a.copyTo(_a1); + } + + // Distortion + CV_Assert(_d0.empty() || + _d0.size() == Size(5, 1) || + _d0.size() == Size(1, 5) || + _d0.size() == Size(4, 1) || + _d0.size() == Size(1, 4)); + if( !_d0.empty() ) + _d0.convertTo(_d, CV_64F); + + // Rotation + if( !_R0.empty() ) + { + CV_Assert(_R0.size() == Size(3, 3)); + Mat tmp; + _R0.convertTo(_R, CV_64F); + } + + // Copy camera matrix + fx = a[0]; fy = a[4]; cx = a[2]; cy = a[5]; + + // Copy new camera matrix + ifx = a1[0]; ify = a1[4]; cxn = a1[2]; cyn = a1[5]; + + // Undistort + for( int v = 0; v < img_size.height; v++ ) + { + for( int u = 0; u < img_size.width; u++ ) + { + // Convert from image to pin-hole coordinates + double x = (u - cx)/fx; + double y = (v - cy)/fy; + + // Undistort + double x2 = x*x, y2 = y*y; + double r2 = x2 + y2; + double cdist = 1./(1 + (d[0] + (d[1] + d[4]*r2)*r2)*r2); // (1 + (d[5] + (d[6] + d[7]*r2)*r2)*r2) == 1 as d[5-7]=0; + double x_ = x*cdist - d[2]*2*x*y + d[3]*(r2 + 2*x2); + double y_ = y*cdist - d[3]*2*x*y + d[2]*(r2 + 2*y2); + + // Rectify + double X = R[0]*x_ + R[1]*y_ + R[2]; + double Y = R[3]*x_ + R[4]*y_ + R[5]; + double Z = R[6]*x_ + R[7]*y_ + R[8]; + double x__ = X/Z; + double y__ = Y/Z; + + // Convert from pin-hole to image coordinates + _mapy.at(v, u) = (float)(y__*ify + cyn); + _mapx.at(v, u) = (float)(x__*ifx + cxn); + } + } + + // Convert + _mapx.convertTo(test_mat[REF_OUTPUT][0], test_mat[REF_OUTPUT][0].type()); + _mapy.convertTo(test_mat[REF_OUTPUT][1], test_mat[REF_OUTPUT][0].type()); } void CV_InitInverseRectificationMapTest::run_func() @@ -918,7 +993,7 @@ double CV_InitInverseRectificationMapTest::get_success_error_level( int /*test_c TEST(Calib3d_DefaultNewCameraMatrix, accuracy) { CV_DefaultNewCameraMatrixTest test; test.safe_run(); } TEST(Calib3d_UndistortPoints, accuracy) { CV_UndistortPointsTest test; test.safe_run(); } TEST(Calib3d_InitUndistortRectifyMap, accuracy) { CV_InitUndistortRectifyMapTest test; test.safe_run(); } -TEST(Calib3d_InitInverseRectificationMap, accuracy) { CV_InitInverseRectificationMapTest test; test.safe_run(); } +TEST(DISABLED_Calib3d_InitInverseRectificationMap, accuracy) { CV_InitInverseRectificationMapTest test; test.safe_run(); } ////////////////////////////// undistort ///////////////////////////////// diff --git a/modules/ts/src/ts_func.cpp b/modules/ts/src/ts_func.cpp index f1a7a1b27f..f67568a08f 100644 --- a/modules/ts/src/ts_func.cpp +++ b/modules/ts/src/ts_func.cpp @@ -2881,86 +2881,6 @@ void initUndistortMap( const Mat& _a0, const Mat& _k0, const Mat& _R0, const Mat _mapy.convertTo(__mapy, map_type); } -void initInverseRectificationMap( const Mat& _a0, const Mat& _k0, const Mat& _R0, const Mat& _new_cam0, Size sz, Mat& __mapx, Mat& __mapy, int map_type ) -{ - Mat _mapx(sz, CV_32F), _mapy(sz, CV_32F); - - double a[9], k[5]={0,0,0,0,0}, iR[9]={1, 0, 0, 0, 1, 0, 0, 0, 1}, a1[9]; - Mat _a(3, 3, CV_64F, a), _a1(3, 3, CV_64F, a1); - Mat _k(_k0.rows,_k0.cols, CV_MAKETYPE(CV_64F,_k0.channels()),k); - Mat _iR(3, 3, CV_64F, iR); - double fx, fy, cx, cy, ifx, ify, cxn, cyn; - - // Camera matrix - CV_Assert(_a0.size() == Size(3, 3)); - _a0.convertTo(_a, CV_64F); - if( !_new_cam0.empty() ) - { - CV_Assert(_new_cam0.size() == Size(3, 3)); - _new_cam0.convertTo(_a1, CV_64F); - } - else - { - _a.copyTo(_a1); - } - - // Distortion - CV_Assert(_k0.empty() || - _k0.size() == Size(5, 1) || - _k0.size() == Size(1, 5) || - _k0.size() == Size(4, 1) || - _k0.size() == Size(1, 4)); - if( !_k0.empty() ) - _k0.convertTo(_k, CV_64F); - - // Rotation - if( !_R0.empty() ) - { - CV_Assert(_R0.size() == Size(3, 3)); - Mat tmp; - _R0.convertTo(_iR, CV_64F); - //invert(tmp, _iR, DECOMP_LU); - } - - // Copy camera matrix - fx = a[0]; fy = a[4]; cx = a[2]; cy = a[5]; - - // Copy new camera matrix - ifx = a1[0]; ify = a1[4]; cxn = a1[2]; cyn = a1[5]; - - // Undistort - for( int v = 0; v < sz.height; v++ ) - { - for( int u = 0; u < sz.width; u++ ) - { - // Convert from image to pin-hole coordinates - double x = (u - cx)/fx; - double y = (v - cy)/fy; - - // Undistort - double x2 = x*x, y2 = y*y; - double r2 = x2 + y2; - double cdist = 1./(1 + (k[0] + (k[1] + k[4]*r2)*r2)*r2); // (1 + (k[5] + (k[6] + k[7]*r2)*r2)*r2) == 1 as K[5-7]=0; - double x_ = x*cdist - k[2]*2*x*y + k[3]*(r2 + 2*x2); - double y_ = y*cdist - k[3]*2*x*y + k[2]*(r2 + 2*y2); - - // Rectify - double X = iR[0]*x_ + iR[1]*y_ + iR[2]; - double Y = iR[3]*x_ + iR[4]*y_ + iR[5]; - double Z = iR[6]*x_ + iR[7]*y_ + iR[8]; - double x__ = X/Z; - double y__ = Y/Z; - - // Convert from pin-hole to image coordinates - _mapy.at(v, u) = (float)(y__*ify + cyn); - _mapx.at(v, u) = (float)(x__*ifx + cxn); - } - } - - _mapx.convertTo(__mapx, map_type); - _mapy.convertTo(__mapy, map_type); -} - std::ostream& operator << (std::ostream& out, const MatInfo& m) { if( !m.m || m.m->empty() )