411 lines
14 KiB
C++
411 lines
14 KiB
C++
|
/*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 "precomp.hpp"
|
||
|
|
|
||
|
|
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& matR, 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( matR.data )
|
||
|
|
R = Mat_<double>(matR);
|
||
|
|
|
||
|
|
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() );
|
||
|
|
CV_Assert( dst.data != src.data );
|
||
|
|
|
||
|
|
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_CONSTANT );
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
CV_IMPL void
|
||
|
|
cvUndistort2( const CvArr* srcarr, CvArr* dstarr, const CvMat* Aarr, const CvMat* dist_coeffs, const CvMat* newAarr )
|
||
|
|
{
|
||
|
|
cv::Mat src = cv::cvarrToMat(srcarr), dst = cv::cvarrToMat(dstarr), dst0 = dst;
|
||
|
|
cv::Mat A = cv::cvarrToMat(Aarr), distCoeffs = cv::cvarrToMat(dist_coeffs), newA;
|
||
|
|
if( newAarr )
|
||
|
|
newA = cv::cvarrToMat(newAarr);
|
||
|
|
|
||
|
|
CV_Assert( src.size() == dst.size() && src.type() == dst.type() );
|
||
|
|
cv::undistort( src, dst, A, distCoeffs, newA );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
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* matR, const CvMat* matP )
|
||
|
|
{
|
||
|
|
double A[3][3], RR[3][3], k[5]={0,0,0,0,0}, fx, fy, ifx, ify, cx, cy;
|
||
|
|
CvMat matA=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) &&
|
||
|
|
_src->cols + _src->rows - 1 == _dst->rows + _dst->cols - 1 &&
|
||
|
|
(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, &matA );
|
||
|
|
|
||
|
|
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( matR )
|
||
|
|
{
|
||
|
|
CV_Assert( CV_IS_MAT(matR) && matR->rows == 3 && matR->cols == 3 );
|
||
|
|
cvConvert( matR, &_RR );
|
||
|
|
}
|
||
|
|
else
|
||
|
|
cvSetIdentity(&_RR);
|
||
|
|
|
||
|
|
if( matP )
|
||
|
|
{
|
||
|
|
double PP[3][3];
|
||
|
|
CvMat _P3x3, _PP=cvMat(3, 3, CV_64F, PP);
|
||
|
|
CV_Assert( CV_IS_MAT(matP) && matP->rows == 3 && (matP->cols == 3 || matP->cols == 4));
|
||
|
|
cvConvert( cvGetCols(matP, &_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;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
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 matR, matP, _distCoeffs, *pR=0, *pP=0, *pD=0;
|
||
|
|
if( R.data )
|
||
|
|
pR = &(matR = R);
|
||
|
|
if( P.data )
|
||
|
|
pP = &(matP = 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 matR, matP, _distCoeffs, *pR=0, *pP=0, *pD=0;
|
||
|
|
if( R.data )
|
||
|
|
pR = &(matR = R);
|
||
|
|
if( P.data )
|
||
|
|
pP = &(matP = P);
|
||
|
|
if( distCoeffs.data )
|
||
|
|
pD = &(_distCoeffs = distCoeffs);
|
||
|
|
cvUndistortPoints(&_src, &_dst, &_cameraMatrix, pD, pR, pP);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* End of file */
|