cvundistort.cpp.svn-base
来自「非结构化路识别」· SVN-BASE 代码 · 共 1,555 行 · 第 1/4 页
SVN-BASE
1,555 行
// step - full width of each image in bytes
// size - ROI size of each image
// intrMatrix - matrix of the camera intrinsic parameters
// distCoeffs - vector of two distortion coefficients (k1 and k2)
// interToggle - interpolation toggle
//
// Returns: CV_NO_ERR or error code
//
// Notes: If interToggle=0, interpolation disabled;
// else bilinear interpolation is used.
//F*/
/*______________________________________________________________________________________*/
#define S 22
#define S2 11
#define FM (float)0x400000
IPCVAPI_IMPL( CvStatus, icvUnDistort1_8u_C1R, ( const uchar* src, int srcStep,
uchar* dst, int dstStep, CvSize size,
const float *intrMatrix,
const float *distCoeffs,
int interToggle ))
{
const float fm = FM;
const float a1 = 1.f / intrMatrix[0], b1 = 1.f / intrMatrix[4],
u0 = intrMatrix[2], v0 = intrMatrix[5], k1 = distCoeffs[0], k2 = distCoeffs[1];
float *x2;
float *du;
int u, v;
uchar buf[1];
if( size.width <= 0 || size.height <= 0 )
return CV_BADSIZE_ERR;
if( !src || !dst || !intrMatrix || !distCoeffs )
return CV_NULLPTR_ERR;
x2 = (float *) icvAlloc( sizeof( float ) * size.width );
if( x2 == NULL )
return CV_OUTOFMEM_ERR;
du = (float *) icvAlloc( sizeof( float ) * size.width );
if( du == NULL )
{
icvFree( (void **) &x2 );
return CV_OUTOFMEM_ERR;
}
memcpy( buf, src, 1 );
memset( (void*)src, 0, 1 );
if( !interToggle )
{
for( u = 0; u < size.width; u++ )
{
float w = u - u0;
float x = a1 * w;
x2[u] = x * x;
du[u] = w;
}
for( v = 0; v < size.height; v++, dst += dstStep )
{
float y = b1 * (v - v0);
float y2 = y * y;
float dv = v - v0;
for( u = 0; u < size.width; u++ )
{
float r2 = x2[u] + y2;
float dist = r2 * (k1 + r2 * k2);
int ud = u + cvRound( du[u] * dist );
int vd = v + cvRound( dv * dist );
dst[u] = (uchar) (ud < 0 || ud >= size.width || vd < 0 ||
vd >= size.height ? 0 : src[vd * srcStep + ud]);
}
}
}
else
{
int sizex = size.width - 2, sizey = size.height - 2;
for( u = 0; u < size.width; u++ )
{
float w = u - u0;
float x = a1 * w;
x2[u] = x * x;
du[u] = fm * w;
}
for( v = 0; v < size.height; v++, dst += dstStep )
{
float y = b1 * (v - v0);
float y2 = y * y;
float dv = fm * (v - v0);
for( u = 0; u < size.width; u++ )
{
float r2 = x2[u] + y2;
float dist = r2 * (k1 + r2 * k2);
int iu = cvRound( du[u] * dist );
int iv = cvRound( dv * dist );
int ud = iu >> S;
int vd = iv >> S;
iu -= ud << S;
iv -= vd << S;
ud += u;
vd += v;
if( ud < 0 || ud > sizex || vd < 0 || vd > sizey )
dst[u] = 0;
else
{
int iuv = (iu >> S2) * (iv >> S2);
int uv = vd * srcStep + ud;
int a0 = src[uv];
int a = src[uv + 1] - a0;
int b = src[uv + srcStep] - a0;
int c = src[uv + srcStep + 1] - a0 - a - b;
int d = ((a * iu + b * iv + c * iuv) >> S) + a0;
d = !(d & ~255) ? d : d < 0 ? 0 : 255;
dst[u] = (uchar) d;
}
}
}
}
memcpy( (void*)src, buf, 1 );
icvFree( (void **) &x2 );
icvFree( (void **) &du );
return CV_NO_ERR;
}
/*______________________________________________________ 3-CHANNEL IMAGES ______________*/
IPCVAPI_IMPL( CvStatus, icvUnDistort1_8u_C3R, ( const uchar* src, int srcStep,
uchar* dst, int dstStep, CvSize size,
const float *intrMatrix,
const float *distCoeffs,
int interToggle ))
{
const float fm = FM;
const float a1 = 1.f / intrMatrix[0], b1 = 1.f / intrMatrix[4],
u0 = intrMatrix[2], v0 = intrMatrix[5], k1 = distCoeffs[0], k2 = distCoeffs[1];
const int p = 0xFFFFFF;
float *x2;
float *du;
int u, v;
uchar buf[3];
if( size.width <= 0 || size.height <= 0 )
return CV_BADSIZE_ERR;
if( !src || !dst || !intrMatrix || !distCoeffs )
return CV_NULLPTR_ERR;
x2 = (float *) icvAlloc( sizeof( float ) * size.width );
if( x2 == NULL )
return CV_OUTOFMEM_ERR;
du = (float *) icvAlloc( sizeof( float ) * size.width );
if( du == NULL )
{
icvFree( (void **) &x2 );
return CV_OUTOFMEM_ERR;
}
memcpy( buf, src, 3 );
memset( (void*)src, 0, 3 );
if( !interToggle )
{
for( u = 0; u < size.width; u++ )
{
float w = u - u0;
float x = a1 * w;
x2[u] = x * x;
du[u] = w;
}
for( v = 0; v < size.height; v++, dst += dstStep )
{
float y = b1 * (v - v0);
float y2 = y * y;
float dv = v - v0;
if( v == size.height - 1 )
size.width--;
for( u = 0; u < size.width; u++ )
{
float r2 = x2[u] + y2;
float dist = r2 * (k1 + r2 * k2);
int ud = u + cvRound( du[u] * dist );
int vd = v + cvRound( dv * dist );
int u3 = u + u + u;
if( ud < 0 || ud >= size.width || vd < 0 || vd >= size.height )
*(int *) (dst + u3) = 0;
else
{
int uv = vd * srcStep + ud + ud + ud;
*(int *) (dst + u3) = *(int *) (src + uv) & p;
}
}
if( v == size.height - 1 )
{
float r2 = x2[u] + y2;
float dist = r2 * (k1 + r2 * k2);
int ud = u + cvRound( du[u] * dist );
int vd = v + cvRound( dv * dist );
int u3 = u + u + u;
size.width++;
if( ud < 0 || ud >= size.width || vd < 0 || vd >= size.height )
dst[u3] = dst[u3 + 1] = dst[u3 + 2] = 0;
else
{
int uv = vd * srcStep + ud + ud + ud;
dst[u3] = src[uv];
dst[u3 + 1] = src[uv + 1];
dst[u3 + 2] = src[uv + 2];
}
}
}
}
else /* interpolation */
{
int sizex = size.width - 2, sizey = size.height - 2;
for( u = 0; u < size.width; u++ )
{
float w = u - u0;
float x = a1 * w;
x2[u] = x * x;
du[u] = fm * w;
}
for( v = 0; v < size.height; v++, dst += dstStep )
{
float y = b1 * (v - v0);
float y2 = y * y;
float dv = fm * (v - v0);
if( v == size.height - 1 )
size.width--;
for( u = 0; u < size.width; u++ )
{
float r2 = x2[u] + y2;
float dist = r2 * (k1 + r2 * k2);
int iu = cvRound( du[u] * dist );
int iv = cvRound( dv * dist );
int ud = iu >> S;
int vd = iv >> S;
int u3 = u + u + u;
iu -= ud << S;
iv -= vd << S;
ud += u;
vd += v;
if( ud < 0 || ud > sizex || vd < 0 || vd > sizey )
*(int *) (dst + u3) = 0;
else
{
int iuv = (iu >> S2) * (iv >> S2);
int uv = vd * srcStep + ud + ud + ud;
int uvs = uv + srcStep;
int a01 = src[uv];
int a02 = src[uv + 1];
int a03 = src[uv + 2];
int a1 = src[uv + 3];
int a2 = src[uv + 4];
int a3 = src[uv + 5];
int b1 = src[uvs];
int b2 = src[uvs + 1];
int b3 = src[uvs + 2];
int c1 = src[uvs + 3];
int c2 = src[uvs + 4];
int c3 = src[uvs + 5];
int d1, d2, d3;
a1 -= a01;
a2 -= a02;
a3 -= a03;
b1 -= a01;
b2 -= a02;
b3 -= a03;
c1 -= a01 + a1 + b1;
c2 -= a02 + a2 + b2;
c3 -= a03 + a3 + b3;
d1 = ((a1 * iu + b1 * iv + c1 * iuv) >> S) + a01;
d2 = ((a2 * iu + b2 * iv + c2 * iuv) >> S) + a02;
d3 = ((a3 * iu + b3 * iv + c3 * iuv) >> S) + a03;
d1 = !(d1 & ~255) ? d1 : d1 < 0 ? 0 : 255;
d2 = !(d2 & ~255) ? d2 : d2 < 0 ? 0 : 255;
d3 = !(d3 & ~255) ? d3 : d3 < 0 ? 0 : 255;
d1 |= (d2 << 8) | (d3 << 16);
*(int *) (dst + u3) = d1;
}
}
if( v == size.height - 1 )
{
float r2 = x2[u] + y2;
float dist = r2 * (k1 + r2 * k2);
int iu = cvRound( du[u] * dist );
int iv = cvRound( dv * dist );
int ud = iu >> S;
int vd = iv >> S;
int u3 = u + u + u;
size.width++;
iu -= ud << S;
iv -= vd << S;
ud += u;
vd += v;
if( ud < 0 || ud > sizex || vd < 0 || vd > sizey )
dst[u3] = dst[u3 + 1] = dst[u3 + 2] = 0;
else
{
int iuv = (iu >> S2) * (iv >> S2);
int uv = vd * srcStep + ud + ud + ud;
int uvs = uv + srcStep;
int a01 = src[uv];
int a02 = src[uv + 1];
int a03 = src[uv + 2];
int a1 = src[uv + 3];
int a2 = src[uv + 4];
int a3 = src[uv + 5];
int b1 = src[uvs];
int b2 = src[uvs + 1];
int b3 = src[uvs + 2];
int c1 = src[uvs + 3];
int c2 = src[uvs + 4];
int c3 = src[uvs + 5];
int d1, d2, d3;
a1 -= a01;
a2 -= a02;
a3 -= a03;
b1 -= a01;
b2 -= a02;
b3 -= a03;
c1 -= a01 + a1 + b1;
c2 -= a02 + a2 + b2;
c3 -= a03 + a3 + b3;
d1 = ((a1 * iu + b1 * iv + c1 * iuv) >> S) + a01;
d2 = ((a2 * iu + b2 * iv + c2 * iuv) >> S) + a02;
d3 = ((a3 * iu + b3 * iv + c3 * iuv) >> S) + a03;
dst[u3] = (uchar) d1;
dst[u3 + 1] = (uchar) d2;
dst[u3 + 2] = (uchar) d3;
}
}
}
}
memcpy( (void*)src, buf, 1 );
icvFree( (void **) &x2 );
icvFree( (void **) &du );
return CV_NO_ERR;
}
/*======================================================================================*/
/*F//////////////////////////////////////////////////////////////////////////////////////
// Names: icvUnDistortEx_8uC1R, icvUnDistortEx_8uC3R
// Purpose: The functions correct radial and tangential image distortions using known
// matrix of the camera intrinsic parameters and distortion coefficients
// Context:
// Parameters: src - source (distorted) image
// dst - output (undistorted) image
// step - full width of each image in bytes
// size - ROI size of each image
// intrMatrix - matrix of the camera intrinsic parameters
// distCoeffs - vector of the distortion coefficients
// (k1, k2, p1, p2)
// interToggle - interpolation toggle
//
// Returns: CV_NO_ERR or error code
//
⌨️ 快捷键说明
复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?