--- /dev/null
+/*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.
+//
+//
+// License Agreement
+// For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., 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 the copyright holders 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"
+
+namespace cv
+{
+
+Mat getDefaultNewCameraMatrix( const Mat& cameraMatrix, Size imgsize,
+ bool centerPrincipalPoint )
+{
+ if( !centerPrincipalPoint && cameraMatrix.type() == CV_64F )
+ return cameraMatrix;
+
+ Mat newCameraMatrix;
+ cameraMatrix.convertTo(newCameraMatrix, CV_64F);
+ if( centerPrincipalPoint )
+ {
+ ((double*)newCameraMatrix.data)[2] = (imgsize.width-1)*0.5;
+ ((double*)newCameraMatrix.data)[5] = (imgsize.height-1)*0.5;
+ }
+ return newCameraMatrix;
+}
+
+void initUndistortRectifyMap( const Mat& _cameraMatrix, const Mat& _distCoeffs,
+ const Mat& _R, const Mat& _newCameraMatrix,
+ Size size, int m1type, Mat& map1, Mat& map2 )
+{
+ if( m1type <= 0 )
+ m1type = CV_16SC2;
+ CV_Assert( m1type == CV_16SC2 || m1type == CV_32FC1 || m1type == CV_32FC2 );
+ map1.create( size, m1type );
+ if( m1type != CV_32FC2 )
+ map2.create( size, m1type == CV_16SC2 ? CV_16UC1 : CV_32FC1 );
+ else
+ map2.release();
+
+ Mat_<double> R = Mat_<double>::eye(3, 3), distCoeffs;
+ Mat_<double> A = Mat_<double>(_cameraMatrix), Ar;
+
+ if( _newCameraMatrix.data )
+ Ar = Mat_<double>(_newCameraMatrix);
+ else
+ Ar = getDefaultNewCameraMatrix( A, size, true );
+
+ if( _R.data )
+ R = Mat_<double>(_R);
+
+ if( _distCoeffs.data )
+ distCoeffs = Mat_<double>(_distCoeffs);
+ else
+ {
+ distCoeffs.create(5, 1);
+ distCoeffs = 0.;
+ }
+
+ CV_Assert( A.size() == Size(3,3) && A.size() == R.size() );
+ CV_Assert( Ar.size() == Size(3,3) || Ar.size() == Size(4, 3));
+ Mat_<double> iR = (Ar.colRange(0,3)*R).inv(DECOMP_LU);
+ const double* ir = &iR(0,0);
+
+ double u0 = A(0, 2), v0 = A(1, 2);
+ double fx = A(0, 0), fy = A(1, 1);
+
+ CV_Assert( distCoeffs.size() == Size(1, 4) || distCoeffs.size() == Size(1, 5) ||
+ distCoeffs.size() == Size(4, 1) || distCoeffs.size() == Size(5, 1));
+
+ if( distCoeffs.rows != 1 && !distCoeffs.isContinuous() )
+ distCoeffs = distCoeffs.t();
+
+ double k1 = ((double*)distCoeffs.data)[0];
+ double k2 = ((double*)distCoeffs.data)[1];
+ double p1 = ((double*)distCoeffs.data)[2];
+ double p2 = ((double*)distCoeffs.data)[3];
+ double k3 = distCoeffs.cols + distCoeffs.rows - 1 == 5 ? ((double*)distCoeffs.data)[4] : 0.;
+
+ for( int i = 0; i < size.height; i++ )
+ {
+ float* m1f = (float*)(map1.data + map1.step*i);
+ float* m2f = (float*)(map2.data + map2.step*i);
+ short* m1 = (short*)m1f;
+ ushort* m2 = (ushort*)m2f;
+ double _x = i*ir[1] + ir[2], _y = i*ir[4] + ir[5], _w = i*ir[7] + ir[8];
+
+ for( int 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;
+ if( m1type == CV_16SC2 )
+ {
+ int iu = saturate_cast<int>(u*INTER_TAB_SIZE);
+ int iv = saturate_cast<int>(v*INTER_TAB_SIZE);
+ m1[j*2] = (short)(iu >> INTER_BITS);
+ m1[j*2+1] = (short)(iv >> INTER_BITS);
+ m2[j] = (ushort)((iv & (INTER_TAB_SIZE-1))*INTER_TAB_SIZE + (iu & (INTER_TAB_SIZE-1)));
+ }
+ else if( m1type == CV_32FC1 )
+ {
+ m1f[j] = (float)u;
+ m2f[j] = (float)v;
+ }
+ else
+ {
+ m1f[j*2] = (float)u;
+ m1f[j*2+1] = (float)v;
+ }
+ }
+ }
+}
+
+
+void undistort( const Mat& src, Mat& dst, const Mat& _cameraMatrix,
+ const Mat& _distCoeffs, const Mat& _newCameraMatrix )
+{
+ dst.create( src.size(), src.type() );
+ int stripe_size0 = std::min(std::max(1, (1 << 12) / std::max(src.cols, 1)), src.rows);
+ Mat map1(stripe_size0, src.cols, CV_16SC2), map2(stripe_size0, src.cols, CV_16UC1);
+
+ Mat_<double> A, distCoeffs, Ar, I = Mat_<double>::eye(3,3);
+
+ _cameraMatrix.convertTo(A, CV_64F);
+ if( _distCoeffs.data )
+ distCoeffs = Mat_<double>(_distCoeffs);
+ else
+ {
+ distCoeffs.create(5, 1);
+ distCoeffs = 0.;
+ }
+
+ if( _newCameraMatrix.data )
+ _newCameraMatrix.convertTo(Ar, CV_64F);
+ else
+ A.copyTo(Ar);
+
+ double v0 = Ar(1, 2);
+ for( int y = 0; y < src.rows; y += stripe_size0 )
+ {
+ int stripe_size = std::min( stripe_size0, src.rows - y );
+ Ar(1, 2) = v0 - y;
+ Mat map1_part = map1.rowRange(0, stripe_size),
+ map2_part = map2.rowRange(0, stripe_size),
+ dst_part = dst.rowRange(y, y + stripe_size);
+
+ initUndistortRectifyMap( A, distCoeffs, I, Ar, Size(src.cols, stripe_size),
+ map1_part.type(), map1_part, map2_part );
+ remap( src, dst_part, map1_part, map2_part, INTER_LINEAR, BORDER_REPLICATE );
+ }
+}
+
+}
+
+
+CV_IMPL void
+cvUndistort2( const CvArr* srcarr, CvArr* dstarr, const CvMat* Aarr, const CvMat* dist_coeffs )
+{
+ cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr), dst0 = dst;
+ cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs = cv::cvarrToMat(dist_coeffs);
+
+ CV_Assert( src.size() == dst.size() && src.type() == dst.type() );
+ cv::undistort( src, dst, A, distCoeffs, cv::Mat() );
+}
+
+
+CV_IMPL void cvInitUndistortMap( const CvMat* Aarr, const CvMat* dist_coeffs,
+ CvArr* mapxarr, CvArr* mapyarr )
+{
+ cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs = cv::cvarrToMat(dist_coeffs);
+ cv::Mat mapx = cv::cvarrToMat(mapxarr), mapy, mapx0 = mapx, mapy0;
+
+ if( mapyarr )
+ mapy0 = mapy = cv::cvarrToMat(mapyarr);
+
+ cv::initUndistortRectifyMap( A, distCoeffs, cv::Mat(), A,
+ mapx.size(), mapx.type(), mapx, mapy );
+ CV_Assert( mapx0.data == mapx.data && mapy0.data == mapy.data );
+}
+
+void
+cvInitUndistortRectifyMap( const CvMat* Aarr, const CvMat* dist_coeffs,
+ const CvMat *Rarr, const CvMat* ArArr, CvArr* mapxarr, CvArr* mapyarr )
+{
+ cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs, R, Ar;
+ cv::Mat mapx = cv::cvarrToMat(mapxarr), mapy, mapx0 = mapx, mapy0;
+
+ if( mapyarr )
+ mapy0 = mapy = cv::cvarrToMat(mapyarr);
+
+ if( dist_coeffs )
+ distCoeffs = cv::cvarrToMat(dist_coeffs);
+ if( Rarr )
+ R = cv::cvarrToMat(Rarr);
+ if( ArArr )
+ Ar = cv::cvarrToMat(ArArr);
+
+ cv::initUndistortRectifyMap( A, distCoeffs, R, Ar, mapx.size(), mapx.type(), mapx, mapy );
+ CV_Assert( mapx0.data == mapx.data && mapy0.data == mapy.data );
+}
+
+
+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, iters = 1;
+
+ 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) &&
+ _cameraMatrix->rows == 3 && _cameraMatrix->cols == 3 );
+
+ cvConvert( _cameraMatrix, &_A );
+
+ if( _distCoeffs )
+ {
+ CV_ASSERT( CV_IS_MAT(_distCoeffs) &&
+ (_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( _distCoeffs, &_Dk );
+ iters = 5;
+ }
+
+ 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 < iters; 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__;
+}
+
+
+void cv::undistortPoints( const Mat& src, Mat& dst,
+ const Mat& cameraMatrix, const Mat& distCoeffs,
+ const Mat& R, const Mat& P )
+{
+ CV_Assert( src.isContinuous() && src.depth() == CV_32F &&
+ ((src.rows == 1 && src.channels() == 2) || src.cols*src.channels() == 2));
+
+ dst.create(src.size(), src.type());
+ CvMat _src = src, _dst = dst, _cameraMatrix = cameraMatrix;
+ CvMat _R, _P, _distCoeffs, *pR=0, *pP=0, *pD=0;
+ if( R.data )
+ pR = &(_R = R);
+ if( P.data )
+ pP = &(_P = P);
+ if( distCoeffs.data )
+ pD = &(_distCoeffs = distCoeffs);
+ cvUndistortPoints(&_src, &_dst, &_cameraMatrix, pD, pR, pP);
+}
+
+void cv::undistortPoints( const Mat& src, std::vector<Point2f>& dst,
+ const Mat& cameraMatrix, const Mat& distCoeffs,
+ const Mat& R, const Mat& P )
+{
+ size_t sz = src.cols*src.rows*src.channels()/2;
+ CV_Assert( src.isContinuous() && src.depth() == CV_32F &&
+ ((src.rows == 1 && src.channels() == 2) || src.cols*src.channels() == 2));
+
+ dst.resize(sz);
+ CvMat _src = src, _dst = Mat(dst), _cameraMatrix = cameraMatrix;
+ CvMat _R, _P, _distCoeffs, *pR=0, *pP=0, *pD=0;
+ if( R.data )
+ pR = &(_R = R);
+ if( P.data )
+ pP = &(_P = P);
+ if( distCoeffs.data )
+ pD = &(_distCoeffs = distCoeffs);
+ cvUndistortPoints(&_src, &_dst, &_cameraMatrix, pD, pR, pP);
+}
+
+/* End of file */
projects / opencv / blobdiff