cvundistort.cpp.svn-base
来自「非结构化路识别」· SVN-BASE 代码 · 共 1,555 行 · 第 1/4 页
SVN-BASE
1,555 行
// Notes: If interToggle=0, interpolation disabled;
// else bilinear interpolation is used.
//F*/
/*______________________________________________________________________________________*/
IPCVAPI_IMPL( CvStatus, icvUnDistortEx_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], p1 = distCoeffs[2], p2 = distCoeffs[3];
float *x1;
float *x2;
float *x3;
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;
/*if ( !p1 && !p2 ) return icvUnDistort1_8uC1R ( src, dst, step, size,
intrMatrix, distCoeffs, interToggle ); */
x1 = (float *) icvAlloc( sizeof( float ) * size.width );
x2 = (float *) icvAlloc( sizeof( float ) * size.width );
x3 = (float *) icvAlloc( sizeof( float ) * size.width );
du = (float *) icvAlloc( sizeof( float ) * size.width );
if( x1 == NULL || x2 == NULL || x3 == NULL || du == NULL )
{
if( x1 )
icvFree( (void **) &x1 );
if( x2 )
icvFree( (void **) &x2 );
if( x3 )
icvFree( (void **) &x3 );
if( du )
icvFree( (void **) &du );
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;
x1[u] = p2 / x;
x2[u] = x * x;
x3[u] = 2.f * p2 * x;
du[u] = w;
}
for( v = 0; v < size.height; v++, dst += dstStep )
{
float dv = v - v0;
float y = b1 * dv;
float y1 = p1 / y;
float y2 = y * y;
float y3 = 2.f * p1 * y;
for( u = 0; u < size.width; u++ )
{
float r2 = x2[u] + y2;
float bx = r2 * (k1 + r2 * k2) + x3[u] + y3;
float by = bx + r2 * y1;
int ud = u, vd = v;
bx += r2 * x1[u];
ud += cvRound( bx * du[u] );
vd += cvRound( by * dv );
dst[u] = (uchar) (ud < 0 || ud >= size.width || vd < 0 ||
vd >= size.height ? 0 : src[vd * srcStep + ud]);
}
}
}
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;
x1[u] = p2 / x;
x2[u] = x * x;
x3[u] = 2.f * p2 * x;
du[u] = fm * w;
}
for( v = 0; v < size.height; v++, dst += dstStep )
{
float dv = v - v0;
float y = b1 * dv;
float y1 = p1 / y;
float y2 = y * y;
float y3 = 2.f * p1 * y;
dv *= fm;
for( u = 0; u < size.width; u++ )
{
float r2 = x2[u] + y2;
float bx = r2 * (k1 + r2 * k2) + x3[u] + y3;
float by = bx + r2 * y1;
int iu, iv, ud, vd;
bx += r2 * x1[u];
iu = cvRound( bx * du[u] );
iv = cvRound( by * dv );
ud = iu >> S;
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 **) &x1 );
icvFree( (void **) &x2 );
icvFree( (void **) &x3 );
icvFree( (void **) &du );
return CV_NO_ERR;
}
/*______________________________________________________ 3-CHANNEL IMAGES ______________*/
IPCVAPI_IMPL( CvStatus, icvUnDistortEx_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], p1 = distCoeffs[2], p2 = distCoeffs[3];
const int p = 0xFFFFFF;
float *x1;
float *x2;
float *x3;
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;
/*if ( !p1 && !p2 ) return icvUnDistort1_8uC3R ( src, dst, srcStep, size,
intrMatrix, distCoeffs, interToggle ); */
x1 = (float *) icvAlloc( sizeof( float ) * size.width );
x2 = (float *) icvAlloc( sizeof( float ) * size.width );
x3 = (float *) icvAlloc( sizeof( float ) * size.width );
du = (float *) icvAlloc( sizeof( float ) * size.width );
if( x1 == NULL || x2 == NULL || x3 == NULL || du == NULL )
{
if( x1 )
icvFree( (void **) &x1 );
if( x2 )
icvFree( (void **) &x2 );
if( x3 )
icvFree( (void **) &x3 );
if( du )
icvFree( (void **) &du );
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;
x1[u] = p2 / x;
x2[u] = x * x;
x3[u] = 2.f * p2 * x;
du[u] = w;
}
for( v = 0; v < size.height; v++, dst += dstStep )
{
float dv = v - v0;
float y = b1 * dv;
float y1 = p1 / y;
float y2 = y * y;
float y3 = 2.f * p1 * y;
if( v == size.height - 1 )
size.width--;
for( u = 0; u < size.width; u++ )
{
float r2 = x2[u] + y2;
float bx = r2 * (k1 + r2 * k2) + x3[u] + y3;
float by = bx + r2 * y1;
int ud = u, vd = v, u3 = u + u + u;
bx += r2 * x1[u];
ud += cvRound( bx * du[u] );
vd += cvRound( by * dv );
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 bx = r2 * (k1 + r2 * k2) + x3[u] + y3;
float by = bx + r2 * y1;
int ud = u, vd = v, u3 = u + u + u;
bx += r2 * x1[u];
ud += cvRound( bx * du[u] );
vd += cvRound( by * dv );
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;
x1[u] = p2 / x;
x2[u] = x * x;
x3[u] = 2.f * p2 * x;
du[u] = fm * w;
}
for( v = 0; v < size.height; v++, dst += dstStep )
{
float dv = v - v0;
float y = b1 * dv;
float y1 = p1 / y;
float y2 = y * y;
float y3 = 2.f * p1 * y;
dv *= fm;
if( v == size.height - 1 )
size.width--;
for( u = 0; u < size.width; u++ )
{
float r2 = x2[u] + y2;
float bx = r2 * (k1 + r2 * k2) + x3[u] + y3;
float by = bx + r2 * y1;
int iu, iv, ud, vd, u3 = u + u + u;
bx += r2 * x1[u];
iu = cvRound( bx * du[u] );
iv = cvRound( by * dv );
ud = iu >> S;
vd = iv >> S;
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 bx = r2 * (k1 + r2 * k2) + x3[u] + y3;
float by = bx + r2 * y1;
int iu, iv, ud, vd, u3 = u + u + u;
bx += r2 * x1[u];
iu = cvRound( bx * du[u] );
iv = cvRound( by * dv );
ud = iu >> S;
vd = iv >> S;
iu -= ud << S;
iv -= vd << S;
ud += u;
vd += v;
size.width++;
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];
⌨️ 快捷键说明
复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?