// Optimal K is computed as avg_x(k-th-smallest_d(||I(x)-J(x+d)||)),
// where k = number_of_disparities*0.25.
static float
icvComputeK( CvStereoGCState* state )
{
int x, y, x1, d, i, j, rows = state->left->rows, cols = state->left->cols, n = 0;
int mind = state->minDisparity, nd = state->numberOfDisparities, maxd = mind + nd;
int k = MIN(MAX((nd + 2)/4, 3), nd);
int *arr = (int*)cvStackAlloc(k*sizeof(arr[0])), delta, t, sum = 0;
for( y = 0; y < rows; y++ )
{
const uchar* lptr = state->left->data.ptr + state->left->step*y;
const uchar* rptr = state->right->data.ptr + state->right->step*y;
for( x = 0; x < cols; x++ )
{
for( d = maxd-1, i = 0; d >= mind; d-- )
{
x1 = x - d;
if( (unsigned)x1 >= (unsigned)cols )
continue;
delta = icvDataCostFuncGraySubpix( lptr + x*3, rptr + x1*3 );
if( i < k )
arr[i++] = delta;
else
for( i = 0; i < k; i++ )
if( delta < arr[i] )
CV_SWAP( arr[i], delta, t );
}
delta = arr[0];
for( j = 1; j < i; j++ )
delta = MAX(delta, arr[j]);
sum += delta;
n++;
}
}
return (float)sum/n;
}
static void
icvSVD_32f( float* a, int lda, int m, int n,
float* w,
float* uT, int lduT, int nu,
float* vT, int ldvT,
float* buffer )
{
float* e;
float* temp;
float *w1, *e1;
float *hv;
double ku0 = 0, kv0 = 0;
double anorm = 0;
float *a1, *u0 = uT, *v0 = vT;
double scale, h;
int i, j, k, l;
int nm, m1, n1;
int nv = n;
int iters = 0;
float* hv0 = (float*)cvStackAlloc( (m+2)*sizeof(hv0[0])) + 1;
e = buffer;
w1 = w;
e1 = e + 1;
nm = n;
temp = buffer + nm;
memset( w, 0, nm * sizeof( w[0] ));
memset( e, 0, nm * sizeof( e[0] ));
m1 = m;
n1 = n;
/* transform a to bi-diagonal form */
for( ;; )
{
int update_u;
int update_v;
if( m1 == 0 )
break;
scale = h = 0;
update_u = uT && m1 > m - nu;
hv = update_u ? uT : hv0;
for( j = 0, a1 = a; j < m1; j++, a1 += lda )
{
double t = a1[0];
scale += fabs( hv[j] = (float)t );
}
if( scale != 0 )
{
double f = 1./scale, g, s = 0;
for( j = 0; j < m1; j++ )
{
double t = (hv[j] = (float)(hv[j]*f));
s += t * t;
}
g = sqrt( s );
f = hv[0];
if( f >= 0 )
g = -g;
hv[0] = (float)(f - g);
h = 1. / (f * g - s);
memset( temp, 0, n1 * sizeof( temp[0] ));
/* calc temp[0:n-i] = a[i:m,i:n]'*hv[0:m-i] */
icvMatrAXPY_32f( m1, n1 - 1, a + 1, lda, hv, temp + 1, 0 );
for( k = 1; k < n1; k++ ) temp[k] = (float)(temp[k]*h);
/* modify a: a[i:m,i:n] = a[i:m,i:n] + hv[0:m-i]*temp[0:n-i]' */
icvMatrAXPY_32f( m1, n1 - 1, temp + 1, 0, hv, a + 1, lda );
*w1 = (float)(g*scale);
}
w1++;
/* store -2/(hv'*hv) */
if( update_u )
{
if( m1 == m )
ku0 = h;
else
hv[-1] = (float)h;
}
a++;
n1--;
if( vT )
vT += ldvT + 1;
if( n1 == 0 )
break;
scale = h = 0;
update_v = vT && n1 > n - nv;
hv = update_v ? vT : hv0;
for( j = 0; j < n1; j++ )
{
double t = a[j];
scale += fabs( hv[j] = (float)t );
}
if( scale != 0 )
{
double f = 1./scale, g, s = 0;
for( j = 0; j < n1; j++ )
{
double t = (hv[j] = (float)(hv[j]*f));
s += t * t;
}
g = sqrt( s );
f = hv[0];
if( f >= 0 )
g = -g;
hv[0] = (float)(f - g);
h = 1. / (f * g - s);
hv[-1] = 0.f;
/* update a[i:m:i+1:n] = a[i:m,i+1:n] + (a[i:m,i+1:n]*hv[0:m-i])*... */
icvMatrAXPY3_32f( m1, n1, hv, lda, a, h );
*e1 = (float)(g*scale);
}
e1++;
/* store -2/(hv'*hv) */
if( update_v )
{
if( n1 == n )
kv0 = h;
else
hv[-1] = (float)h;
}
a += lda;
m1--;
if( uT )
uT += lduT + 1;
}
m1 -= m1 != 0;
n1 -= n1 != 0;
/* accumulate left transformations */
if( uT )
{
m1 = m - m1;
uT = u0 + m1 * lduT;
for( i = m1; i < nu; i++, uT += lduT )
{
memset( uT + m1, 0, (m - m1) * sizeof( uT[0] ));
uT[i] = 1.;
}
for( i = m1 - 1; i >= 0; i-- )
{
double s;
int lh = nu - i;
l = m - i;
hv = u0 + (lduT + 1) * i;
h = i == 0 ? ku0 : hv[-1];
assert( h <= 0 );
if( h != 0 )
{
uT = hv;
icvMatrAXPY3_32f( lh, l-1, hv+1, lduT, uT+1, h );
s = hv[0] * h;
for( k = 0; k < l; k++ ) hv[k] = (float)(hv[k]*s);
hv[0] += 1;
}
else
{
for( j = 1; j < l; j++ )
hv[j] = 0;
for( j = 1; j < lh; j++ )
hv[j * lduT] = 0;
hv[0] = 1;
}
}
uT = u0;
}
/* accumulate right transformations */
if( vT )
{
n1 = n - n1;
vT = v0 + n1 * ldvT;
for( i = n1; i < nv; i++, vT += ldvT )
{
memset( vT + n1, 0, (n - n1) * sizeof( vT[0] ));
vT[i] = 1.;
}
for( i = n1 - 1; i >= 0; i-- )
{
double s;
int lh = nv - i;
l = n - i;
hv = v0 + (ldvT + 1) * i;
h = i == 0 ? kv0 : hv[-1];
assert( h <= 0 );
if( h != 0 )
{
vT = hv;
icvMatrAXPY3_32f( lh, l-1, hv+1, ldvT, vT+1, h );
s = hv[0] * h;
for( k = 0; k < l; k++ ) hv[k] = (float)(hv[k]*s);
hv[0] += 1;
}
else
{
for( j = 1; j < l; j++ )
hv[j] = 0;
for( j = 1; j < lh; j++ )
hv[j * ldvT] = 0;
hv[0] = 1;
}
}
vT = v0;
}
for( i = 0; i < nm; i++ )
{
double tnorm = fabs( w[i] );
tnorm += fabs( e[i] );
if( anorm < tnorm )
anorm = tnorm;
}
anorm *= FLT_EPSILON;
/* diagonalization of the bidiagonal form */
for( k = nm - 1; k >= 0; k-- )
{
double z = 0;
iters = 0;
for( ;; ) /* do iterations */
{
double c, s, f, g, x, y;
int flag = 0;
/* test for splitting */
for( l = k; l >= 0; l-- )
{
if( fabs( e[l] ) <= anorm )
{
flag = 1;
break;
}
assert( l > 0 );
if( fabs( w[l - 1] ) <= anorm )
break;
}
if( !flag )
{
c = 0;
s = 1;
for( i = l; i <= k; i++ )
{
f = s * e[i];
e[i] = (float)(e[i]*c);
if( anorm + fabs( f ) == anorm )
break;
g = w[i];
h = pythag( f, g );
w[i] = (float)h;
c = g / h;
s = -f / h;
if( uT )
icvGivens_32f( m, uT + lduT * (l - 1), uT + lduT * i, c, s );
}
}
z = w[k];
if( l == k || iters++ == MAX_ITERS )
break;
/* shift from bottom 2x2 minor */
x = w[l];
y = w[k - 1];
g = e[k - 1];
h = e[k];
f = 0.5 * (((g + z) / h) * ((g - z) / y) + y / h - h / y);
g = pythag( f, 1 );
if( f < 0 )
g = -g;
f = x - (z / x) * z + (h / x) * (y / (f + g) - h);
/* next QR transformation */
c = s = 1;
for( i = l + 1; i <= k; i++ )
{
g = e[i];
y = w[i];
h = s * g;
g *= c;
z = pythag( f, h );
e[i - 1] = (float)z;
c = f / z;
s = h / z;
f = x * c + g * s;
g = -x * s + g * c;
h = y * s;
y *= c;
if( vT )
icvGivens_32f( n, vT + ldvT * (i - 1), vT + ldvT * i, c, s );
z = pythag( f, h );
w[i - 1] = (float)z;
/* rotation can be arbitrary if z == 0 */
if( z != 0 )
{
c = f / z;
s = h / z;
}
f = c * g + s * y;
x = -s * g + c * y;
if( uT )
icvGivens_32f( m, uT + lduT * (i - 1), uT + lduT * i, c, s );
}
e[l] = 0;
e[k] = (float)f;
w[k] = (float)x;
} /* end of iteration loop */
if( iters > MAX_ITERS )
break;
if( z < 0 )
{
w[k] = (float)(-z);
if( vT )
{
for( j = 0; j < n; j++ )
vT[j + k * ldvT] = -vT[j + k * ldvT];
}
}
} /* end of diagonalization loop */
/* sort singular values and corresponding vectors */
for( i = 0; i < nm; i++ )
{
k = i;
for( j = i + 1; j < nm; j++ )
if( w[k] < w[j] )
k = j;
if( k != i )
{
float t;
CV_SWAP( w[i], w[k], t );
if( vT )
for( j = 0; j < n; j++ )
CV_SWAP( vT[j + ldvT*k], vT[j + ldvT*i], t );
if( uT )
for( j = 0; j < m; j++ )
CV_SWAP( uT[j + lduT*k], uT[j + lduT*i], t );
}
}
}
CV_IMPL void
cvSVBkSb( const CvArr* warr, const CvArr* uarr,
const CvArr* varr, const CvArr* barr,
CvArr* xarr, int flags )
{
uchar* buffer = 0;
int local_alloc = 0;
CV_FUNCNAME( "cvSVBkSb" );
__BEGIN__;
CvMat wstub, *w = (CvMat*)warr;
CvMat bstub, *b = (CvMat*)barr;
CvMat xstub, *x = (CvMat*)xarr;
CvMat ustub, ustub2, *u = (CvMat*)uarr;
CvMat vstub, vstub2, *v = (CvMat*)varr;
uchar* tw = 0;
int type;
int temp_u = 0, temp_v = 0;
int u_buf_offset = 0, v_buf_offset = 0, w_buf_offset = 0, t_buf_offset = 0;
int buf_size = 0, pix_size;
int m, n, nm;
int u_rows, u_cols;
int v_rows, v_cols;
if( !CV_IS_MAT( w ))
CV_CALL( w = cvGetMat( w, &wstub ));
if( !CV_IS_MAT( u ))
CV_CALL( u = cvGetMat( u, &ustub ));
if( !CV_IS_MAT( v ))
CV_CALL( v = cvGetMat( v, &vstub ));
if( !CV_IS_MAT( x ))
CV_CALL( x = cvGetMat( x, &xstub ));
if( !CV_ARE_TYPES_EQ( w, u ) || !CV_ARE_TYPES_EQ( w, v ) || !CV_ARE_TYPES_EQ( w, x ))
CV_ERROR( CV_StsUnmatchedFormats, "All matrices must have the same type" );
type = CV_MAT_TYPE( w->type );
pix_size = CV_ELEM_SIZE(type);
if( !(flags & CV_SVD_U_T) )
{
temp_u = 1;
u_buf_offset = buf_size;
buf_size += u->cols*u->rows*pix_size;
u_rows = u->rows;
u_cols = u->cols;
}
else
{
u_rows = u->cols;
u_cols = u->rows;
}
if( !(flags & CV_SVD_V_T) )
{
temp_v = 1;
v_buf_offset = buf_size;
buf_size += v->cols*v->rows*pix_size;
v_rows = v->rows;
v_cols = v->cols;
}
else
{
v_rows = v->cols;
v_cols = v->rows;
}
m = u_rows;
n = v_rows;
nm = MIN(n,m);
if( (u_rows != u_cols && v_rows != v_cols) || x->rows != v_rows )
CV_ERROR( CV_StsBadSize, "V or U matrix must be square" );
if( (w->rows == 1 || w->cols == 1) && w->rows + w->cols - 1 == nm )
{
if( CV_IS_MAT_CONT(w->type) )
tw = w->data.ptr;
else
{
w_buf_offset = buf_size;
buf_size += nm*pix_size;
}
}
else
{
if( w->cols != v_cols || w->rows != u_cols )
CV_ERROR( CV_StsBadSize, "W must be 1d array of MIN(m,n) elements or "
"matrix which size matches to U and V" );
w_buf_offset = buf_size;
buf_size += nm*pix_size;
}
if( b )
{
if( !CV_IS_MAT( b ))
CV_CALL( b = cvGetMat( b, &bstub ));
if( !CV_ARE_TYPES_EQ( w, b ))
CV_ERROR( CV_StsUnmatchedFormats, "All matrices must have the same type" );
if( b->cols != x->cols || b->rows != m )
CV_ERROR( CV_StsUnmatchedSizes, "b matrix must have (m x x->cols) size" );
}
else
{
b = &bstub;
memset( b, 0, sizeof(*b));
}
t_buf_offset = buf_size;
buf_size += (MAX(m,n) + b->cols)*pix_size;
if( buf_size <= CV_MAX_LOCAL_SIZE )
{
buffer = (uchar*)cvStackAlloc( buf_size );
local_alloc = 1;
}
else
CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
if( temp_u )
{
cvInitMatHeader( &ustub2, u_cols, u_rows, type, buffer + u_buf_offset );
cvT( u, &ustub2 );
u = &ustub2;
}
if( temp_v )
{
cvInitMatHeader( &vstub2, v_cols, v_rows, type, buffer + v_buf_offset );
cvT( v, &vstub2 );
v = &vstub2;
}
if( !tw )
{
int i, shift = w->cols > 1 ? pix_size : 0;
tw = buffer + w_buf_offset;
for( i = 0; i < nm; i++ )
memcpy( tw + i*pix_size, w->data.ptr + i*(w->step + shift), pix_size );
}
if( type == CV_32FC1 )
{
icvSVBkSb_32f( m, n, (float*)tw, u->data.fl, u->step/sizeof(float),
v->data.fl, v->step/sizeof(float),
b->data.fl, b->step/sizeof(float), b->cols,
x->data.fl, x->step/sizeof(float),
(float*)(buffer + t_buf_offset) );
}
else if( type == CV_64FC1 )
{
icvSVBkSb_64f( m, n, (double*)tw, u->data.db, u->step/sizeof(double),
v->data.db, v->step/sizeof(double),
b->data.db, b->step/sizeof(double), b->cols,
x->data.db, x->step/sizeof(double),
(double*)(buffer + t_buf_offset) );
}
else
{
CV_ERROR( CV_StsUnsupportedFormat, "" );
}
__END__;
if( buffer && !local_alloc )
cvFree( &buffer );
}
CV_IMPL void
cvSVD( CvArr* aarr, CvArr* warr, CvArr* uarr, CvArr* varr, int flags )
{
uchar* buffer = 0;
int local_alloc = 0;
CV_FUNCNAME( "cvSVD" );
__BEGIN__;
CvMat astub, *a = (CvMat*)aarr;
CvMat wstub, *w = (CvMat*)warr;
CvMat ustub, *u;
CvMat vstub, *v;
CvMat tmat;
uchar* tw = 0;
int type;
int a_buf_offset = 0, u_buf_offset = 0, buf_size, pix_size;
int temp_u = 0, /* temporary storage for U is needed */
t_svd; /* special case: a->rows < a->cols */
int m, n;
int w_rows, w_cols;
int u_rows = 0, u_cols = 0;
int w_is_mat = 0;
if( !CV_IS_MAT( a ))
CV_CALL( a = cvGetMat( a, &astub ));
if( !CV_IS_MAT( w ))
CV_CALL( w = cvGetMat( w, &wstub ));
if( !CV_ARE_TYPES_EQ( a, w ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( a->rows >= a->cols )
{
m = a->rows;
n = a->cols;
w_rows = w->rows;
w_cols = w->cols;
t_svd = 0;
}
else
{
CvArr* t;
CV_SWAP( uarr, varr, t );
flags = (flags & CV_SVD_U_T ? CV_SVD_V_T : 0)|
(flags & CV_SVD_V_T ? CV_SVD_U_T : 0);
m = a->cols;
n = a->rows;
w_rows = w->cols;
w_cols = w->rows;
t_svd = 1;
}
u = (CvMat*)uarr;
v = (CvMat*)varr;
w_is_mat = w_cols > 1 && w_rows > 1;
if( !w_is_mat && CV_IS_MAT_CONT(w->type) && w_cols + w_rows - 1 == n )
tw = w->data.ptr;
if( u )
{
if( !CV_IS_MAT( u ))
CV_CALL( u = cvGetMat( u, &ustub ));
if( !(flags & CV_SVD_U_T) )
{
u_rows = u->rows;
u_cols = u->cols;
}
else
{
u_rows = u->cols;
u_cols = u->rows;
}
if( !CV_ARE_TYPES_EQ( a, u ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( u_rows != m || (u_cols != m && u_cols != n))
CV_ERROR( CV_StsUnmatchedSizes, !t_svd ? "U matrix has unappropriate size" :
"V matrix has unappropriate size" );
temp_u = (u_rows != u_cols && !(flags & CV_SVD_U_T)) || u->data.ptr==a->data.ptr;
if( w_is_mat && u_cols != w_rows )
CV_ERROR( CV_StsUnmatchedSizes, !t_svd ? "U and W have incompatible sizes" :
"V and W have incompatible sizes" );
}
else
{
u = &ustub;
u->data.ptr = 0;
u->step = 0;
}
if( v )
{
int v_rows, v_cols;
if( !CV_IS_MAT( v ))
CV_CALL( v = cvGetMat( v, &vstub ));
if( !(flags & CV_SVD_V_T) )
{
v_rows = v->rows;
v_cols = v->cols;
}
else
{
v_rows = v->cols;
v_cols = v->rows;
}
if( !CV_ARE_TYPES_EQ( a, v ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( v_rows != n || v_cols != n )
CV_ERROR( CV_StsUnmatchedSizes, t_svd ? "U matrix has unappropriate size" :
"V matrix has unappropriate size" );
if( w_is_mat && w_cols != v_cols )
CV_ERROR( CV_StsUnmatchedSizes, t_svd ? "U and W have incompatible sizes" :
"V and W have incompatible sizes" );
}
else
{
v = &vstub;
v->data.ptr = 0;
v->step = 0;
}
type = CV_MAT_TYPE( a->type );
pix_size = CV_ELEM_SIZE(type);
buf_size = n*2 + m;
if( !(flags & CV_SVD_MODIFY_A) )
{
a_buf_offset = buf_size;
buf_size += a->rows*a->cols;
}
if( temp_u )
{
u_buf_offset = buf_size;
buf_size += u->rows*u->cols;
}
buf_size *= pix_size;
if( buf_size <= CV_MAX_LOCAL_SIZE )
{
buffer = (uchar*)cvStackAlloc( buf_size );
local_alloc = 1;
}
else
{
CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
}
if( !(flags & CV_SVD_MODIFY_A) )
{
cvInitMatHeader( &tmat, m, n, type,
buffer + a_buf_offset*pix_size );
if( !t_svd )
cvCopy( a, &tmat );
else
cvT( a, &tmat );
a = &tmat;
}
if( temp_u )
{
cvInitMatHeader( &ustub, u_cols, u_rows, type, buffer + u_buf_offset*pix_size );
u = &ustub;
}
if( !tw )
tw = buffer + (n + m)*pix_size;
if( type == CV_32FC1 )
{
icvSVD_32f( a->data.fl, a->step/sizeof(float), a->rows, a->cols,
(float*)tw, u->data.fl, u->step/sizeof(float), u_cols,
v->data.fl, v->step/sizeof(float), (float*)buffer );
}
else if( type == CV_64FC1 )
{
icvSVD_64f( a->data.db, a->step/sizeof(double), a->rows, a->cols,
(double*)tw, u->data.db, u->step/sizeof(double), u_cols,
v->data.db, v->step/sizeof(double), (double*)buffer );
}
else
{
CV_ERROR( CV_StsUnsupportedFormat, "" );
}
if( tw != w->data.ptr )
{
int shift = w->cols != 1;
cvSetZero( w );
if( type == CV_32FC1 )
for( int i = 0; i < n; i++ )
((float*)(w->data.ptr + i*w->step))[i*shift] = ((float*)tw)[i];
else
for( int i = 0; i < n; i++ )
((double*)(w->data.ptr + i*w->step))[i*shift] = ((double*)tw)[i];
}
if( uarr )
{
if( !(flags & CV_SVD_U_T))
cvT( u, uarr );
else if( temp_u )
cvCopy( u, uarr );
/*CV_CHECK_NANS( uarr );*/
}
if( varr )
{
if( !(flags & CV_SVD_V_T))
cvT( v, varr );
/*CV_CHECK_NANS( varr );*/
}
CV_CHECK_NANS( w );
__END__;
if( buffer && !local_alloc )
cvFree( &buffer );
}
float
CvEM::predict( const CvMat* _sample, CvMat* _probs ) const
{
float* sample_data = 0;
void* buffer = 0;
int allocated_buffer = 0;
int cls = 0;
CV_FUNCNAME( "CvEM::predict" );
__BEGIN__;
int i, k, dims;
int nclusters;
int cov_mat_type = params.cov_mat_type;
double opt = FLT_MAX;
size_t size;
CvMat diff, expo;
dims = means->cols;
nclusters = params.nclusters;
CV_CALL( cvPreparePredictData( _sample, dims, 0, params.nclusters, _probs, &sample_data ));
// allocate memory and initializing headers for calculating
size = sizeof(double) * (nclusters + dims);
if( size <= CV_MAX_LOCAL_SIZE )
buffer = cvStackAlloc( size );
else
{
CV_CALL( buffer = cvAlloc( size ));
allocated_buffer = 1;
}
expo = cvMat( 1, nclusters, CV_64FC1, buffer );
diff = cvMat( 1, dims, CV_64FC1, (double*)buffer + nclusters );
// calculate the probabilities
for( k = 0; k < nclusters; k++ )
{
const double* mean_k = (const double*)(means->data.ptr + means->step*k);
const double* w = (const double*)(inv_eigen_values->data.ptr + inv_eigen_values->step*k);
double cur = log_weight_div_det->data.db[k];
CvMat* u = cov_rotate_mats[k];
// cov = u w u' --> cov^(-1) = u w^(-1) u'
if( cov_mat_type == COV_MAT_SPHERICAL )
{
double w0 = w[0];
for( i = 0; i < dims; i++ )
{
double val = sample_data[i] - mean_k[i];
cur += val*val*w0;
}
}
else
{
for( i = 0; i < dims; i++ )
diff.data.db[i] = sample_data[i] - mean_k[i];
if( cov_mat_type == COV_MAT_GENERIC )
cvGEMM( &diff, u, 1, 0, 0, &diff, CV_GEMM_B_T );
for( i = 0; i < dims; i++ )
{
double val = diff.data.db[i];
cur += val*val*w[i];
}
}
expo.data.db[k] = cur;
if( cur < opt )
{
cls = k;
opt = cur;
}
/* probability = (2*pi)^(-dims/2)*exp( -0.5 * cur ) */
}
if( _probs )
{
CV_CALL( cvConvertScale( &expo, &expo, -0.5 ));
CV_CALL( cvExp( &expo, &expo ));
if( _probs->cols == 1 )
CV_CALL( cvReshape( &expo, &expo, 0, nclusters ));
CV_CALL( cvConvertScale( &expo, _probs, 1./cvSum( &expo ).val[0] ));
}
__END__;
if( sample_data != _sample->data.fl )
cvFree( &sample_data );
if( allocated_buffer )
cvFree( &buffer );
return (float)cls;
}
CV_IMPL void cvFindStereoCorrespondenceGC(const CvArr* _left, const CvArr* _right,
CvArr* _dispLeft, CvArr* _dispRight, CvStereoGCState* state, int useDisparityGuess) {
CvStereoGCState2 state2;
state2.orphans = 0;
state2.maxOrphans = 0;
CvMat lstub, *left = cvGetMat(_left, &lstub);
CvMat rstub, *right = cvGetMat(_right, &rstub);
CvMat dlstub, *dispLeft = cvGetMat(_dispLeft, &dlstub);
CvMat drstub, *dispRight = cvGetMat(_dispRight, &drstub);
CvSize size;
int iter, i, nZeroExpansions = 0;
CvRNG rng = cvRNG(-1);
int* disp;
CvMat _disp;
int64 E;
CV_Assert(state != 0);
CV_Assert(CV_ARE_SIZES_EQ(left, right) && CV_ARE_TYPES_EQ(left, right) &&
CV_MAT_TYPE(left->type) == CV_8UC1);
CV_Assert(!dispLeft ||
(CV_ARE_SIZES_EQ(dispLeft, left) && CV_MAT_CN(dispLeft->type) == 1));
CV_Assert(!dispRight ||
(CV_ARE_SIZES_EQ(dispRight, left) && CV_MAT_CN(dispRight->type) == 1));
size = cvGetSize(left);
if (!state->left || state->left->width != size.width || state->left->height != size.height) {
int pcn = (int)(sizeof(GCVtx*) / sizeof(int));
int vcn = (int)(sizeof(GCVtx) / sizeof(int));
int ecn = (int)(sizeof(GCEdge) / sizeof(int));
cvReleaseMat(&state->left);
cvReleaseMat(&state->right);
cvReleaseMat(&state->ptrLeft);
cvReleaseMat(&state->ptrRight);
cvReleaseMat(&state->dispLeft);
cvReleaseMat(&state->dispRight);
state->left = cvCreateMat(size.height, size.width, CV_8UC3);
state->right = cvCreateMat(size.height, size.width, CV_8UC3);
state->dispLeft = cvCreateMat(size.height, size.width, CV_16SC1);
state->dispRight = cvCreateMat(size.height, size.width, CV_16SC1);
state->ptrLeft = cvCreateMat(size.height, size.width, CV_32SC(pcn));
state->ptrRight = cvCreateMat(size.height, size.width, CV_32SC(pcn));
state->vtxBuf = cvCreateMat(1, size.height * size.width * 2, CV_32SC(vcn));
state->edgeBuf = cvCreateMat(1, size.height * size.width * 12 + 16, CV_32SC(ecn));
}
if (!useDisparityGuess) {
cvSet(state->dispLeft, cvScalarAll(OCCLUDED));
cvSet(state->dispRight, cvScalarAll(OCCLUDED));
} else {
CV_Assert(dispLeft && dispRight);
cvConvert(dispLeft, state->dispLeft);
cvConvert(dispRight, state->dispRight);
}
state2.Ithreshold = state->Ithreshold;
state2.interactionRadius = state->interactionRadius;
state2.lambda = cvRound(state->lambda * DENOMINATOR);
state2.lambda1 = cvRound(state->lambda1 * DENOMINATOR);
state2.lambda2 = cvRound(state->lambda2 * DENOMINATOR);
state2.K = cvRound(state->K * DENOMINATOR);
icvInitStereoConstTabs();
icvInitGraySubpix(left, right, state->left, state->right);
disp = (int*)cvStackAlloc(state->numberOfDisparities * sizeof(disp[0]));
_disp = cvMat(1, state->numberOfDisparities, CV_32S, disp);
cvRange(&_disp, state->minDisparity, state->minDisparity + state->numberOfDisparities);
cvRandShuffle(&_disp, &rng);
if (state2.lambda < 0 && (state2.K < 0 || state2.lambda1 < 0 || state2.lambda2 < 0)) {
float L = icvComputeK(state) * 0.2f;
state2.lambda = cvRound(L * DENOMINATOR);
}
if (state2.K < 0) {
state2.K = state2.lambda * 5;
}
if (state2.lambda1 < 0) {
state2.lambda1 = state2.lambda * 3;
}
if (state2.lambda2 < 0) {
state2.lambda2 = state2.lambda;
}
icvInitStereoTabs(&state2);
E = icvComputeEnergy(state, &state2, !useDisparityGuess);
for (iter = 0; iter < state->maxIters; iter++) {
for (i = 0; i < state->numberOfDisparities; i++) {
int alpha = disp[i];
int64 Enew = icvAlphaExpand(E, -alpha, state, &state2);
if (Enew < E) {
nZeroExpansions = 0;
E = Enew;
} else if (++nZeroExpansions >= state->numberOfDisparities) {
break;
}
}
}
if (dispLeft) {
cvConvert(state->dispLeft, dispLeft);
}
if (dispRight) {
cvConvert(state->dispRight, dispRight);
}
cvFree(&state2.orphans);
}
static CvStatus CV_STDCALL
icvCopyReflect101Border_8u( const uchar* src, int srcstep, CvSize srcroi,
uchar* dst, int dststep, CvSize dstroi,
int top, int left, int cn )
{
const int isz = (int)sizeof(int);
int i, j, k, t, dj, tab_size, int_mode = 0;
const int* isrc = (const int*)src;
int* idst = (int*)dst, *tab;
if( (cn | srcstep | dststep | (size_t)src | (size_t)dst) % isz == 0 )
{
cn /= isz;
srcstep /= isz;
dststep /= isz;
int_mode = 1;
}
srcroi.width *= cn;
dstroi.width *= cn;
left *= cn;
tab_size = dstroi.width - srcroi.width;
tab = (int*)cvStackAlloc( tab_size*sizeof(tab[0]) );
if( srcroi.width == 1 )
{
for( k = 0; k < cn; k++ )
for( i = 0; i < tab_size; i += cn )
tab[i + k] = k + left;
}
else
{
j = dj = cn;
for( i = left - cn; i >= 0; i -= cn )
{
for( k = 0; k < cn; k++ )
tab[i + k] = j + k + left;
if( (unsigned)(j += dj) >= (unsigned)srcroi.width )
j -= 2*dj, dj = -dj;
}
j = srcroi.width - cn*2;
dj = -cn;
for( i = left; i < tab_size; i += cn )
{
for( k = 0; k < cn; k++ )
tab[i + k] = j + k + left;
if( (unsigned)(j += dj) >= (unsigned)srcroi.width )
j -= 2*dj, dj = -dj;
}
}
if( int_mode )
{
idst += top*dststep;
for( i = 0; i < srcroi.height; i++, isrc += srcstep, idst += dststep )
{
if( idst + left != isrc )
memcpy( idst + left, isrc, srcroi.width*sizeof(idst[0]) );
for( j = 0; j < left; j++ )
{
k = tab[j];
idst[j] = idst[k];
}
for( ; j < tab_size; j++ )
{
k = tab[j];
idst[j + srcroi.width] = idst[k];
}
}
isrc -= srcroi.height*srcstep;
idst -= (top + srcroi.height)*dststep;
}
else
{
dst += top*dststep;
for( i = 0; i < srcroi.height; i++, src += srcstep, dst += dststep )
{
if( dst + left != src )
memcpy( dst + left, src, srcroi.width );
for( j = 0; j < left; j++ )
{
k = tab[j];
dst[j] = dst[k];
}
for( ; j < tab_size; j++ )
{
k = tab[j];
dst[j + srcroi.width] = dst[k];
}
}
src -= srcroi.height*srcstep;
dst -= (top + srcroi.height)*dststep;
}
for( t = 0; t < 2; t++ )
{
int i1, i2, di;
if( t == 0 )
i1 = top-1, i2 = 0, di = -1, j = 1, dj = 1;
else
i1 = top+srcroi.height, i2=dstroi.height, di = 1, j = srcroi.height-2, dj = -1;
for( i = i1; (di > 0 && i < i2) || (di < 0 && i > i2); i += di )
{
if( int_mode )
{
const int* s = idst + i*dststep;
int* d = idst + (j+top)*dststep;
for( k = 0; k < dstroi.width; k++ )
d[k] = s[k];
}
else
{
const uchar* s = dst + i*dststep;
uchar* d = dst + (j+top)*dststep;
for( k = 0; k < dstroi.width; k++ )
d[k] = s[k];
}
if( (unsigned)(j += dj) >= (unsigned)srcroi.height )
j -= 2*dj, dj = -dj;
}
}
return CV_OK;
}
CV_IMPL void
cvFilter2D( const CvArr* _src, CvArr* _dst, const CvMat* _kernel, CvPoint anchor )
{
// below that approximate size OpenCV is faster
const int ipp_lower_limit = 20;
static CvFuncTable filter_tab;
static int inittab = 0;
CvFilterState *state = 0;
float* kernel_data = 0;
int local_alloc = 1;
CvMat* temp = 0;
CV_FUNCNAME( "cvFilter2D" );
__BEGIN__;
CvFilterFunc func = 0;
int coi1 = 0, coi2 = 0;
CvMat srcstub, *src = (CvMat*)_src;
CvMat dststub, *dst = (CvMat*)_dst;
CvSize size;
int type, depth;
int src_step, dst_step;
CvMat kernel_hdr;
const CvMat* kernel = _kernel;
if( !inittab )
{
icvInitFilterTab( &filter_tab );
inittab = 1;
}
CV_CALL( src = cvGetMat( src, &srcstub, &coi1 ));
CV_CALL( dst = cvGetMat( dst, &dststub, &coi2 ));
if( coi1 != 0 || coi2 != 0 )
CV_ERROR( CV_BadCOI, "" );
type = CV_MAT_TYPE( src->type );
if( !CV_ARE_SIZES_EQ( src, dst ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
if( !CV_ARE_TYPES_EQ( src, dst ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( !CV_IS_MAT(kernel) ||
(CV_MAT_TYPE(kernel->type) != CV_32F &&
CV_MAT_TYPE(kernel->type) != CV_64F ))
CV_ERROR( CV_StsBadArg, "kernel must be single-channel floating-point matrix" );
if( anchor.x == -1 && anchor.y == -1 )
anchor = cvPoint(kernel->cols/2,kernel->rows/2);
if( (unsigned)anchor.x >= (unsigned)kernel->cols ||
(unsigned)anchor.y >= (unsigned)kernel->rows )
CV_ERROR( CV_StsOutOfRange, "anchor point is out of kernel" );
if( CV_MAT_TYPE(kernel->type) != CV_32FC1 || !CV_IS_MAT_CONT(kernel->type) || icvFilter_8u_C1R_p )
{
int sz = kernel->rows*kernel->cols*sizeof(kernel_data[0]);
if( sz < CV_MAX_LOCAL_SIZE )
kernel_data = (float*)cvStackAlloc( sz );
else
{
CV_CALL( kernel_data = (float*)cvAlloc( sz ));
local_alloc = 0;
}
kernel_hdr = cvMat( kernel->rows, kernel->cols, CV_32F, kernel_data );
if( CV_MAT_TYPE(kernel->type) == CV_32FC1 )
cvCopy( kernel, &kernel_hdr );
else
cvConvertScale( kernel, &kernel_hdr, 1, 0 );
kernel = &kernel_hdr;
}
size = cvGetMatSize( src );
depth = CV_MAT_DEPTH(type);
src_step = src->step;
dst_step = dst->step ? dst->step : CV_STUB_STEP;
if( icvFilter_8u_C1R_p && (src->rows >= ipp_lower_limit || src->cols >= ipp_lower_limit) )
{
CvFilterIPPFunc ipp_func =
type == CV_8UC1 ? (CvFilterIPPFunc)icvFilter_8u_C1R_p :
type == CV_8UC3 ? (CvFilterIPPFunc)icvFilter_8u_C3R_p :
type == CV_8UC4 ? (CvFilterIPPFunc)icvFilter_8u_C4R_p :
type == CV_16SC1 ? (CvFilterIPPFunc)icvFilter_16s_C1R_p :
type == CV_16SC3 ? (CvFilterIPPFunc)icvFilter_16s_C3R_p :
type == CV_16SC4 ? (CvFilterIPPFunc)icvFilter_16s_C4R_p :
type == CV_32FC1 ? (CvFilterIPPFunc)icvFilter_32f_C1R_p :
type == CV_32FC3 ? (CvFilterIPPFunc)icvFilter_32f_C3R_p :
type == CV_32FC4 ? (CvFilterIPPFunc)icvFilter_32f_C4R_p : 0;
if( ipp_func )
{
CvSize el_size = { kernel->cols, kernel->rows };
CvPoint el_anchor = { el_size.width - anchor.x - 1, el_size.height - anchor.y - 1 };
int stripe_size = 1 << 16; // the optimal value may depend on CPU cache,
// overhead of current IPP code etc.
const uchar* shifted_ptr;
int i, j, y, dy = 0;
int temp_step;
// mirror the kernel around the center
for( i = 0; i < (el_size.height+1)/2; i++ )
{
float* top_row = kernel->data.fl + el_size.width*i;
float* bottom_row = kernel->data.fl + el_size.width*(el_size.height - i - 1);
for( j = 0; j < (el_size.width+1)/2; j++ )
{
float a = top_row[j], b = top_row[el_size.width - j - 1];
float c = bottom_row[j], d = bottom_row[el_size.width - j - 1];
top_row[j] = d;
top_row[el_size.width - j - 1] = c;
bottom_row[j] = b;
bottom_row[el_size.width - j - 1] = a;
}
}
CV_CALL( temp = icvIPPFilterInit( src, stripe_size, el_size ));
shifted_ptr = temp->data.ptr +
anchor.y*temp->step + anchor.x*CV_ELEM_SIZE(type);
temp_step = temp->step ? temp->step : CV_STUB_STEP;
for( y = 0; y < src->rows; y += dy )
{
dy = icvIPPFilterNextStripe( src, temp, y, el_size, anchor );
IPPI_CALL( ipp_func( shifted_ptr, temp_step,
dst->data.ptr + y*dst_step, dst_step, cvSize(src->cols, dy),
kernel->data.fl, el_size, el_anchor ));
}
EXIT;
}
}
CV_CALL( state = icvFilterInitAlloc( src->cols, cv32f, CV_MAT_CN(type),
cvSize(kernel->cols, kernel->rows), anchor,
kernel->data.ptr, ICV_GENERIC_KERNEL ));
if( CV_MAT_CN(type) == 2 )
CV_ERROR( CV_BadNumChannels, "Unsupported number of channels" );
func = (CvFilterFunc)(filter_tab.fn_2d[depth]);
if( !func )
CV_ERROR( CV_StsUnsupportedFormat, "" );
if( size.height == 1 )
src_step = dst_step = CV_STUB_STEP;
IPPI_CALL( func( src->data.ptr, src_step, dst->data.ptr,
dst_step, &size, state, 0 ));
__END__;
cvReleaseMat( &temp );
icvFilterFree( &state );
if( !local_alloc )
cvFree( (void**)&kernel_data );
}
int icvIPPSepFilter( const CvMat* src, CvMat* dst, const CvMat* kernelX,
const CvMat* kernelY, CvPoint anchor )
{
int result = 0;
CvMat* top_bottom = 0;
CvMat* vout_hin = 0;
CvMat* dst_buf = 0;
CV_FUNCNAME( "icvIPPSepFilter" );
__BEGIN__;
CvSize ksize;
CvPoint el_anchor;
CvSize size;
int type, depth, pix_size;
int i, x, y, dy = 0, prev_dy = 0, max_dy;
CvMat vout;
CvCopyNonConstBorderFunc copy_border_func;
CvIPPSepFilterFunc x_func = 0, y_func = 0;
int src_step, top_bottom_step;
float *kx, *ky;
int align, stripe_size;
if( !icvFilterRow_8u_C1R_p )
EXIT;
if( !CV_ARE_TYPES_EQ( src, dst ) || !CV_ARE_SIZES_EQ( src, dst ) ||
!CV_IS_MAT_CONT(kernelX->type & kernelY->type) ||
CV_MAT_TYPE(kernelX->type) != CV_32FC1 ||
CV_MAT_TYPE(kernelY->type) != CV_32FC1 ||
kernelX->cols != 1 && kernelX->rows != 1 ||
kernelY->cols != 1 && kernelY->rows != 1 ||
(unsigned)anchor.x >= (unsigned)(kernelX->cols + kernelX->rows - 1) ||
(unsigned)anchor.y >= (unsigned)(kernelY->cols + kernelY->rows - 1) )
CV_ERROR( CV_StsError, "Internal Error: incorrect parameters" );
ksize.width = kernelX->cols + kernelX->rows - 1;
ksize.height = kernelY->cols + kernelY->rows - 1;
/*if( ksize.width <= 5 && ksize.height <= 5 )
{
float* ker = (float*)cvStackAlloc( ksize.width*ksize.height*sizeof(ker[0]));
CvMat kernel = cvMat( ksize.height, ksize.width, CV_32F, ker );
for( y = 0, i = 0; y < ksize.height; y++ )
for( x = 0; x < ksize.width; x++, i++ )
ker[i] = kernelY->data.fl[y]*kernelX->data.fl[x];
CV_CALL( cvFilter2D( src, dst, &kernel, anchor ));
EXIT;
}*/
type = CV_MAT_TYPE(src->type);
depth = CV_MAT_DEPTH(type);
pix_size = CV_ELEM_SIZE(type);
if( type == CV_8UC1 )
x_func = icvFilterRow_8u_C1R_p, y_func = icvFilterColumn_8u_C1R_p;
else if( type == CV_8UC3 )
x_func = icvFilterRow_8u_C3R_p, y_func = icvFilterColumn_8u_C3R_p;
else if( type == CV_8UC4 )
x_func = icvFilterRow_8u_C4R_p, y_func = icvFilterColumn_8u_C4R_p;
else if( type == CV_16SC1 )
x_func = icvFilterRow_16s_C1R_p, y_func = icvFilterColumn_16s_C1R_p;
else if( type == CV_16SC3 )
x_func = icvFilterRow_16s_C3R_p, y_func = icvFilterColumn_16s_C3R_p;
else if( type == CV_16SC4 )
x_func = icvFilterRow_16s_C4R_p, y_func = icvFilterColumn_16s_C4R_p;
else if( type == CV_32FC1 )
x_func = icvFilterRow_32f_C1R_p, y_func = icvFilterColumn_32f_C1R_p;
else if( type == CV_32FC3 )
x_func = icvFilterRow_32f_C3R_p, y_func = icvFilterColumn_32f_C3R_p;
else if( type == CV_32FC4 )
x_func = icvFilterRow_32f_C4R_p, y_func = icvFilterColumn_32f_C4R_p;
else
EXIT;
size = cvGetMatSize(src);
stripe_size = src->data.ptr == dst->data.ptr ? 1 << 15 : 1 << 16;
max_dy = MAX( ksize.height - 1, stripe_size/(size.width + ksize.width - 1));
max_dy = MIN( max_dy, size.height + ksize.height - 1 );
align = 8/CV_ELEM_SIZE(depth);
CV_CALL( top_bottom = cvCreateMat( ksize.height*2, cvAlign(size.width,align), type ));
CV_CALL( vout_hin = cvCreateMat( max_dy + ksize.height,
cvAlign(size.width + ksize.width - 1, align), type ));
if( src->data.ptr == dst->data.ptr && size.height )
CV_CALL( dst_buf = cvCreateMat( max_dy + ksize.height,
cvAlign(size.width, align), type ));
kx = (float*)cvStackAlloc( ksize.width*sizeof(kx[0]) );
ky = (float*)cvStackAlloc( ksize.height*sizeof(ky[0]) );
// mirror the kernels
for( i = 0; i < ksize.width; i++ )
kx[i] = kernelX->data.fl[ksize.width - i - 1];
for( i = 0; i < ksize.height; i++ )
ky[i] = kernelY->data.fl[ksize.height - i - 1];
el_anchor = cvPoint( ksize.width - anchor.x - 1, ksize.height - anchor.y - 1 );
cvGetCols( vout_hin, &vout, anchor.x, anchor.x + size.width );
copy_border_func = icvGetCopyNonConstBorderFunc( pix_size, IPL_BORDER_REPLICATE );
src_step = src->step ? src->step : CV_STUB_STEP;
top_bottom_step = top_bottom->step ? top_bottom->step : CV_STUB_STEP;
vout.step = vout.step ? vout.step : CV_STUB_STEP;
for( y = 0; y < size.height; y += dy )
{
const CvMat *vin = src, *hout = dst;
int src_y = y, dst_y = y;
dy = MIN( max_dy, size.height - (ksize.height - anchor.y - 1) - y );
if( y < anchor.y || dy < anchor.y )
{
int ay = anchor.y;
CvSize src_stripe_size = size;
if( y < anchor.y )
{
src_y = 0;
dy = MIN( anchor.y, size.height );
src_stripe_size.height = MIN( dy + ksize.height - anchor.y - 1, size.height );
}
else
{
src_y = MAX( y - anchor.y, 0 );
dy = size.height - y;
src_stripe_size.height = MIN( dy + anchor.y, size.height );
ay = MAX( anchor.y - y, 0 );
}
copy_border_func( src->data.ptr + src_y*src_step, src_step, src_stripe_size,
top_bottom->data.ptr, top_bottom_step,
cvSize(size.width, dy + ksize.height - 1),
ay, 0 );
vin = top_bottom;
src_y = anchor.y;
}
// do vertical convolution
IPPI_CALL( y_func( vin->data.ptr + src_y*vin->step, vin->step ? vin->step : CV_STUB_STEP,
vout.data.ptr, vout.step, cvSize(size.width, dy),
ky, ksize.height, el_anchor.y ));
// now it's time to copy the previously processed stripe to the input/output image
if( src->data.ptr == dst->data.ptr )
{
for( i = 0; i < prev_dy; i++ )
memcpy( dst->data.ptr + (y - prev_dy + i)*dst->step,
dst_buf->data.ptr + i*dst_buf->step, size.width*pix_size );
if( y + dy < size.height )
{
hout = dst_buf;
dst_y = 0;
}
}
// create a border for every line by replicating the left-most/right-most elements
for( i = 0; i < dy; i++ )
{
uchar* ptr = vout.data.ptr + i*vout.step;
for( x = -1; x >= -anchor.x*pix_size; x-- )
ptr[x] = ptr[x + pix_size];
for( x = size.width*pix_size; x < (size.width+ksize.width-anchor.x-1)*pix_size; x++ )
ptr[x] = ptr[x - pix_size];
}
// do horizontal convolution
IPPI_CALL( x_func( vout.data.ptr, vout.step, hout->data.ptr + dst_y*hout->step,
hout->step ? hout->step : CV_STUB_STEP,
cvSize(size.width, dy), kx, ksize.width, el_anchor.x ));
prev_dy = dy;
}
result = 1;
__END__;
cvReleaseMat( &vout_hin );
cvReleaseMat( &dst_buf );
cvReleaseMat( &top_bottom );
return result;
}
CV_IMPL void cvRandShuffle( CvArr* arr, CvRNG* rng, double iter_factor )
{
CV_FUNCNAME( "cvRandShuffle" );
__BEGIN__;
const int sizeof_int = (int)sizeof(int);
CvMat stub, *mat = (CvMat*)arr;
int i, j, k, iters, delta = 0;
int cont_flag, arr_size, elem_size, cols, step;
const int pair_buf_sz = 100;
int* pair_buf = (int*)cvStackAlloc( pair_buf_sz*sizeof(pair_buf[0])*2 );
CvMat _pair_buf = cvMat( 1, pair_buf_sz*2, CV_32S, pair_buf );
CvRNG _rng = cvRNG(-1);
uchar* data = 0;
int* idata = 0;
if( !CV_IS_MAT(mat) )
CV_CALL( mat = cvGetMat( mat, &stub ));
if( !rng )
rng = &_rng;
cols = mat->cols;
step = mat->step;
arr_size = cols*mat->rows;
iters = cvRound(iter_factor*arr_size)*2;
cont_flag = CV_IS_MAT_CONT(mat->type);
elem_size = CV_ELEM_SIZE(mat->type);
if( elem_size % sizeof_int == 0 && (cont_flag || step % sizeof_int == 0) )
{
idata = mat->data.i;
step /= sizeof_int;
elem_size /= sizeof_int;
}
else
data = mat->data.ptr;
for( i = 0; i < iters; i += delta )
{
delta = MIN( iters - i, pair_buf_sz*2 );
_pair_buf.cols = delta;
cvRandArr( rng, &_pair_buf, CV_RAND_UNI, cvRealScalar(0), cvRealScalar(arr_size) );
if( cont_flag )
{
if( idata )
for( j = 0; j < delta; j += 2 )
{
int* p = idata + pair_buf[j]*elem_size, *q = idata + pair_buf[j+1]*elem_size, t;
for( k = 0; k < elem_size; k++ )
CV_SWAP( p[k], q[k], t );
}
else
for( j = 0; j < delta; j += 2 )
{
uchar* p = data + pair_buf[j]*elem_size, *q = data + pair_buf[j+1]*elem_size, t;
for( k = 0; k < elem_size; k++ )
CV_SWAP( p[k], q[k], t );
}
}
else
{
if( idata )
for( j = 0; j < delta; j += 2 )
{
int idx1 = pair_buf[j], idx2 = pair_buf[j+1], row1, row2;
int* p, *q, t;
row1 = idx1/step; row2 = idx2/step;
p = idata + row1*step + (idx1 - row1*cols)*elem_size;
q = idata + row2*step + (idx2 - row2*cols)*elem_size;
for( k = 0; k < elem_size; k++ )
CV_SWAP( p[k], q[k], t );
}
else
for( j = 0; j < delta; j += 2 )
{
int idx1 = pair_buf[j], idx2 = pair_buf[j+1], row1, row2;
uchar* p, *q, t;
row1 = idx1/step; row2 = idx2/step;
p = data + row1*step + (idx1 - row1*cols)*elem_size;
q = data + row2*step + (idx2 - row2*cols)*elem_size;
for( k = 0; k < elem_size; k++ )
CV_SWAP( p[k], q[k], t );
}
}
}
__END__;
}
float CvANN_MLP::predict( const CvMat* _inputs, CvMat* _outputs ) const
{
CV_FUNCNAME( "CvANN_MLP::predict" );
__BEGIN__;
double* buf;
int i, j, n, dn = 0, l_count, dn0, buf_sz, min_buf_sz;
if( !layer_sizes )
CV_ERROR( CV_StsError, "The network has not been initialized" );
if( !CV_IS_MAT(_inputs) || !CV_IS_MAT(_outputs) ||
!CV_ARE_TYPES_EQ(_inputs,_outputs) ||
CV_MAT_TYPE(_inputs->type) != CV_32FC1 &&
CV_MAT_TYPE(_inputs->type) != CV_64FC1 ||
_inputs->rows != _outputs->rows )
CV_ERROR( CV_StsBadArg, "Both input and output must be floating-point matrices "
"of the same type and have the same number of rows" );
if( _inputs->cols != layer_sizes->data.i[0] )
CV_ERROR( CV_StsBadSize, "input matrix must have the same number of columns as "
"the number of neurons in the input layer" );
if( _outputs->cols != layer_sizes->data.i[layer_sizes->cols - 1] )
CV_ERROR( CV_StsBadSize, "output matrix must have the same number of columns as "
"the number of neurons in the output layer" );
n = dn0 = _inputs->rows;
min_buf_sz = 2*max_count;
buf_sz = n*min_buf_sz;
if( buf_sz > max_buf_sz )
{
dn0 = max_buf_sz/min_buf_sz;
dn0 = MAX( dn0, 1 );
buf_sz = dn0*min_buf_sz;
}
buf = (double*)cvStackAlloc( buf_sz*sizeof(buf[0]) );
l_count = layer_sizes->cols;
for( i = 0; i < n; i += dn )
{
CvMat hdr[2], _w, *layer_in = &hdr[0], *layer_out = &hdr[1], *temp;
dn = MIN( dn0, n - i );
cvGetRows( _inputs, layer_in, i, i + dn );
cvInitMatHeader( layer_out, dn, layer_in->cols, CV_64F, buf );
scale_input( layer_in, layer_out );
CV_SWAP( layer_in, layer_out, temp );
for( j = 1; j < l_count; j++ )
{
double* data = buf + (j&1 ? max_count*dn0 : 0);
int cols = layer_sizes->data.i[j];
cvInitMatHeader( layer_out, dn, cols, CV_64F, data );
cvInitMatHeader( &_w, layer_in->cols, layer_out->cols, CV_64F, weights[j] );
cvGEMM( layer_in, &_w, 1, 0, 0, layer_out );
calc_activ_func( layer_out, _w.data.db + _w.rows*_w.cols );
CV_SWAP( layer_in, layer_out, temp );
}
cvGetRows( _outputs, layer_out, i, i + dn );
scale_output( layer_in, layer_out );
}
__END__;
return 0.f;
}
static void
icvMorphOp( const void* srcarr, void* dstarr, IplConvKernel* element,
int iterations, int mop )
{
CvMorphology morphology;
void* buffer = 0;
int local_alloc = 0;
void* morphstate = 0;
CvMat* temp = 0;
CV_FUNCNAME( "icvMorphOp" );
__BEGIN__;
int i, coi1 = 0, coi2 = 0;
CvMat srcstub, *src = (CvMat*)srcarr;
CvMat dststub, *dst = (CvMat*)dstarr;
CvMat el_hdr, *el = 0;
CvSize size, el_size;
CvPoint el_anchor;
int el_shape;
int type;
bool inplace;
if( !CV_IS_MAT(src) )
CV_CALL( src = cvGetMat( src, &srcstub, &coi1 ));
if( src != &srcstub )
{
srcstub = *src;
src = &srcstub;
}
if( dstarr == srcarr )
dst = src;
else
{
CV_CALL( dst = cvGetMat( dst, &dststub, &coi2 ));
if( !CV_ARE_TYPES_EQ( src, dst ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( !CV_ARE_SIZES_EQ( src, dst ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
}
if( dst != &dststub )
{
dststub = *dst;
dst = &dststub;
}
if( coi1 != 0 || coi2 != 0 )
CV_ERROR( CV_BadCOI, "" );
type = CV_MAT_TYPE( src->type );
size = cvGetMatSize( src );
inplace = src->data.ptr == dst->data.ptr;
if( iterations == 0 || (element && element->nCols == 1 && element->nRows == 1))
{
if( src->data.ptr != dst->data.ptr )
cvCopy( src, dst );
EXIT;
}
if( element )
{
el_size = cvSize( element->nCols, element->nRows );
el_anchor = cvPoint( element->anchorX, element->anchorY );
el_shape = (int)(element->nShiftR);
el_shape = el_shape < CV_SHAPE_CUSTOM ? el_shape : CV_SHAPE_CUSTOM;
}
else
{
el_size = cvSize(3,3);
el_anchor = cvPoint(1,1);
el_shape = CV_SHAPE_RECT;
}
if( el_shape == CV_SHAPE_RECT && iterations > 1 )
{
el_size.width = 1 + (el_size.width-1)*iterations;
el_size.height = 1 + (el_size.height-1)*iterations;
el_anchor.x *= iterations;
el_anchor.y *= iterations;
iterations = 1;
}
if( el_shape == CV_SHAPE_RECT && icvErodeRect_GetBufSize_8u_C1R_p )
{
CvMorphRectFunc_IPP rect_func = 0;
CvMorphRectGetBufSizeFunc_IPP rect_getbufsize_func = 0;
if( mop == 0 )
{
if( type == CV_8UC1 )
rect_getbufsize_func = icvErodeRect_GetBufSize_8u_C1R_p,
rect_func = icvErodeRect_8u_C1R_p;
else if( type == CV_8UC3 )
rect_getbufsize_func = icvErodeRect_GetBufSize_8u_C3R_p,
rect_func = icvErodeRect_8u_C3R_p;
else if( type == CV_8UC4 )
rect_getbufsize_func = icvErodeRect_GetBufSize_8u_C4R_p,
rect_func = icvErodeRect_8u_C4R_p;
else if( type == CV_16UC1 )
rect_getbufsize_func = icvErodeRect_GetBufSize_16u_C1R_p,
rect_func = icvErodeRect_16u_C1R_p;
else if( type == CV_16UC3 )
rect_getbufsize_func = icvErodeRect_GetBufSize_16u_C3R_p,
rect_func = icvErodeRect_16u_C3R_p;
else if( type == CV_16UC4 )
rect_getbufsize_func = icvErodeRect_GetBufSize_16u_C4R_p,
rect_func = icvErodeRect_16u_C4R_p;
else if( type == CV_32FC1 )
rect_getbufsize_func = icvErodeRect_GetBufSize_32f_C1R_p,
rect_func = icvErodeRect_32f_C1R_p;
else if( type == CV_32FC3 )
rect_getbufsize_func = icvErodeRect_GetBufSize_32f_C3R_p,
rect_func = icvErodeRect_32f_C3R_p;
else if( type == CV_32FC4 )
rect_getbufsize_func = icvErodeRect_GetBufSize_32f_C4R_p,
rect_func = icvErodeRect_32f_C4R_p;
}
else
{
if( type == CV_8UC1 )
rect_getbufsize_func = icvDilateRect_GetBufSize_8u_C1R_p,
rect_func = icvDilateRect_8u_C1R_p;
else if( type == CV_8UC3 )
rect_getbufsize_func = icvDilateRect_GetBufSize_8u_C3R_p,
rect_func = icvDilateRect_8u_C3R_p;
else if( type == CV_8UC4 )
rect_getbufsize_func = icvDilateRect_GetBufSize_8u_C4R_p,
rect_func = icvDilateRect_8u_C4R_p;
else if( type == CV_16UC1 )
rect_getbufsize_func = icvDilateRect_GetBufSize_16u_C1R_p,
rect_func = icvDilateRect_16u_C1R_p;
else if( type == CV_16UC3 )
rect_getbufsize_func = icvDilateRect_GetBufSize_16u_C3R_p,
rect_func = icvDilateRect_16u_C3R_p;
else if( type == CV_16UC4 )
rect_getbufsize_func = icvDilateRect_GetBufSize_16u_C4R_p,
rect_func = icvDilateRect_16u_C4R_p;
else if( type == CV_32FC1 )
rect_getbufsize_func = icvDilateRect_GetBufSize_32f_C1R_p,
rect_func = icvDilateRect_32f_C1R_p;
else if( type == CV_32FC3 )
rect_getbufsize_func = icvDilateRect_GetBufSize_32f_C3R_p,
rect_func = icvDilateRect_32f_C3R_p;
else if( type == CV_32FC4 )
rect_getbufsize_func = icvDilateRect_GetBufSize_32f_C4R_p,
rect_func = icvDilateRect_32f_C4R_p;
}
if( rect_getbufsize_func && rect_func )
{
int bufsize = 0;
CvStatus status = rect_getbufsize_func( size.width, el_size, &bufsize );
if( status >= 0 && bufsize > 0 )
{
if( bufsize < CV_MAX_LOCAL_SIZE )
{
buffer = cvStackAlloc( bufsize );
local_alloc = 1;
}
else
CV_CALL( buffer = cvAlloc( bufsize ));
}
if( status >= 0 )
{
int src_step, dst_step = dst->step ? dst->step : CV_STUB_STEP;
if( inplace )
{
CV_CALL( temp = cvCloneMat( dst ));
src = temp;
}
src_step = src->step ? src->step : CV_STUB_STEP;
status = rect_func( src->data.ptr, src_step, dst->data.ptr,
dst_step, size, el_size, el_anchor, buffer );
}
if( status >= 0 )
EXIT;
}
}
else if( el_shape == CV_SHAPE_CUSTOM && icvMorphInitAlloc_8u_C1R_p && icvMorphFree_p &&
src->data.ptr != dst->data.ptr )
{
CvMorphCustomFunc_IPP custom_func = 0;
CvMorphCustomInitAllocFunc_IPP custom_initalloc_func = 0;
const int bordertype = 1; // replication border
if( type == CV_8UC1 )
custom_initalloc_func = icvMorphInitAlloc_8u_C1R_p,
custom_func = mop == 0 ? icvErode_8u_C1R_p : icvDilate_8u_C1R_p;
else if( type == CV_8UC3 )
custom_initalloc_func = icvMorphInitAlloc_8u_C3R_p,
custom_func = mop == 0 ? icvErode_8u_C3R_p : icvDilate_8u_C3R_p;
else if( type == CV_8UC4 )
custom_initalloc_func = icvMorphInitAlloc_8u_C4R_p,
custom_func = mop == 0 ? icvErode_8u_C4R_p : icvDilate_8u_C4R_p;
else if( type == CV_16UC1 )
custom_initalloc_func = icvMorphInitAlloc_16u_C1R_p,
custom_func = mop == 0 ? icvErode_16u_C1R_p : icvDilate_16u_C1R_p;
else if( type == CV_16UC3 )
custom_initalloc_func = icvMorphInitAlloc_16u_C3R_p,
custom_func = mop == 0 ? icvErode_16u_C3R_p : icvDilate_16u_C3R_p;
else if( type == CV_16UC4 )
custom_initalloc_func = icvMorphInitAlloc_16u_C4R_p,
custom_func = mop == 0 ? icvErode_16u_C4R_p : icvDilate_16u_C4R_p;
else if( type == CV_32FC1 )
custom_initalloc_func = icvMorphInitAlloc_32f_C1R_p,
custom_func = mop == 0 ? icvErode_32f_C1R_p : icvDilate_32f_C1R_p;
else if( type == CV_32FC3 )
custom_initalloc_func = icvMorphInitAlloc_32f_C3R_p,
custom_func = mop == 0 ? icvErode_32f_C3R_p : icvDilate_32f_C3R_p;
else if( type == CV_32FC4 )
custom_initalloc_func = icvMorphInitAlloc_32f_C4R_p,
custom_func = mop == 0 ? icvErode_32f_C4R_p : icvDilate_32f_C4R_p;
if( custom_initalloc_func && custom_func )
{
uchar *src_ptr, *dst_ptr = dst->data.ptr;
int src_step, dst_step = dst->step ? dst->step : CV_STUB_STEP;
int el_len = el_size.width*el_size.height;
uchar* el_mask = (uchar*)cvStackAlloc( el_len );
CvStatus status;
for( i = 0; i < el_len; i++ )
el_mask[i] = (uchar)(element->values[i] != 0);
status = custom_initalloc_func( size.width, el_mask, el_size,
el_anchor, &morphstate );
if( status >= 0 && (inplace || iterations > 1) )
{
CV_CALL( temp = cvCloneMat( src ));
src = temp;
}
src_ptr = src->data.ptr;
src_step = src->step ? src->step : CV_STUB_STEP;
for( i = 0; i < iterations && status >= 0 && morphstate; i++ )
{
uchar* t_ptr;
int t_step;
status = custom_func( src_ptr, src_step, dst_ptr, dst_step,
size, bordertype, morphstate );
CV_SWAP( src_ptr, dst_ptr, t_ptr );
CV_SWAP( src_step, dst_step, t_step );
if( i == 0 && temp )
{
dst_ptr = temp->data.ptr;
dst_step = temp->step ? temp->step : CV_STUB_STEP;
}
}
if( status >= 0 )
{
if( iterations % 2 == 0 )
cvCopy( temp, dst );
EXIT;
}
}
}
if( el_shape != CV_SHAPE_RECT )
{
el_hdr = cvMat( element->nRows, element->nCols, CV_32SC1, element->values );
el = &el_hdr;
el_shape = CV_SHAPE_CUSTOM;
}
CV_CALL( morphology.init( mop, src->cols, src->type,
el_shape, el, el_size, el_anchor ));
for( i = 0; i < iterations; i++ )
{
CV_CALL( morphology.process( src, dst ));
src = dst;
}
__END__;
if( !local_alloc )
cvFree( &buffer );
if( morphstate )
icvMorphFree_p( morphstate );
cvReleaseMat( &temp );
}