From b05631432b39f1e9d152e8f8e20ddb401356e999 Mon Sep 17 00:00:00 2001 From: Ian Maquignaz <9im14@queensu.ca> Date: Fri, 28 May 2021 16:47:36 -0400 Subject: [PATCH] Added declaration, definition and unit test for initInverseRectificationMap() Fixed trailing whitespace Update to initInverseRectificationMap documentation for clarity Added test case for initInverseRectificationMap() Updated documentation. Fixed whitespace error in docs Small update to test function Now passes success_error_level final update to inverseRectification documentation --- modules/calib3d/include/opencv2/calib3d.hpp | 71 +++++++ modules/calib3d/perf/perf_undistort.cpp | 11 ++ modules/calib3d/src/undistort.dispatch.cpp | 121 ++++++++++++ modules/calib3d/test/test_undistort.cpp | 195 ++++++++++++++++++++ modules/ts/include/opencv2/ts.hpp | 1 + modules/ts/src/ts_func.cpp | 80 ++++++++ 6 files changed, 479 insertions(+) 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() )