16acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*M/////////////////////////////////////////////////////////////////////////////////////// 26acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 36acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 46acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 56acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// By downloading, copying, installing or using the software you agree to this license. 66acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// If you do not agree to this license, do not download, install, 76acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// copy or use the software. 86acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 96acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Intel License Agreement 116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// For Open Source Computer Vision Library 126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Copyright (C) 2000, Intel Corporation, all rights reserved. 146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Third party copyrights are property of their respective owners. 156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Redistribution and use in source and binary forms, with or without modification, 176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// are permitted provided that the following conditions are met: 186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's of source code must retain the above copyright notice, 206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer. 216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's in binary form must reproduce the above copyright notice, 236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer in the documentation 246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and/or other materials provided with the distribution. 256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * The name of Intel Corporation may not be used to endorse or promote products 276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// derived from this software without specific prior written permission. 286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// This software is provided by the copyright holders and contributors "as is" and 306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// any express or implied warranties, including, but not limited to, the implied 316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// warranties of merchantability and fitness for a particular purpose are disclaimed. 326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// In no event shall the Intel Corporation or contributors be liable for any direct, 336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// indirect, incidental, special, exemplary, or consequential damages 346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// (including, but not limited to, procurement of substitute goods or services; 356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// loss of use, data, or profits; or business interruption) however caused 366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and on any theory of liability, whether in contract, strict liability, 376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// or tort (including negligence or otherwise) arising in any way out of 386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// the use of this software, even if advised of the possibility of such damage. 396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//M*/ 416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "_cv.h" 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic CvStatus 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvUnDistort_8u_CnR( const uchar* src, int srcstep, 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* dst, int dststep, CvSize size, 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* intrinsic_matrix, 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* dist_coeffs, int cn ) 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int u, v, i; 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float u0 = intrinsic_matrix[2], v0 = intrinsic_matrix[5]; 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float x0 = (size.width-1)*0.5f, y0 = (size.height-1)*0.5f; 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float fx = intrinsic_matrix[0], fy = intrinsic_matrix[4]; 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float ifx = 1.f/fx, ify = 1.f/fy; 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float k1 = dist_coeffs[0], k2 = dist_coeffs[1], k3 = dist_coeffs[4]; 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float p1 = dist_coeffs[2], p2 = dist_coeffs[3]; 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn srcstep /= sizeof(src[0]); 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dststep /= sizeof(dst[0]); 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( v = 0; v < size.height; v++, dst += dststep ) 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float y = (v - v0)*ify, y2 = y*y; 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( u = 0; u < size.width; u++ ) 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float x = (u - u0)*ifx, x2 = x*x, r2 = x2 + y2, _2xy = 2*x*y; 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2; 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float _x = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + x0; 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float _y = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + y0; 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int ix = cvFloor(_x), iy = cvFloor(_y); 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (unsigned)iy < (unsigned)(size.height - 1) && 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (unsigned)ix < (unsigned)(size.width - 1) ) 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const uchar* ptr = src + iy*srcstep + ix*cn; 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _x -= ix; _y -= iy; 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < cn; i++ ) 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float t0 = CV_8TO32F(ptr[i]), t1 = CV_8TO32F(ptr[i+srcstep]); 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t0 += _x*(CV_8TO32F(ptr[i+cn]) - t0); 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t1 += _x*(CV_8TO32F(ptr[i + srcstep + cn]) - t1); 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst[u*cn + i] = (uchar)cvRound(t0 + _y*(t1 - t0)); 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < cn; i++ ) 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst[u*cn + i] = 0; 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CV_OK; 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvUndistortGetSize_t icvUndistortGetSize_p = 0; 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvCreateMapCameraUndistort_32f_C1R_t icvCreateMapCameraUndistort_32f_C1R_p = 0; 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvUndistortRadial_8u_C1R_t icvUndistortRadial_8u_C1R_p = 0; 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvUndistortRadial_8u_C3R_t icvUndistortRadial_8u_C3R_p = 0; 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renntypedef CvStatus (CV_STDCALL * CvUndistortRadialIPPFunc) 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ( const void* pSrc, int srcStep, void* pDst, int dstStep, CvSize roiSize, 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float fx, float fy, float cx, float cy, float k1, float k2, uchar *pBuffer ); 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvUndistort2( const CvArr* _src, CvArr* _dst, const CvMat* A, const CvMat* dist_coeffs ) 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn static int inittab = 0; 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvUndistort2" ); 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float a[9], k[5]={0,0,0,0,0}; 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int coi1 = 0, coi2 = 0; 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat srcstub, *src = (CvMat*)_src; 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dststub, *dst = (CvMat*)_dst; 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _a = cvMat( 3, 3, CV_32F, a ), _k; 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int cn, src_step, dst_step; 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize size; 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !inittab ) 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvInitLinearCoeffTab(); 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvInitCubicCoeffTab(); 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn inittab = 1; 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( src = cvGetMat( src, &srcstub, &coi1 )); 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( dst = cvGetMat( dst, &dststub, &coi2 )); 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( coi1 != 0 || coi2 != 0 ) 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_BadCOI, "The function does not support COI" ); 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_DEPTH(src->type) != CV_8U ) 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Only 8-bit images are supported" ); 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( src->data.ptr == dst->data.ptr ) 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNotImplemented, "In-place undistortion is not implemented" ); 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_TYPES_EQ( src, dst )) 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedFormats, "" ); 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_SIZES_EQ( src, dst )) 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "" ); 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3 || 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1) ) 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" ); 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(dist_coeffs) || (dist_coeffs->rows != 1 && dist_coeffs->cols != 1) || 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 && 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 5) || 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_DEPTH(dist_coeffs->type) != CV_64F && 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_DEPTH(dist_coeffs->type) != CV_32F) ) 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "Distortion coefficients must be 1x4, 4x1, 1x5 or 5x1 floating-point vector" ); 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( A, &_a ); 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _k = cvMat( dist_coeffs->rows, dist_coeffs->cols, 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAKETYPE(CV_32F, CV_MAT_CN(dist_coeffs->type)), k ); 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( dist_coeffs, &_k ); 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cn = CV_MAT_CN(src->type); 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn size = cvGetMatSize(src); 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn src_step = src->step ? src->step : CV_STUB_STEP; 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst_step = dst->step ? dst->step : CV_STUB_STEP; 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvUnDistort_8u_CnR( src->data.ptr, src_step, 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst->data.ptr, dst_step, size, a, k, cn ); 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvInitUndistortMap( const CvMat* A, const CvMat* dist_coeffs, 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvArr* mapxarr, CvArr* mapyarr ) 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvInitUndistortMap" ); 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float a[9], k[5]={0,0,0,0,0}; 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int coi1 = 0, coi2 = 0; 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat mapxstub, *_mapx = (CvMat*)mapxarr; 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat mapystub, *_mapy = (CvMat*)mapyarr; 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _a = cvMat( 3, 3, CV_32F, a ), _k; 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int u, v; 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float u0, v0, fx, fy, ifx, ify, x0, y0, k1, k2, k3, p1, p2; 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize size; 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _mapx = cvGetMat( _mapx, &mapxstub, &coi1 )); 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _mapy = cvGetMat( _mapy, &mapystub, &coi2 )); 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( coi1 != 0 || coi2 != 0 ) 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_BadCOI, "The function does not support COI" ); 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(_mapx->type) != CV_32FC1 ) 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Both maps must have 32fC1 type" ); 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_TYPES_EQ( _mapx, _mapy )) 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedFormats, "" ); 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_SIZES_EQ( _mapx, _mapy )) 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "" ); 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn size = cvGetMatSize(_mapx); 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3 || 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1) ) 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" ); 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(dist_coeffs) || (dist_coeffs->rows != 1 && dist_coeffs->cols != 1) || 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 && 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 5) || 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_DEPTH(dist_coeffs->type) != CV_64F && 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_DEPTH(dist_coeffs->type) != CV_32F) ) 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "Distortion coefficients must be 1x4, 4x1, 1x5 or 5x1 floating-point vector" ); 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( A, &_a ); 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _k = cvMat( dist_coeffs->rows, dist_coeffs->cols, 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAKETYPE(CV_32F, CV_MAT_CN(dist_coeffs->type)), k ); 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( dist_coeffs, &_k ); 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u0 = a[2]; v0 = a[5]; 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fx = a[0]; fy = a[4]; 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ifx = 1.f/fx; ify = 1.f/fy; 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn k1 = k[0]; k2 = k[1]; k3 = k[4]; 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p1 = k[2]; p2 = k[3]; 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x0 = (size.width-1)*0.5f; 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y0 = (size.height-1)*0.5f; 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( v = 0; v < size.height; v++ ) 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* mapx = (float*)(_mapx->data.ptr + _mapx->step*v); 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* mapy = (float*)(_mapy->data.ptr + _mapy->step*v); 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float y = (v - v0)*ify, y2 = y*y; 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( u = 0; u < size.width; u++ ) 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float x = (u - u0)*ifx, x2 = x*x, r2 = x2 + y2, _2xy = 2*x*y; 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2; 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _x = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + x0; 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _y = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + y0; 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mapx[u] = (float)_x; 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mapy[u] = (float)_y; 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvInitUndistortRectifyMap( const CvMat* A, const CvMat* distCoeffs, 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat *R, const CvMat* Ar, CvArr* mapxarr, CvArr* mapyarr ) 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvInitUndistortMap" ); 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double a[9], ar[9], r[9], ir[9], k[5]={0,0,0,0,0}; 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int coi1 = 0, coi2 = 0; 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat mapxstub, *_mapx = (CvMat*)mapxarr; 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat mapystub, *_mapy = (CvMat*)mapyarr; 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _a = cvMat( 3, 3, CV_64F, a ); 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _k = cvMat( 4, 1, CV_64F, k ); 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _ar = cvMat( 3, 3, CV_64F, ar ); 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _r = cvMat( 3, 3, CV_64F, r ); 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _ir = cvMat( 3, 3, CV_64F, ir ); 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j; 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double fx, fy, u0, v0, k1, k2, k3, p1, p2; 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize size; 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _mapx = cvGetMat( _mapx, &mapxstub, &coi1 )); 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _mapy = cvGetMat( _mapy, &mapystub, &coi2 )); 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( coi1 != 0 || coi2 != 0 ) 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_BadCOI, "The function does not support COI" ); 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(_mapx->type) != CV_32FC1 ) 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Both maps must have 32fC1 type" ); 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_TYPES_EQ( _mapx, _mapy )) 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedFormats, "" ); 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_SIZES_EQ( _mapx, _mapy )) 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "" ); 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( A ) 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3 || 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1) ) 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" ); 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( A, &_a ); 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSetIdentity( &_a ); 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( Ar ) 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat Ar33; 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(Ar) || Ar->rows != 3 || (Ar->cols != 3 && Ar->cols != 4) || 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_TYPE(Ar->type) != CV_32FC1 && CV_MAT_TYPE(Ar->type) != CV_64FC1) ) 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "The new intrinsic matrix must be a valid 3x3 floating-point matrix" ); 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( Ar, &Ar33, 0, 3 ); 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &Ar33, &_ar ); 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSetIdentity( &_ar ); 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(R) || R->rows != 3 || R->cols != 3 || 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_TYPE(R->type) != CV_32FC1 && CV_MAT_TYPE(R->type) != CV_64FC1) ) 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "Rotaion/homography matrix must be a valid 3x3 floating-point matrix" ); 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( distCoeffs ) 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_IS_MAT(distCoeffs) && 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (distCoeffs->rows == 1 || distCoeffs->cols == 1) && 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) == 4 || 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) == 5) && 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_DEPTH(distCoeffs->type) == CV_64F || 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_DEPTH(distCoeffs->type) == CV_32F) ); 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _k = cvMat( distCoeffs->rows, distCoeffs->cols, 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAKETYPE(CV_64F, CV_MAT_CN(distCoeffs->type)), k ); 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( distCoeffs, &_k ); 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( &_k ); 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( R, &_r ); // rectification matrix 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &_ar, &_r, &_r ); // Ar*R 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvInvert( &_r, &_ir ); // inverse: R^-1*Ar^-1 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn u0 = a[2]; v0 = a[5]; 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fx = a[0]; fy = a[4]; 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn k1 = k[0]; k2 = k[1]; k3 = k[4]; 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn p1 = k[2]; p2 = k[3]; 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn size = cvGetMatSize(_mapx); 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < size.height; i++ ) 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* mapx = (float*)(_mapx->data.ptr + _mapx->step*i); 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* mapy = (float*)(_mapy->data.ptr + _mapy->step*i); 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _x = i*ir[1] + ir[2], _y = i*ir[4] + ir[5], _w = i*ir[7] + ir[8]; 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < size.width; j++, _x += ir[0], _y += ir[3], _w += ir[6] ) 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double w = 1./_w, x = _x*w, y = _y*w; 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x2 = x*x, y2 = y*y; 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double r2 = x2 + y2, _2xy = 2*x*y; 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2; 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double u = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + u0; 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double v = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + v0; 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mapx[j] = (float)u; 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mapy[j] = (float)v; 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix, 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* _distCoeffs, 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* _R, const CvMat* _P ) 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvUndistortPoints" ); 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double A[3][3], RR[3][3], k[5]={0,0,0,0,0}, fx, fy, ifx, ify, cx, cy; 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _A=cvMat(3, 3, CV_64F, A), _Dk; 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _RR=cvMat(3, 3, CV_64F, RR); 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvPoint2D32f* srcf; 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvPoint2D64f* srcd; 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f* dstf; 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64f* dstd; 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int stype, dtype; 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int sstep, dstep; 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j, n; 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_IS_MAT(_src) && CV_IS_MAT(_dst) && 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (_src->rows == 1 || _src->cols == 1) && 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (_dst->rows == 1 || _dst->cols == 1) && 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ARE_SIZES_EQ(_src, _dst) && 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) && 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2)); 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_IS_MAT(_cameraMatrix) && CV_IS_MAT(_distCoeffs) && 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _cameraMatrix->rows == 3 && _cameraMatrix->cols == 3 && 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (_distCoeffs->rows == 1 || _distCoeffs->cols == 1) && 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (_distCoeffs->rows*_distCoeffs->cols == 4 || 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _distCoeffs->rows*_distCoeffs->cols == 5) ); 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _Dk = cvMat( _distCoeffs->rows, _distCoeffs->cols, 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k); 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( _cameraMatrix, &_A ); 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( _distCoeffs, &_Dk ); 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _R ) 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_IS_MAT(_R) && _R->rows == 3 && _R->cols == 3 ); 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( _R, &_RR ); 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSetIdentity(&_RR); 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _P ) 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double PP[3][3]; 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _P3x3, _PP=cvMat(3, 3, CV_64F, PP); 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_IS_MAT(_P) && _P->rows == 3 && (_P->cols == 3 || _P->cols == 4)); 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( cvGetCols(_P, &_P3x3, 0, 3), &_PP ); 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &_PP, &_RR, &_RR ); 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn srcf = (const CvPoint2D32f*)_src->data.ptr; 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn srcd = (const CvPoint2D64f*)_src->data.ptr; 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstf = (CvPoint2D32f*)_dst->data.ptr; 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstd = (CvPoint2D64f*)_dst->data.ptr; 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn stype = CV_MAT_TYPE(_src->type); 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dtype = CV_MAT_TYPE(_dst->type); 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype); 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype); 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n = _src->rows + _src->cols - 1; 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fx = A[0][0]; 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fy = A[1][1]; 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ifx = 1./fx; 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ify = 1./fy; 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cx = A[0][2]; 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cy = A[1][2]; 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < n; i++ ) 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x, y, x0, y0; 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( stype == CV_32FC2 ) 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = srcf[i*sstep].x; 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = srcf[i*sstep].y; 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = srcd[i*sstep].x; 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = srcd[i*sstep].y; 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x0 = x = (x - cx)*ifx; 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y0 = y = (y - cy)*ify; 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // compensate distortion iteratively 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < 5; j++ ) 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double r2 = x*x + y*y; 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double icdist = 1./(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2); 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x); 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y; 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = (x0 - deltaX)*icdist; 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = (y0 - deltaY)*icdist; 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double xx = RR[0][0]*x + RR[0][1]*y + RR[0][2]; 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double yy = RR[1][0]*x + RR[1][1]*y + RR[1][2]; 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double ww = 1./(RR[2][0]*x + RR[2][1]*y + RR[2][2]); 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x = xx*ww; 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn y = yy*ww; 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dtype == CV_32FC2 ) 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstf[i*dstep].x = (float)x; 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstf[i*dstep].y = (float)y; 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstd[i*dstep].x = x; 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dstd[i*dstep].y = y; 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* End of file */ 492