| /*M/////////////////////////////////////////////////////////////////////////////////////// |
| // |
| // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. |
| // |
| // By downloading, copying, installing or using the software you agree to this license. |
| // If you do not agree to this license, do not download, install, |
| // copy or use the software. |
| // |
| // |
| // Intel License Agreement |
| // For Open Source Computer Vision Library |
| // |
| // Copyright (C) 2000, Intel Corporation, all rights reserved. |
| // Third party copyrights are property of their respective owners. |
| // |
| // Redistribution and use in source and binary forms, with or without modification, |
| // are permitted provided that the following conditions are met: |
| // |
| // * Redistribution's of source code must retain the above copyright notice, |
| // this list of conditions and the following disclaimer. |
| // |
| // * Redistribution's in binary form must reproduce the above copyright notice, |
| // this list of conditions and the following disclaimer in the documentation |
| // and/or other materials provided with the distribution. |
| // |
| // * The name of Intel Corporation may not be used to endorse or promote products |
| // derived from this software without specific prior written permission. |
| // |
| // This software is provided by the copyright holders and contributors "as is" and |
| // any express or implied warranties, including, but not limited to, the implied |
| // warranties of merchantability and fitness for a particular purpose are disclaimed. |
| // In no event shall the Intel Corporation or contributors be liable for any direct, |
| // indirect, incidental, special, exemplary, or consequential damages |
| // (including, but not limited to, procurement of substitute goods or services; |
| // loss of use, data, or profits; or business interruption) however caused |
| // and on any theory of liability, whether in contract, strict liability, |
| // or tort (including negligence or otherwise) arising in any way out of |
| // the use of this software, even if advised of the possibility of such damage. |
| // |
| //M*/ |
| |
| #include "_cv.h" |
| |
| static CvStatus |
| icvUnDistort_8u_CnR( const uchar* src, int srcstep, |
| uchar* dst, int dststep, CvSize size, |
| const float* intrinsic_matrix, |
| const float* dist_coeffs, int cn ) |
| { |
| int u, v, i; |
| float u0 = intrinsic_matrix[2], v0 = intrinsic_matrix[5]; |
| float x0 = (size.width-1)*0.5f, y0 = (size.height-1)*0.5f; |
| float fx = intrinsic_matrix[0], fy = intrinsic_matrix[4]; |
| float ifx = 1.f/fx, ify = 1.f/fy; |
| float k1 = dist_coeffs[0], k2 = dist_coeffs[1], k3 = dist_coeffs[4]; |
| float p1 = dist_coeffs[2], p2 = dist_coeffs[3]; |
| |
| srcstep /= sizeof(src[0]); |
| dststep /= sizeof(dst[0]); |
| |
| for( v = 0; v < size.height; v++, dst += dststep ) |
| { |
| float y = (v - v0)*ify, y2 = y*y; |
| |
| for( u = 0; u < size.width; u++ ) |
| { |
| float x = (u - u0)*ifx, x2 = x*x, r2 = x2 + y2, _2xy = 2*x*y; |
| float kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2; |
| float _x = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + x0; |
| float _y = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + y0; |
| int ix = cvFloor(_x), iy = cvFloor(_y); |
| |
| if( (unsigned)iy < (unsigned)(size.height - 1) && |
| (unsigned)ix < (unsigned)(size.width - 1) ) |
| { |
| const uchar* ptr = src + iy*srcstep + ix*cn; |
| _x -= ix; _y -= iy; |
| for( i = 0; i < cn; i++ ) |
| { |
| float t0 = CV_8TO32F(ptr[i]), t1 = CV_8TO32F(ptr[i+srcstep]); |
| t0 += _x*(CV_8TO32F(ptr[i+cn]) - t0); |
| t1 += _x*(CV_8TO32F(ptr[i + srcstep + cn]) - t1); |
| dst[u*cn + i] = (uchar)cvRound(t0 + _y*(t1 - t0)); |
| } |
| } |
| else |
| { |
| for( i = 0; i < cn; i++ ) |
| dst[u*cn + i] = 0; |
| } |
| |
| } |
| } |
| |
| return CV_OK; |
| } |
| |
| |
| icvUndistortGetSize_t icvUndistortGetSize_p = 0; |
| icvCreateMapCameraUndistort_32f_C1R_t icvCreateMapCameraUndistort_32f_C1R_p = 0; |
| icvUndistortRadial_8u_C1R_t icvUndistortRadial_8u_C1R_p = 0; |
| icvUndistortRadial_8u_C3R_t icvUndistortRadial_8u_C3R_p = 0; |
| |
| typedef CvStatus (CV_STDCALL * CvUndistortRadialIPPFunc) |
| ( const void* pSrc, int srcStep, void* pDst, int dstStep, CvSize roiSize, |
| float fx, float fy, float cx, float cy, float k1, float k2, uchar *pBuffer ); |
| |
| CV_IMPL void |
| cvUndistort2( const CvArr* _src, CvArr* _dst, const CvMat* A, const CvMat* dist_coeffs ) |
| { |
| static int inittab = 0; |
| |
| CV_FUNCNAME( "cvUndistort2" ); |
| |
| __BEGIN__; |
| |
| float a[9], k[5]={0,0,0,0,0}; |
| int coi1 = 0, coi2 = 0; |
| CvMat srcstub, *src = (CvMat*)_src; |
| CvMat dststub, *dst = (CvMat*)_dst; |
| CvMat _a = cvMat( 3, 3, CV_32F, a ), _k; |
| int cn, src_step, dst_step; |
| CvSize size; |
| |
| if( !inittab ) |
| { |
| icvInitLinearCoeffTab(); |
| icvInitCubicCoeffTab(); |
| inittab = 1; |
| } |
| |
| CV_CALL( src = cvGetMat( src, &srcstub, &coi1 )); |
| CV_CALL( dst = cvGetMat( dst, &dststub, &coi2 )); |
| |
| if( coi1 != 0 || coi2 != 0 ) |
| CV_ERROR( CV_BadCOI, "The function does not support COI" ); |
| |
| if( CV_MAT_DEPTH(src->type) != CV_8U ) |
| CV_ERROR( CV_StsUnsupportedFormat, "Only 8-bit images are supported" ); |
| |
| if( src->data.ptr == dst->data.ptr ) |
| CV_ERROR( CV_StsNotImplemented, "In-place undistortion is not implemented" ); |
| |
| if( !CV_ARE_TYPES_EQ( src, dst )) |
| CV_ERROR( CV_StsUnmatchedFormats, "" ); |
| |
| if( !CV_ARE_SIZES_EQ( src, dst )) |
| CV_ERROR( CV_StsUnmatchedSizes, "" ); |
| |
| if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3 || |
| (CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1) ) |
| CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" ); |
| |
| if( !CV_IS_MAT(dist_coeffs) || (dist_coeffs->rows != 1 && dist_coeffs->cols != 1) || |
| (dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 && |
| dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 5) || |
| (CV_MAT_DEPTH(dist_coeffs->type) != CV_64F && |
| CV_MAT_DEPTH(dist_coeffs->type) != CV_32F) ) |
| CV_ERROR( CV_StsBadArg, |
| "Distortion coefficients must be 1x4, 4x1, 1x5 or 5x1 floating-point vector" ); |
| |
| cvConvert( A, &_a ); |
| _k = cvMat( dist_coeffs->rows, dist_coeffs->cols, |
| CV_MAKETYPE(CV_32F, CV_MAT_CN(dist_coeffs->type)), k ); |
| cvConvert( dist_coeffs, &_k ); |
| |
| cn = CV_MAT_CN(src->type); |
| size = cvGetMatSize(src); |
| src_step = src->step ? src->step : CV_STUB_STEP; |
| dst_step = dst->step ? dst->step : CV_STUB_STEP; |
| |
| icvUnDistort_8u_CnR( src->data.ptr, src_step, |
| dst->data.ptr, dst_step, size, a, k, cn ); |
| |
| __END__; |
| } |
| |
| |
| CV_IMPL void |
| cvInitUndistortMap( const CvMat* A, const CvMat* dist_coeffs, |
| CvArr* mapxarr, CvArr* mapyarr ) |
| { |
| CV_FUNCNAME( "cvInitUndistortMap" ); |
| |
| __BEGIN__; |
| |
| float a[9], k[5]={0,0,0,0,0}; |
| int coi1 = 0, coi2 = 0; |
| CvMat mapxstub, *_mapx = (CvMat*)mapxarr; |
| CvMat mapystub, *_mapy = (CvMat*)mapyarr; |
| CvMat _a = cvMat( 3, 3, CV_32F, a ), _k; |
| int u, v; |
| float u0, v0, fx, fy, ifx, ify, x0, y0, k1, k2, k3, p1, p2; |
| CvSize size; |
| |
| CV_CALL( _mapx = cvGetMat( _mapx, &mapxstub, &coi1 )); |
| CV_CALL( _mapy = cvGetMat( _mapy, &mapystub, &coi2 )); |
| |
| if( coi1 != 0 || coi2 != 0 ) |
| CV_ERROR( CV_BadCOI, "The function does not support COI" ); |
| |
| if( CV_MAT_TYPE(_mapx->type) != CV_32FC1 ) |
| CV_ERROR( CV_StsUnsupportedFormat, "Both maps must have 32fC1 type" ); |
| |
| if( !CV_ARE_TYPES_EQ( _mapx, _mapy )) |
| CV_ERROR( CV_StsUnmatchedFormats, "" ); |
| |
| if( !CV_ARE_SIZES_EQ( _mapx, _mapy )) |
| CV_ERROR( CV_StsUnmatchedSizes, "" ); |
| |
| size = cvGetMatSize(_mapx); |
| |
| if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3 || |
| (CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1) ) |
| CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" ); |
| |
| if( !CV_IS_MAT(dist_coeffs) || (dist_coeffs->rows != 1 && dist_coeffs->cols != 1) || |
| (dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 && |
| dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 5) || |
| (CV_MAT_DEPTH(dist_coeffs->type) != CV_64F && |
| CV_MAT_DEPTH(dist_coeffs->type) != CV_32F) ) |
| CV_ERROR( CV_StsBadArg, |
| "Distortion coefficients must be 1x4, 4x1, 1x5 or 5x1 floating-point vector" ); |
| |
| cvConvert( A, &_a ); |
| _k = cvMat( dist_coeffs->rows, dist_coeffs->cols, |
| CV_MAKETYPE(CV_32F, CV_MAT_CN(dist_coeffs->type)), k ); |
| cvConvert( dist_coeffs, &_k ); |
| |
| u0 = a[2]; v0 = a[5]; |
| fx = a[0]; fy = a[4]; |
| ifx = 1.f/fx; ify = 1.f/fy; |
| k1 = k[0]; k2 = k[1]; k3 = k[4]; |
| p1 = k[2]; p2 = k[3]; |
| x0 = (size.width-1)*0.5f; |
| y0 = (size.height-1)*0.5f; |
| |
| for( v = 0; v < size.height; v++ ) |
| { |
| float* mapx = (float*)(_mapx->data.ptr + _mapx->step*v); |
| float* mapy = (float*)(_mapy->data.ptr + _mapy->step*v); |
| float y = (v - v0)*ify, y2 = y*y; |
| |
| for( u = 0; u < size.width; u++ ) |
| { |
| float x = (u - u0)*ifx, x2 = x*x, r2 = x2 + y2, _2xy = 2*x*y; |
| double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2; |
| double _x = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + x0; |
| double _y = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + y0; |
| mapx[u] = (float)_x; |
| mapy[u] = (float)_y; |
| } |
| } |
| |
| __END__; |
| } |
| |
| |
| void |
| cvInitUndistortRectifyMap( const CvMat* A, const CvMat* distCoeffs, |
| const CvMat *R, const CvMat* Ar, CvArr* mapxarr, CvArr* mapyarr ) |
| { |
| CV_FUNCNAME( "cvInitUndistortMap" ); |
| |
| __BEGIN__; |
| |
| double a[9], ar[9], r[9], ir[9], k[5]={0,0,0,0,0}; |
| int coi1 = 0, coi2 = 0; |
| CvMat mapxstub, *_mapx = (CvMat*)mapxarr; |
| CvMat mapystub, *_mapy = (CvMat*)mapyarr; |
| CvMat _a = cvMat( 3, 3, CV_64F, a ); |
| CvMat _k = cvMat( 4, 1, CV_64F, k ); |
| CvMat _ar = cvMat( 3, 3, CV_64F, ar ); |
| CvMat _r = cvMat( 3, 3, CV_64F, r ); |
| CvMat _ir = cvMat( 3, 3, CV_64F, ir ); |
| int i, j; |
| double fx, fy, u0, v0, k1, k2, k3, p1, p2; |
| CvSize size; |
| |
| CV_CALL( _mapx = cvGetMat( _mapx, &mapxstub, &coi1 )); |
| CV_CALL( _mapy = cvGetMat( _mapy, &mapystub, &coi2 )); |
| |
| if( coi1 != 0 || coi2 != 0 ) |
| CV_ERROR( CV_BadCOI, "The function does not support COI" ); |
| |
| if( CV_MAT_TYPE(_mapx->type) != CV_32FC1 ) |
| CV_ERROR( CV_StsUnsupportedFormat, "Both maps must have 32fC1 type" ); |
| |
| if( !CV_ARE_TYPES_EQ( _mapx, _mapy )) |
| CV_ERROR( CV_StsUnmatchedFormats, "" ); |
| |
| if( !CV_ARE_SIZES_EQ( _mapx, _mapy )) |
| CV_ERROR( CV_StsUnmatchedSizes, "" ); |
| |
| if( A ) |
| { |
| if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3 || |
| (CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1) ) |
| CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" ); |
| cvConvert( A, &_a ); |
| } |
| else |
| cvSetIdentity( &_a ); |
| |
| if( Ar ) |
| { |
| CvMat Ar33; |
| if( !CV_IS_MAT(Ar) || Ar->rows != 3 || (Ar->cols != 3 && Ar->cols != 4) || |
| (CV_MAT_TYPE(Ar->type) != CV_32FC1 && CV_MAT_TYPE(Ar->type) != CV_64FC1) ) |
| CV_ERROR( CV_StsBadArg, "The new intrinsic matrix must be a valid 3x3 floating-point matrix" ); |
| cvGetCols( Ar, &Ar33, 0, 3 ); |
| cvConvert( &Ar33, &_ar ); |
| } |
| else |
| cvSetIdentity( &_ar ); |
| |
| if( !CV_IS_MAT(R) || R->rows != 3 || R->cols != 3 || |
| (CV_MAT_TYPE(R->type) != CV_32FC1 && CV_MAT_TYPE(R->type) != CV_64FC1) ) |
| CV_ERROR( CV_StsBadArg, "Rotaion/homography matrix must be a valid 3x3 floating-point matrix" ); |
| |
| if( distCoeffs ) |
| { |
| CV_ASSERT( CV_IS_MAT(distCoeffs) && |
| (distCoeffs->rows == 1 || distCoeffs->cols == 1) && |
| (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) == 4 || |
| distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) == 5) && |
| (CV_MAT_DEPTH(distCoeffs->type) == CV_64F || |
| CV_MAT_DEPTH(distCoeffs->type) == CV_32F) ); |
| _k = cvMat( distCoeffs->rows, distCoeffs->cols, |
| CV_MAKETYPE(CV_64F, CV_MAT_CN(distCoeffs->type)), k ); |
| cvConvert( distCoeffs, &_k ); |
| } |
| else |
| cvZero( &_k ); |
| |
| cvConvert( R, &_r ); // rectification matrix |
| cvMatMul( &_ar, &_r, &_r ); // Ar*R |
| cvInvert( &_r, &_ir ); // inverse: R^-1*Ar^-1 |
| |
| u0 = a[2]; v0 = a[5]; |
| fx = a[0]; fy = a[4]; |
| k1 = k[0]; k2 = k[1]; k3 = k[4]; |
| p1 = k[2]; p2 = k[3]; |
| |
| size = cvGetMatSize(_mapx); |
| |
| for( i = 0; i < size.height; i++ ) |
| { |
| float* mapx = (float*)(_mapx->data.ptr + _mapx->step*i); |
| float* mapy = (float*)(_mapy->data.ptr + _mapy->step*i); |
| double _x = i*ir[1] + ir[2], _y = i*ir[4] + ir[5], _w = i*ir[7] + ir[8]; |
| |
| for( j = 0; j < size.width; j++, _x += ir[0], _y += ir[3], _w += ir[6] ) |
| { |
| double w = 1./_w, x = _x*w, y = _y*w; |
| double x2 = x*x, y2 = y*y; |
| double r2 = x2 + y2, _2xy = 2*x*y; |
| double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2; |
| double u = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + u0; |
| double v = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + v0; |
| mapx[j] = (float)u; |
| mapy[j] = (float)v; |
| } |
| } |
| |
| __END__; |
| } |
| |
| |
| void |
| cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix, |
| const CvMat* _distCoeffs, |
| const CvMat* _R, const CvMat* _P ) |
| { |
| CV_FUNCNAME( "cvUndistortPoints" ); |
| |
| __BEGIN__; |
| |
| double A[3][3], RR[3][3], k[5]={0,0,0,0,0}, fx, fy, ifx, ify, cx, cy; |
| CvMat _A=cvMat(3, 3, CV_64F, A), _Dk; |
| CvMat _RR=cvMat(3, 3, CV_64F, RR); |
| const CvPoint2D32f* srcf; |
| const CvPoint2D64f* srcd; |
| CvPoint2D32f* dstf; |
| CvPoint2D64f* dstd; |
| int stype, dtype; |
| int sstep, dstep; |
| int i, j, n; |
| |
| CV_ASSERT( CV_IS_MAT(_src) && CV_IS_MAT(_dst) && |
| (_src->rows == 1 || _src->cols == 1) && |
| (_dst->rows == 1 || _dst->cols == 1) && |
| CV_ARE_SIZES_EQ(_src, _dst) && |
| (CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) && |
| (CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2)); |
| |
| CV_ASSERT( CV_IS_MAT(_cameraMatrix) && CV_IS_MAT(_distCoeffs) && |
| _cameraMatrix->rows == 3 && _cameraMatrix->cols == 3 && |
| (_distCoeffs->rows == 1 || _distCoeffs->cols == 1) && |
| (_distCoeffs->rows*_distCoeffs->cols == 4 || |
| _distCoeffs->rows*_distCoeffs->cols == 5) ); |
| _Dk = cvMat( _distCoeffs->rows, _distCoeffs->cols, |
| CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k); |
| cvConvert( _cameraMatrix, &_A ); |
| cvConvert( _distCoeffs, &_Dk ); |
| |
| if( _R ) |
| { |
| CV_ASSERT( CV_IS_MAT(_R) && _R->rows == 3 && _R->cols == 3 ); |
| cvConvert( _R, &_RR ); |
| } |
| else |
| cvSetIdentity(&_RR); |
| |
| if( _P ) |
| { |
| double PP[3][3]; |
| CvMat _P3x3, _PP=cvMat(3, 3, CV_64F, PP); |
| CV_ASSERT( CV_IS_MAT(_P) && _P->rows == 3 && (_P->cols == 3 || _P->cols == 4)); |
| cvConvert( cvGetCols(_P, &_P3x3, 0, 3), &_PP ); |
| cvMatMul( &_PP, &_RR, &_RR ); |
| } |
| |
| srcf = (const CvPoint2D32f*)_src->data.ptr; |
| srcd = (const CvPoint2D64f*)_src->data.ptr; |
| dstf = (CvPoint2D32f*)_dst->data.ptr; |
| dstd = (CvPoint2D64f*)_dst->data.ptr; |
| stype = CV_MAT_TYPE(_src->type); |
| dtype = CV_MAT_TYPE(_dst->type); |
| sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype); |
| dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype); |
| |
| n = _src->rows + _src->cols - 1; |
| |
| fx = A[0][0]; |
| fy = A[1][1]; |
| ifx = 1./fx; |
| ify = 1./fy; |
| cx = A[0][2]; |
| cy = A[1][2]; |
| |
| for( i = 0; i < n; i++ ) |
| { |
| double x, y, x0, y0; |
| if( stype == CV_32FC2 ) |
| { |
| x = srcf[i*sstep].x; |
| y = srcf[i*sstep].y; |
| } |
| else |
| { |
| x = srcd[i*sstep].x; |
| y = srcd[i*sstep].y; |
| } |
| |
| x0 = x = (x - cx)*ifx; |
| y0 = y = (y - cy)*ify; |
| |
| // compensate distortion iteratively |
| for( j = 0; j < 5; j++ ) |
| { |
| double r2 = x*x + y*y; |
| double icdist = 1./(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2); |
| double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x); |
| double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y; |
| x = (x0 - deltaX)*icdist; |
| y = (y0 - deltaY)*icdist; |
| } |
| |
| double xx = RR[0][0]*x + RR[0][1]*y + RR[0][2]; |
| double yy = RR[1][0]*x + RR[1][1]*y + RR[1][2]; |
| double ww = 1./(RR[2][0]*x + RR[2][1]*y + RR[2][2]); |
| x = xx*ww; |
| y = yy*ww; |
| |
| if( dtype == CV_32FC2 ) |
| { |
| dstf[i*dstep].x = (float)x; |
| dstf[i*dstep].y = (float)y; |
| } |
| else |
| { |
| dstd[i*dstep].x = x; |
| dstd[i*dstep].y = y; |
| } |
| } |
| |
| __END__; |
| } |
| |
| /* End of file */ |