diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index 95e91afe4c..3bd8a9fe4f 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -3711,6 +3711,77 @@ void initUndistortRectifyMap(InputArray cameraMatrix, InputArray distCoeffs, InputArray R, InputArray newCameraMatrix, Size size, int m1type, OutputArray map1, OutputArray map2); +/** @brief Computes the projection and inverse-rectification transformation map. In essense, this is the inverse of +#initUndistortRectifyMap to accomodate stereo-rectification of projectors ('inverse-cameras') in projector-camera pairs. + +The function computes the joint projection and inverse rectification transformation and represents the +result in the form of maps for #remap. The projected image looks like a distorted version of the original which, +once projected by a projector, should visually match the original. In case of a monocular camera, newCameraMatrix +is usually equal to cameraMatrix, or it can be computed by +#getOptimalNewCameraMatrix for a better control over scaling. In case of a projector-camera pair, +newCameraMatrix is normally set to P1 or P2 computed by #stereoRectify . + +The projector is oriented differently in the coordinate space, according to R. In case of projector-camera pairs, +this helps align the projector (in the same manner as #initUndistortRectifyMap for the camera) to create a stereo-rectified pair. This +allows epipolar lines on both images to become horizontal and have the same y-coordinate (in case of a horizontally aligned projector-camera pair). + +The function builds the maps for the inverse mapping algorithm that is used by #remap. That +is, for each pixel \f$(u, v)\f$ in the destination (projected and inverse-rectified) image, the function +computes the corresponding coordinates in the source image (that is, in the original digital image). The following process is applied: + +\f[ +\begin{array}{l} +\text{newCameraMatrix}\\ +x \leftarrow (u - {c'}_x)/{f'}_x \\ +y \leftarrow (v - {c'}_y)/{f'}_y \\ + +\\\text{Undistortion} +\\\scriptsize{\textit{though equation shown is for radial undistortion, function implements cv::undistortPoints()}}\\ +r^2 \leftarrow x^2 + y^2 \\ +\theta \leftarrow \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6}\\ +x' \leftarrow \frac{x}{\theta} \\ +y' \leftarrow \frac{y}{\theta} \\ + +\\\text{Rectification}\\ +{[X\,Y\,W]} ^T \leftarrow R*[x' \, y' \, 1]^T \\ +x'' \leftarrow X/W \\ +y'' \leftarrow Y/W \\ + +\\\text{cameraMatrix}\\ +map_x(u,v) \leftarrow x'' f_x + c_x \\ +map_y(u,v) \leftarrow y'' f_y + c_y +\end{array} +\f] +where \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ +are the distortion coefficients vector distCoeffs. + +In case of a stereo-rectified projector-camera pair, this function is called for the projector while #initUndistortRectifyMap is called for the camera head. +This is done after #stereoRectify, which in turn is called after #stereoCalibrate. If the projector-camera pair +is not calibrated, it is still possible to compute the rectification transformations directly from +the fundamental matrix using #stereoRectifyUncalibrated. For the projector and camera, the function computes +homography H as the rectification transformation in a pixel domain, not a rotation matrix R in 3D +space. R can be computed from H as +\f[\texttt{R} = \texttt{cameraMatrix} ^{-1} \cdot \texttt{H} \cdot \texttt{cameraMatrix}\f] +where cameraMatrix can be chosen arbitrarily. + +@param cameraMatrix Input camera matrix \f$A=\vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ . +@param distCoeffs Input vector of distortion coefficients +\f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ +of 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed. +@param R Optional rectification transformation in the object space (3x3 matrix). R1 or R2, +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 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 ); + //! initializes maps for #remap for wide-angle CV_EXPORTS float initWideAngleProjMap(InputArray cameraMatrix, InputArray distCoeffs, diff --git a/modules/calib3d/perf/perf_undistort.cpp b/modules/calib3d/perf/perf_undistort.cpp index 5372aaea92..1b1661c7d2 100644 --- a/modules/calib3d/perf/perf_undistort.cpp +++ b/modules/calib3d/perf/perf_undistort.cpp @@ -16,4 +16,15 @@ PERF_TEST(Undistort, InitUndistortMap) SANITY_CHECK_NOTHING(); } +PERF_TEST(Undistort, InitInverseRectificationMap) +{ + Size size_w_h(512 + 3, 512); + Mat k(3, 3, CV_32FC1); + Mat d(1, 14, CV_64FC1); + Mat dst(size_w_h, CV_32FC2); + declare.in(k, d, WARMUP_RNG).out(dst); + TEST_CYCLE() initInverseRectificationMap(k, d, noArray(), k, size_w_h, CV_32FC2, dst, noArray()); + SANITY_CHECK_NOTHING(); +} + } // namespace diff --git a/modules/calib3d/src/undistort.dispatch.cpp b/modules/calib3d/src/undistort.dispatch.cpp index 2dd52037a9..5f0fda3e61 100644 --- a/modules/calib3d/src/undistort.dispatch.cpp +++ b/modules/calib3d/src/undistort.dispatch.cpp @@ -164,6 +164,127 @@ void initUndistortRectifyMap( InputArray _cameraMatrix, InputArray _distCoeffs, fx, fy, k1, k2, p1, p2, k3, k4, k5, k6, s1, s2, s3, s4)); } +void initInverseRectificationMap( InputArray _cameraMatrix, InputArray _distCoeffs, + InputArray _matR, InputArray _newCameraMatrix, + Size size, int m1type, OutputArray _map1, OutputArray _map2 ) +{ + // Parameters + Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); + Mat matR = _matR.getMat(), newCameraMatrix = _newCameraMatrix.getMat(); + + // Check m1type validity + if( m1type <= 0 ) + m1type = CV_16SC2; + CV_Assert( m1type == CV_16SC2 || m1type == CV_32FC1 || m1type == CV_32FC2 ); + + // Init Maps + _map1.create( size, m1type ); + Mat map1 = _map1.getMat(), map2; + if( m1type != CV_32FC2 ) + { + _map2.create( size, m1type == CV_16SC2 ? CV_16UC1 : CV_32FC1 ); + map2 = _map2.getMat(); + } + else { + _map2.release(); + } + + // Init camera intrinsics + Mat_ A = Mat_(cameraMatrix), Ar; + if( !newCameraMatrix.empty() ) + Ar = Mat_(newCameraMatrix); + else + Ar = getDefaultNewCameraMatrix( A, size, true ); + CV_Assert( A.size() == Size(3,3) ); + CV_Assert( Ar.size() == Size(3,3) || Ar.size() == Size(4, 3)); + + // Init rotation matrix + Mat_ R = Mat_::eye(3, 3); + if( !matR.empty() ) + { + R = Mat_(matR); + //Note, do not inverse + } + CV_Assert( Size(3,3) == R.size() ); + + // Init distortion vector + if( !distCoeffs.empty() ) + distCoeffs = Mat_(distCoeffs); + else + { + distCoeffs.create(14, 1, CV_64F); + distCoeffs = 0.; + } + + // 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(); + + // Create objectPoints + std::vector p2i_objPoints; + std::vector p2f_objPoints; + for (int r = 0; r < size.height; r++) + { + for (int c = 0; c < size.width; c++) + { + p2i_objPoints.push_back(cv::Point2i(c, r)); + p2f_objPoints.push_back(cv::Point2f(static_cast(c), static_cast(r))); + } + } + + // Undistort + std::vector p2f_objPoints_undistorted; + undistortPoints( + p2f_objPoints, + p2f_objPoints_undistorted, + A, + distCoeffs, + cv::Mat::eye(cv::Size(3, 3), CV_64FC1), // R + cv::Mat::eye(cv::Size(3, 3), CV_64FC1) // P = New K + ); + + // Rectify + std::vector p2f_sourcePoints_pinHole; + perspectiveTransform( + p2f_objPoints_undistorted, + p2f_sourcePoints_pinHole, + R + ); + + // Project points back to camera coordinates. + std::vector p2f_sourcePoints; + undistortPoints( + p2f_sourcePoints_pinHole, + p2f_sourcePoints, + cv::Mat::eye(cv::Size(3, 3), CV_32FC1), // K + cv::Mat::zeros(cv::Size(1, 4), CV_32FC1), // Distortion + cv::Mat::eye(cv::Size(3, 3), CV_32FC1), // R + Ar // New K + ); + + // Copy to map + if (m1type == CV_16SC2) { + for (size_t i=0; i < p2i_objPoints.size(); i++) { + map1.at(p2i_objPoints[i].y, p2i_objPoints[i].x) = Vec2s(saturate_cast(p2f_sourcePoints[i].x), saturate_cast(p2f_sourcePoints[i].y)); + } + } else if (m1type == CV_32FC2) { + for (size_t i=0; i < p2i_objPoints.size(); i++) { + map1.at(p2i_objPoints[i].y, p2i_objPoints[i].x) = Vec2f(p2f_sourcePoints[i]); + } + } else { // m1type == CV_32FC1 + for (size_t i=0; i < p2i_objPoints.size(); i++) { + map1.at(p2i_objPoints[i].y, p2i_objPoints[i].x) = p2f_sourcePoints[i].x; + map2.at(p2i_objPoints[i].y, p2i_objPoints[i].x) = p2f_sourcePoints[i].y; + } + } +} void undistort( InputArray _src, OutputArray _dst, InputArray _cameraMatrix, InputArray _distCoeffs, InputArray _newCameraMatrix ) diff --git a/modules/calib3d/test/test_undistort.cpp b/modules/calib3d/test/test_undistort.cpp index c1ec2063ee..f64002d02a 100644 --- a/modules/calib3d/test/test_undistort.cpp +++ b/modules/calib3d/test/test_undistort.cpp @@ -719,11 +719,206 @@ double CV_InitUndistortRectifyMapTest::get_success_error_level( int /*test_case_ return 8; } +//------------------------------------------------------ + +class CV_InitInverseRectificationMapTest : public cvtest::ArrayTest +{ +public: + CV_InitInverseRectificationMapTest(); +protected: + int prepare_test_case (int test_case_idx); + void prepare_to_validation( int test_case_idx ); + void get_test_array_types_and_sizes( int test_case_idx, vector >& sizes, vector >& types ); + double get_success_error_level( int test_case_idx, int i, int j ); + void run_func(); + +private: + static const int MAX_X = 1024; + static const int MAX_Y = 1024; + bool zero_new_cam; + bool zero_distortion; + bool zero_R; + + cv::Size img_size; + int map_type; +}; + +CV_InitInverseRectificationMapTest::CV_InitInverseRectificationMapTest() +{ + test_array[INPUT].push_back(NULL); // camera matrix + test_array[INPUT].push_back(NULL); // distortion coeffs + test_array[INPUT].push_back(NULL); // R matrix + test_array[INPUT].push_back(NULL); // new camera matrix + test_array[OUTPUT].push_back(NULL); // inverse rectified mapx + test_array[OUTPUT].push_back(NULL); // inverse rectified mapy + test_array[REF_OUTPUT].push_back(NULL); + test_array[REF_OUTPUT].push_back(NULL); + + zero_distortion = zero_new_cam = zero_R = false; + map_type = 0; +} + +void CV_InitInverseRectificationMapTest::get_test_array_types_and_sizes( int test_case_idx, vector >& sizes, vector >& types ) +{ + cvtest::ArrayTest::get_test_array_types_and_sizes(test_case_idx,sizes,types); + RNG& rng = ts->get_rng(); + //rng.next(); + + map_type = CV_32F; + types[OUTPUT][0] = types[OUTPUT][1] = types[REF_OUTPUT][0] = types[REF_OUTPUT][1] = map_type; + + img_size.width = cvtest::randInt(rng) % MAX_X + 1; + img_size.height = cvtest::randInt(rng) % MAX_Y + 1; + + types[INPUT][0] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F; + types[INPUT][1] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F; + types[INPUT][2] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F; + types[INPUT][3] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F; + + sizes[OUTPUT][0] = sizes[OUTPUT][1] = sizes[REF_OUTPUT][0] = sizes[REF_OUTPUT][1] = img_size; + sizes[INPUT][0] = sizes[INPUT][2] = sizes[INPUT][3] = cvSize(3,3); + + Size dsize; + + if (cvtest::randInt(rng)%2) + { + if (cvtest::randInt(rng)%2) + { + dsize = Size(1,4); + } + else + { + dsize = Size(1,5); + } + } + else + { + if (cvtest::randInt(rng)%2) + { + dsize = Size(4,1); + } + else + { + dsize = Size(5,1); + } + } + sizes[INPUT][1] = dsize; +} + + +int CV_InitInverseRectificationMapTest::prepare_test_case(int test_case_idx) +{ + RNG& rng = ts->get_rng(); + int code = cvtest::ArrayTest::prepare_test_case( test_case_idx ); + + if (code <= 0) + return code; + + int dist_size = test_mat[INPUT][1].cols > test_mat[INPUT][1].rows ? test_mat[INPUT][1].cols : test_mat[INPUT][1].rows; + double cam[9] = {0,0,0,0,0,0,0,0,1}; + vector dist(dist_size); + vector new_cam(test_mat[INPUT][3].cols * test_mat[INPUT][3].rows); + + Mat _camera(3,3,CV_64F,cam); + Mat _distort(test_mat[INPUT][1].size(),CV_64F,&dist[0]); + Mat _new_cam(test_mat[INPUT][3].size(),CV_64F,&new_cam[0]); + + //Generating camera matrix + double sz = MAX(img_size.width,img_size.height); + double aspect_ratio = cvtest::randReal(rng)*0.6 + 0.7; + cam[2] = (img_size.width - 1)*0.5 + cvtest::randReal(rng)*10 - 5; + cam[5] = (img_size.height - 1)*0.5 + cvtest::randReal(rng)*10 - 5; + cam[0] = sz/(0.9 - cvtest::randReal(rng)*0.6); + cam[4] = aspect_ratio*cam[0]; + + //Generating distortion coeffs + dist[0] = cvtest::randReal(rng)*0.06 - 0.03; + dist[1] = cvtest::randReal(rng)*0.06 - 0.03; + if( dist[0]*dist[1] > 0 ) + dist[1] = -dist[1]; + if( cvtest::randInt(rng)%4 != 0 ) + { + dist[2] = cvtest::randReal(rng)*0.004 - 0.002; + dist[3] = cvtest::randReal(rng)*0.004 - 0.002; + if (dist_size > 4) + dist[4] = cvtest::randReal(rng)*0.004 - 0.002; + } + else + { + dist[2] = dist[3] = 0; + if (dist_size > 4) + dist[4] = 0; + } + + //Generating new camera matrix + _new_cam = Scalar::all(0); + new_cam[8] = 1; + + // If P == K + //new_cam[0] = cam[0]; + //new_cam[4] = cam[4]; + //new_cam[2] = cam[2]; + //new_cam[5] = cam[5]; + + // If P != K + new_cam[0] = cam[0] + (cvtest::randReal(rng) - (double)0.5)*0.2*cam[0]; //10% + new_cam[4] = cam[4] + (cvtest::randReal(rng) - (double)0.5)*0.2*cam[4]; //10% + new_cam[2] = cam[2] + (cvtest::randReal(rng) - (double)0.5)*0.3*img_size.width; //15% + new_cam[5] = cam[5] + (cvtest::randReal(rng) - (double)0.5)*0.3*img_size.height; //15% + + //Generating R matrix + Mat _rot(3,3,CV_64F); + Mat rotation(1,3,CV_64F); + rotation.at(0) = CV_PI/8*(cvtest::randReal(rng) - (double)0.5); // phi + rotation.at(1) = CV_PI/8*(cvtest::randReal(rng) - (double)0.5); // ksi + rotation.at(2) = CV_PI/3*(cvtest::randReal(rng) - (double)0.5); //khi + cvtest::Rodrigues(rotation, _rot); + + //cvSetIdentity(_rot); + //copying data + cvtest::convert( _camera, test_mat[INPUT][0], test_mat[INPUT][0].type()); + cvtest::convert( _distort, test_mat[INPUT][1], test_mat[INPUT][1].type()); + cvtest::convert( _rot, test_mat[INPUT][2], test_mat[INPUT][2].type()); + cvtest::convert( _new_cam, test_mat[INPUT][3], test_mat[INPUT][3].type()); + + zero_distortion = (cvtest::randInt(rng)%2) == 0 ? false : true; + zero_new_cam = (cvtest::randInt(rng)%2) == 0 ? false : true; + zero_R = (cvtest::randInt(rng)%2) == 0 ? false : true; + + return code; +} + +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()); +} + +void CV_InitInverseRectificationMapTest::run_func() +{ + cv::Mat camera_mat = test_mat[INPUT][0]; + cv::Mat dist = zero_distortion ? cv::Mat() : test_mat[INPUT][1]; + cv::Mat R = zero_R ? cv::Mat() : test_mat[INPUT][2]; + cv::Mat new_cam = zero_new_cam ? cv::Mat() : test_mat[INPUT][3]; + cv::Mat& mapx = test_mat[OUTPUT][0], &mapy = test_mat[OUTPUT][1]; + cv::initInverseRectificationMap(camera_mat,dist,R,new_cam,img_size,map_type,mapx,mapy); +} + +double CV_InitInverseRectificationMapTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int /*j*/ ) +{ + return 8; +} + ////////////////////////////////////////////////////////////////////////////////////////////////////// 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(); } ////////////////////////////// undistort ///////////////////////////////// diff --git a/modules/ts/include/opencv2/ts.hpp b/modules/ts/include/opencv2/ts.hpp index ed7491a89a..394bc6e0fa 100644 --- a/modules/ts/include/opencv2/ts.hpp +++ b/modules/ts/include/opencv2/ts.hpp @@ -326,6 +326,7 @@ Mat calcSobelKernel2D( int dx, int dy, int apertureSize, int origin=0 ); Mat calcLaplaceKernel2D( int aperture_size ); void initUndistortMap( const Mat& a, const Mat& k, const Mat& R, const Mat& new_a, Size sz, Mat& mapx, Mat& mapy, int map_type ); +void initInverseRectificationMap( const Mat& a, const Mat& k, const Mat& R, const Mat& new_a, Size sz, Mat& mapx, Mat& mapy, int map_type ); void minMaxLoc(const Mat& src, double* minval, double* maxval, vector* minloc, vector* maxloc, const Mat& mask=Mat()); diff --git a/modules/ts/src/ts_func.cpp b/modules/ts/src/ts_func.cpp index f67568a08f..f1a7a1b27f 100644 --- a/modules/ts/src/ts_func.cpp +++ b/modules/ts/src/ts_func.cpp @@ -2881,6 +2881,86 @@ 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() )