| 60 | } |
| 61 | |
| 62 | void cv::initUndistortRectifyMap( InputArray _cameraMatrix, InputArray _distCoeffs, |
| 63 | InputArray _matR, InputArray _newCameraMatrix, |
| 64 | Size size, int m1type, OutputArray _map1, OutputArray _map2 ) |
| 65 | { |
| 66 | Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); |
| 67 | Mat matR = _matR.getMat(), newCameraMatrix = _newCameraMatrix.getMat(); |
| 68 | |
| 69 | if( m1type <= 0 ) |
| 70 | m1type = CV_16SC2; |
| 71 | CV_Assert( m1type == CV_16SC2 || m1type == CV_32FC1 || m1type == CV_32FC2 ); |
| 72 | _map1.create( size, m1type ); |
| 73 | Mat map1 = _map1.getMat(), map2; |
| 74 | if( m1type != CV_32FC2 ) |
| 75 | { |
| 76 | _map2.create( size, m1type == CV_16SC2 ? CV_16UC1 : CV_32FC1 ); |
| 77 | map2 = _map2.getMat(); |
| 78 | } |
| 79 | else |
| 80 | _map2.release(); |
| 81 | |
| 82 | Mat_<double> R = Mat_<double>::eye(3, 3); |
| 83 | Mat_<double> A = Mat_<double>(cameraMatrix), Ar; |
| 84 | |
| 85 | if( newCameraMatrix.data ) |
| 86 | Ar = Mat_<double>(newCameraMatrix); |
| 87 | else |
| 88 | Ar = getDefaultNewCameraMatrix( A, size, true ); |
| 89 | |
| 90 | if( matR.data ) |
| 91 | R = Mat_<double>(matR); |
| 92 | |
| 93 | if( distCoeffs.data ) |
| 94 | distCoeffs = Mat_<double>(distCoeffs); |
| 95 | else |
| 96 | { |
| 97 | distCoeffs.create(8, 1, CV_64F); |
| 98 | distCoeffs = 0.; |
| 99 | } |
| 100 | |
| 101 | CV_Assert( A.size() == Size(3,3) && A.size() == R.size() ); |
| 102 | CV_Assert( Ar.size() == Size(3,3) || Ar.size() == Size(4, 3)); |
| 103 | Mat_<double> iR = (Ar.colRange(0,3)*R).inv(DECOMP_LU); |
| 104 | const double* ir = &iR(0,0); |
| 105 | |
| 106 | double u0 = A(0, 2), v0 = A(1, 2); |
| 107 | double fx = A(0, 0), fy = A(1, 1); |
| 108 | |
| 109 | CV_Assert( distCoeffs.size() == Size(1, 4) || distCoeffs.size() == Size(4, 1) || |
| 110 | distCoeffs.size() == Size(1, 5) || distCoeffs.size() == Size(5, 1) || |
| 111 | distCoeffs.size() == Size(1, 8) || distCoeffs.size() == Size(8, 1)); |
| 112 | |
| 113 | if( distCoeffs.rows != 1 && !distCoeffs.isContinuous() ) |
| 114 | distCoeffs = distCoeffs.t(); |
| 115 | |
| 116 | double k1 = ((double*)distCoeffs.data)[0]; |
| 117 | double k2 = ((double*)distCoeffs.data)[1]; |
| 118 | double p1 = ((double*)distCoeffs.data)[2]; |
| 119 | double p2 = ((double*)distCoeffs.data)[3]; |