static CvStatus
icvUnDistort_8u_CnR( const uchar* src, int srcstep,
uchar* dst, int dststep, CvSize size,
const float* intrinsic_matrix,
const float* dist_coeffs, int cn )
{
int u, v, i;
float u0 = intrinsic_matrix[2], v0 = intrinsic_matrix[5];
float x0 = (size.width-1)*0.5f, y0 = (size.height-1)*0.5f;
float fx = intrinsic_matrix[0], fy = intrinsic_matrix[4];
float ifx = 1.f/fx, ify = 1.f/fy;
float k1 = dist_coeffs[0], k2 = dist_coeffs[1], k3 = dist_coeffs[4];
float p1 = dist_coeffs[2], p2 = dist_coeffs[3];
srcstep /= sizeof(src[0]);
dststep /= sizeof(dst[0]);
for( v = 0; v < size.height; v++, dst += dststep )
{
float y = (v - v0)*ify, y2 = y*y;
for( u = 0; u < size.width; u++ )
{
float x = (u - u0)*ifx, x2 = x*x, r2 = x2 + y2, _2xy = 2*x*y;
float kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2;
float _x = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + x0;
float _y = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + y0;
int ix = cvFloor(_x), iy = cvFloor(_y);
if( (unsigned)iy < (unsigned)(size.height - 1) &&
(unsigned)ix < (unsigned)(size.width - 1) )
{
const uchar* ptr = src + iy*srcstep + ix*cn;
_x -= ix; _y -= iy;
for( i = 0; i < cn; i++ )
{
float t0 = CV_8TO32F(ptr[i]), t1 = CV_8TO32F(ptr[i+srcstep]);
t0 += _x*(CV_8TO32F(ptr[i+cn]) - t0);
t1 += _x*(CV_8TO32F(ptr[i + srcstep + cn]) - t1);
dst[u*cn + i] = (uchar)cvRound(t0 + _y*(t1 - t0));
}
}
else
{
for( i = 0; i < cn; i++ )
dst[u*cn + i] = 0;
}
}
}
return CV_OK;
}
// convert data of specified type to CvScalar
void
cvRawDataToScalar( void* data, int flags, CvScalar* scalar )
{
CV_FUNCNAME( "cvRawDataToScalar" );
__BEGIN__;
int cn = CV_ARR_CN( flags );
assert( scalar && data );
assert( (unsigned)(cn - 1) < 4 );
memset( scalar->val, 0, sizeof(scalar->val));
switch( CV_ARR_DEPTH( flags ))
{
case CV_8U:
while( cn-- )
scalar->val[cn] = CV_8TO32F(((uchar*)data)[cn]);
break;
case CV_8S:
while( cn-- )
scalar->val[cn] = CV_8TO32F(((char*)data)[cn]);
break;
case CV_16S:
while( cn-- )
scalar->val[cn] = ((short*)data)[cn];
break;
case CV_32S:
while( cn-- )
scalar->val[cn] = ((int*)data)[cn];
break;
case CV_32F:
while( cn-- )
scalar->val[cn] = ((float*)data)[cn];
break;
case CV_64F:
while( cn-- )
scalar->val[cn] = ((double*)data)[cn];
break;
default:
assert(0);
CV_ERROR_FROM_CODE( CV_BadDepth );
}
__END__;
}
inline Vec3f multiply(const Vec3b& a, const Vec3b& b)
{
return Vec3f(
CV_8TO32F(a[0])*CV_8TO32F(b[0]),
CV_8TO32F(a[1])*CV_8TO32F(b[1]),
CV_8TO32F(a[2])*CV_8TO32F(b[2]));
}
CvStatus CV_STDCALL icvGetRectSubPix_8u32f_C1R
( const uchar* src, int src_step, CvSize src_size,
float* dst, int dst_step, CvSize win_size, CvPoint2D32f center )
{
CvPoint ip;
float a12, a22, b1, b2;
float a, b;
double s = 0;
int i, j;
center.x -= (win_size.width-1)*0.5f;
center.y -= (win_size.height-1)*0.5f;
ip.x = cvFloor( center.x );
ip.y = cvFloor( center.y );
if( win_size.width <= 0 || win_size.height <= 0 )
return CV_BADRANGE_ERR;
a = center.x - ip.x;
b = center.y - ip.y;
a = MAX(a,0.0001f);
a12 = a*(1.f-b);
a22 = a*b;
b1 = 1.f - b;
b2 = b;
s = (1. - a)/a;
src_step /= sizeof(src[0]);
dst_step /= sizeof(dst[0]);
if( 0 <= ip.x && ip.x + win_size.width < src_size.width &&
0 <= ip.y && ip.y + win_size.height < src_size.height )
{
// extracted rectangle is totally inside the image
src += ip.y * src_step + ip.x;
#if 0
if( icvCopySubpix_8u32f_C1R_p &&
icvCopySubpix_8u32f_C1R_p( src, src_step, dst,
dst_step*sizeof(dst[0]), win_size, a, b ) >= 0 )
return CV_OK;
#endif
for( ; win_size.height--; src += src_step, dst += dst_step )
{
float prev = (1 - a)*(b1*CV_8TO32F(src[0]) + b2*CV_8TO32F(src[src_step]));
for( j = 0; j < win_size.width; j++ )
{
float t = a12*CV_8TO32F(src[j+1]) + a22*CV_8TO32F(src[j+1+src_step]);
dst[j] = prev + t;
prev = (float)(t*s);
}
}
}
else
{
CvRect r;
src = (const uchar*)icvAdjustRect( src, src_step*sizeof(*src),
sizeof(*src), src_size, win_size,ip, &r);
for( i = 0; i < win_size.height; i++, dst += dst_step )
{
const uchar *src2 = src + src_step;
if( i < r.y || i >= r.height )
src2 -= src_step;
for( j = 0; j < r.x; j++ )
{
float s0 = CV_8TO32F(src[r.x])*b1 +
CV_8TO32F(src2[r.x])*b2;
dst[j] = (float)(s0);
}
if( j < r.width )
{
float prev = (1 - a)*(b1*CV_8TO32F(src[j]) + b2*CV_8TO32F(src2[j]));
for( ; j < r.width; j++ )
{
float t = a12*CV_8TO32F(src[j+1]) + a22*CV_8TO32F(src2[j+1]);
dst[j] = prev + t;
prev = (float)(t*s);
}
}
for( ; j < win_size.width; j++ )
{
float s0 = CV_8TO32F(src[r.width])*b1 +
CV_8TO32F(src2[r.width])*b2;
dst[j] = (float)(s0);
}
if( i < r.height )
src = src2;
}
}
return CV_OK;
}
inline double addw(uchar a, double alpha, double b, double beta)
{
return b*beta + CV_8TO32F(a)*alpha;
}
inline float addw(uchar a, float alpha, float b, float beta)
{
return b*beta + CV_8TO32F(a)*alpha;
}
inline float multiply(uchar a, uchar b) { return CV_8TO32F(a)*CV_8TO32F(b); }
inline Vec3d addw(const Vec3b& a, double alpha, const Vec3d& b, double beta)
{
return Vec3d(b[0]*beta + CV_8TO32F(a[0])*alpha,
b[1]*beta + CV_8TO32F(a[1])*alpha,
b[2]*beta + CV_8TO32F(a[2])*alpha);
}
inline Vec3f addw(const Vec3b& a, float alpha, const Vec3f& b, float beta)
{
return Vec3f(b[0]*beta + CV_8TO32F(a[0])*alpha,
b[1]*beta + CV_8TO32F(a[1])*alpha,
b[2]*beta + CV_8TO32F(a[2])*alpha);
}
static CvStatus
icvUnDistort_8u_CnR( const uchar* src, int srcstep,
uchar* dst, int dststep, CvSize size,
const float* intrinsic_matrix,
const float* dist_coeffs, int cn )
{
int u, v, i;
float u0 = intrinsic_matrix[2], v0 = intrinsic_matrix[5];
float fx = intrinsic_matrix[0], fy = intrinsic_matrix[4];
float _fx = 1.f/fx, _fy = 1.f/fy;
float k1 = dist_coeffs[0], k2 = dist_coeffs[1];
float p1 = dist_coeffs[2], p2 = dist_coeffs[3];
srcstep /= sizeof(src[0]);
dststep /= sizeof(dst[0]);
for( v = 0; v < size.height; v++, dst += dststep )
{
float y = (v - v0)*_fy;
float y2 = y*y;
float ky = 1 + (k1 + k2*y2)*y2;
float k2y = 2*k2*y2;
float _2p1y = 2*p1*y;
float _3p1y2 = 3*p1*y2;
float p2y2 = p2*y2;
for( u = 0; u < size.width; u++ )
{
float x = (u - u0)*_fx;
float x2 = x*x;
float kx = (k1 + k2*x2)*x2;
float d = kx + ky + k2y*x2;
float _u = fx*(x*(d + _2p1y) + p2y2 + (3*p2)*x2) + u0;
float _v = fy*(y*(d + (2*p2)*x) + _3p1y2 + p1*x2) + v0;
int iu = cvRound(_u*(1 << ICV_WARP_SHIFT));
int iv = cvRound(_v*(1 << ICV_WARP_SHIFT));
int ifx = iu & ICV_WARP_MASK;
int ify = iv & ICV_WARP_MASK;
iu >>= ICV_WARP_SHIFT;
iv >>= ICV_WARP_SHIFT;
float a0 = icvLinearCoeffs[ifx*2];
float a1 = icvLinearCoeffs[ifx*2 + 1];
float b0 = icvLinearCoeffs[ify*2];
float b1 = icvLinearCoeffs[ify*2 + 1];
if( (unsigned)iv < (unsigned)(size.height - 1) &&
(unsigned)iu < (unsigned)(size.width - 1) )
{
const uchar* ptr = src + iv*srcstep + iu*cn;
for( i = 0; i < cn; i++ )
{
float t0 = a1*CV_8TO32F(ptr[i]) + a0*CV_8TO32F(ptr[i+cn]);
float t1 = a1*CV_8TO32F(ptr[i+srcstep]) + a0*CV_8TO32F(ptr[i + srcstep + cn]);
dst[u*cn + i] = (uchar)cvRound(b1*t0 + b0*t1);
}
}
else
{
for( i = 0; i < cn; i++ )
dst[u*cn + i] = 0;
}
}
}
return CV_OK;
}
