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

@@ -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,

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

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_<double> A = Mat_<double>(cameraMatrix), Ar;
+    if( !newCameraMatrix.empty() )
+        Ar = Mat_<double>(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_<double> R = Mat_<double>::eye(3, 3);
+    if( !matR.empty() )
+    {
+        R = Mat_<double>(matR);
+        //Note, do not inverse
+    }
+    CV_Assert( Size(3,3) == R.size() );
+
+    // Init distortion vector
+    if( !distCoeffs.empty() )
+        distCoeffs = Mat_<double>(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<cv::Point2i> p2i_objPoints;
+    std::vector<cv::Point2f> 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<float>(c), static_cast<float>(r)));
+        }
+    }
+
+    // Undistort
+    std::vector<cv::Point2f> 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<cv::Point2f> p2f_sourcePoints_pinHole;
+    perspectiveTransform(
+        p2f_objPoints_undistorted,
+        p2f_sourcePoints_pinHole,
+        R
+    );
+
+    // Project points back to camera coordinates.
+    std::vector<cv::Point2f> 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<Vec2s>(p2i_objPoints[i].y, p2i_objPoints[i].x) = Vec2s(saturate_cast<short>(p2f_sourcePoints[i].x), saturate_cast<short>(p2f_sourcePoints[i].y));
+        }
+    } else if (m1type == CV_32FC2) {
+        for (size_t i=0; i < p2i_objPoints.size(); i++) {
+            map1.at<Vec2f>(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<float>(p2i_objPoints[i].y, p2i_objPoints[i].x) = p2f_sourcePoints[i].x;
+            map2.at<float>(p2i_objPoints[i].y, p2i_objPoints[i].x) = p2f_sourcePoints[i].y;
+        }
+    }
+}
 
 void undistort( InputArray _src, OutputArray _dst, InputArray _cameraMatrix,
                 InputArray _distCoeffs, InputArray _newCameraMatrix )

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<vector<Size> >& sizes, vector<vector<int> >& 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<vector<Size> >& sizes, vector<vector<int> >& 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<double> dist(dist_size);
+    vector<double> 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<double>(0) = CV_PI/8*(cvtest::randReal(rng) - (double)0.5); // phi
+    rotation.at<double>(1) = CV_PI/8*(cvtest::randReal(rng) - (double)0.5); // ksi
+    rotation.at<double>(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 /////////////////////////////////
 

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<int>* minloc, vector<int>* maxloc, const Mat& mask=Mat());

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<float>(v, u) = (float)(y__*ify + cyn);
+            _mapx.at<float>(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() )