CV_IMPL void cvCurvature(const CvArr* srcarr_x,
const CvArr* srcarr_y,
CvArr* dstarr)
{
CV_FUNCNAME("cvCurvature");
__BEGIN__;
CvMat sstub_x, sstub_y, *src_x, *src_y;
CvMat dstub, *dst;
CvSize size;
CvMat *Nxx=0, *Nyy=0, *ones=0;
CV_CALL( src_x = cvGetMat(srcarr_x, &sstub_x ));
CV_CALL( src_y = cvGetMat(srcarr_y, &sstub_y ));
CV_CALL( dst = cvGetMat(dstarr, &dstub ));
if( CV_MAT_TYPE(src_x->type) != CV_32FC1)
CV_ERROR( CV_StsUnsupportedFormat, "Only-32bit, 1-channel input images are supported" );
if( CV_MAT_TYPE(src_y->type) != CV_32FC1)
CV_ERROR( CV_StsUnsupportedFormat, "Only-32bit, 1-channel input images are supported" );
if( CV_MAT_TYPE(dst->type) != CV_32FC1)
CV_ERROR( CV_StsUnsupportedFormat, "Only-32bit, 1-channel input images are supported" );
if( !CV_ARE_SIZES_EQ( src_x, src_y ))
CV_ERROR( CV_StsUnmatchedSizes, "The input images must have the same size" );
size = cvGetMatSize( src_x );
Nxx = cvCreateMat(size.height, size.width, CV_32FC1 );
Nyy = cvCreateMat(size.height, size.width, CV_32FC1 );
ones= cvCreateMat(size.height, size.width, CV_32FC1 );
cvSetZero(Nxx);
cvSetZero(Nyy);
cvSet(ones, cvScalar(1.0f));
cvSobel(src_x, Nxx, 1, 0, 1);
cvSobel(src_y, Nyy, 0, 1, 1);
cvMul(Nxx, ones, Nxx, 0.25f);
cvMul(Nyy, ones, Nyy, 0.25f);
cvAdd(Nxx, Nyy, dst);
cvReleaseMat(&Nxx);
cvReleaseMat(&Nyy);
cvReleaseMat(&ones);
__END__;
}
CV_IMPL void cvShowImage( const char* name, const CvArr* arr)
{
CV_FUNCNAME( "cvShowImage" );
__BEGIN__;
CvWindow* window;
int origin = 0;
int resize = 0;
CvMat stub, *image;
if( !name )
CV_ERROR( CV_StsNullPtr, "NULL name" );
window = icvFindWindowByName(name);
if( !window || !arr )
EXIT; // keep silence here.
if( CV_IS_IMAGE_HDR( arr ))
origin = ((IplImage*)arr)->origin;
CV_CALL( image = cvGetMat( arr, &stub ));
/*
if( !window->image )
cvResizeWindow( name, image->cols, image->rows );
*/
if( window->image &&
!CV_ARE_SIZES_EQ(window->image, image) ) {
if ( ! (window->flags & CV_WINDOW_AUTOSIZE) )//FD
resize = 1;
cvReleaseMat( &window->image );
}
if( !window->image ) {
resize = 1;//FD
window->image = cvCreateMat( image->rows, image->cols, CV_8UC3 );
}
cvConvertImage( image, window->image, (origin != 0 ? CV_CVTIMG_FLIP : 0) + CV_CVTIMG_SWAP_RB );
icvPutImage( window );
if ( resize )//FD
icvUpdateWindowSize( window );
__END__;
}
CV_IMPL void cvDirac(const CvArr* srcarr,
CvArr* dstarr,
double sigma)
{
CV_FUNCNAME("cvDirac");
__BEGIN__;
CvMat sstub, *src;
CvMat dstub, *dst;
CvSize size;
int i, j, iStep_src, iStep_dst;
float* fPtr_src, *fPtr_dst, flag=0.0f;
float temp1=0.0f, temp2=0.0f;
CV_CALL( src = cvGetMat(srcarr, &sstub ));
CV_CALL( dst = cvGetMat(dstarr, &dstub ));
if( CV_MAT_TYPE(src->type) != CV_32FC1)
CV_ERROR( CV_StsUnsupportedFormat, "Only-32bit, 1-channel input images are supported" );
if( CV_MAT_TYPE(dst->type) != CV_32FC1)
CV_ERROR( CV_StsUnsupportedFormat, "Only-32bit, 1-channel output images are supported" );
if( !CV_ARE_SIZES_EQ( src, dst ))
CV_ERROR( CV_StsUnmatchedSizes, "The input images must have the same size" );
size = cvGetMatSize( src );
iStep_src = src->step / sizeof(fPtr_src[0]);
fPtr_src = src->data.fl;
iStep_dst = dst->step / sizeof(fPtr_dst[0]);
fPtr_dst = dst->data.fl;
for (j=0; j<size.height; j++){
for (i=0; i<size.width; i++){
temp1 = fPtr_src[i+iStep_src*j];
temp2 = (1.0f/2.0f/sigma)*(1.0f+cos(PI*temp1/sigma));
if (int(temp1*10000)<=int(sigma*10000) && int(temp1*10000)>=int(-sigma*10000)) {
flag = 1.0f;
} else {
flag = 0.0f;
}
fPtr_dst[i+iStep_dst*j]=temp2*flag;
}
}
__END__;
}
/* motion templates */
CV_IMPL void
cvUpdateMotionHistory( const void* silhouette, void* mhimg,
double timestamp, double mhi_duration )
{
CvSize size;
CvMat silhstub, *silh = (CvMat*)silhouette;
CvMat mhistub, *mhi = (CvMat*)mhimg;
int mhi_step, silh_step;
CV_FUNCNAME( "cvUpdateMHIByTime" );
__BEGIN__;
CV_CALL( silh = cvGetMat( silh, &silhstub ));
CV_CALL( mhi = cvGetMat( mhi, &mhistub ));
if( !CV_IS_MASK_ARR( silh ))
CV_ERROR( CV_StsBadMask, "" );
if( CV_MAT_CN( mhi->type ) > 1 )
CV_ERROR( CV_BadNumChannels, "" );
if( CV_MAT_DEPTH( mhi->type ) != CV_32F )
CV_ERROR( CV_BadDepth, "" );
if( !CV_ARE_SIZES_EQ( mhi, silh ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
size = cvGetMatSize( mhi );
mhi_step = mhi->step;
silh_step = silh->step;
if( CV_IS_MAT_CONT( mhi->type & silh->type ))
{
size.width *= size.height;
mhi_step = silh_step = CV_STUB_STEP;
size.height = 1;
}
IPPI_CALL( icvUpdateMotionHistory_8u32f_C1IR( (const uchar*)(silh->data.ptr), silh_step,
mhi->data.fl, mhi_step, size,
(float)timestamp, (float)mhi_duration ));
__END__;
}
CV_IMPL void
cvPreCornerDetect( const void* srcarr, void* dstarr, int aperture_size )
{
static CvFuncTable pre_tab;
static int inittab = 0;
CV_FUNCNAME( "cvPreCornerDetect" );
__BEGIN__;
CvSize src_size;
CvPreCornerFunc func = 0;
CvMat srcstub, *src = (CvMat*)srcarr;
CvMat dststub, *dst = (CvMat*)dstarr;
if( !inittab )
{
icvInitPreCornerDetectTable( &pre_tab );
inittab = 1;
}
CV_CALL( src = cvGetMat( srcarr, &srcstub ));
CV_CALL( dst = cvGetMat( dstarr, &dststub ));
if( CV_ARR_CN(src->type) != 1 || CV_ARR_CN(dst->type) != 1 )
CV_ERROR(CV_StsBadArg, "Source or min-eigen-val images have more than 1 channel");
if( CV_ARR_DEPTH(dst->type) != CV_32F )
CV_ERROR( CV_BadDepth, "min-eigen-val image does not have IPL_DEPTH_32F depth" );
if( !CV_ARE_SIZES_EQ( src, dst ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
func = (CvPreCornerFunc)(pre_tab.fn_2d[CV_ARR_DEPTH(src->type)]);
if( !func )
CV_ERROR( CV_StsUnsupportedFormat, "" );
src_size = icvGetMatSize( src );
IPPI_CALL( func( src->data.ptr, src->step, dst->data.ptr, dst->step,
src_size, aperture_size ));
__END__;
}
static void
icvTrueDistTrans( const CvMat* src, CvMat* dst )
{
const float inf = 1e15f;
if( !CV_ARE_SIZES_EQ( src, dst ))
CV_Error( CV_StsUnmatchedSizes, "" );
if( CV_MAT_TYPE(src->type) != CV_8UC1 ||
CV_MAT_TYPE(dst->type) != CV_32FC1 )
CV_Error( CV_StsUnsupportedFormat,
"The input image must have 8uC1 type and the output one must have 32fC1 type" );
int i, m = src->rows, n = src->cols;
cv::AutoBuffer<uchar> _buf(std::max(m*2*sizeof(float) + (m*3+1)*sizeof(int), n*2*sizeof(float)));
// stage 1: compute 1d distance transform of each column
float* sqr_tab = (float*)(uchar*)_buf;
int* sat_tab = cv::alignPtr((int*)(sqr_tab + m*2), sizeof(int));
int shift = m*2;
for( i = 0; i < m; i++ )
sqr_tab[i] = (float)(i*i);
for( i = m; i < m*2; i++ )
sqr_tab[i] = inf;
for( i = 0; i < shift; i++ )
sat_tab[i] = 0;
for( ; i <= m*3; i++ )
sat_tab[i] = i - shift;
cv::parallel_for(cv::BlockedRange(0, n), cv::DTColumnInvoker(src, dst, sat_tab, sqr_tab));
// stage 2: compute modified distance transform for each row
float* inv_tab = sqr_tab + n;
inv_tab[0] = sqr_tab[0] = 0.f;
for( i = 1; i < n; i++ )
{
inv_tab[i] = (float)(0.5/i);
sqr_tab[i] = (float)(i*i);
}
cv::parallel_for(cv::BlockedRange(0, m), cv::DTRowInvoker(dst, sqr_tab, inv_tab));
}
/*F///////////////////////////////////////////////////////////////////////////////////////
// Name: cvAdaptiveThreshold
// Purpose: Adaptive Thresholding the source image
// Context:
// Parameters:
// srcIm - source image
// dstIm - result thresholding image
// maxValue - the maximum value of the image pixel
// method - method for the adaptive threshold calculation
// (now CV_STDDEF_ADAPTIVE_THRESH only)
// type - thresholding type, must be one of
// CV_THRESH_BINARY - val = (val > Thresh ? MAX : 0)
// CV_THRESH_BINARY_INV - val = (val > Thresh ? 0 : MAX)
// CV_THRESH_TOZERO - val = (val > Thresh ? val : 0)
// CV_THRESH_TOZERO_INV - val = (val > Thresh ? 0 : val)
// parameters - pointer to the input parameters (for the
// CV_STDDEF_ADAPTIVE_THRESH method parameters[0] is size of
// the neighborhood thresholding, (one of the 1-(3x3),2-(5x5),or
// 3-(7x7)), parameters[1] is the value of the minimum variance
// Returns:
// Notes:
//F*/
CV_IMPL void
cvAdaptiveThreshold( const void *srcIm, void *dstIm,
double maxValue, CvAdaptiveThreshMethod method,
CvThreshType type, double *parameters )
{
///////////////// Some variables /////////////////
CvMat src_stub, dst_stub;
CvMat *src = 0, *dst = 0;
CV_FUNCNAME( "cvAdaptiveThreshold" );
__BEGIN__;
///////////////// Checking /////////////////
CV_CALL( src = cvGetMat( srcIm, &src_stub ));
CV_CALL( dst = cvGetMat( dstIm, &dst_stub ));
if( !CV_ARE_TYPES_EQ( src, dst ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( CV_ARR_TYPE(src->type) != CV_8UC1 )
CV_ERROR( CV_StsUnsupportedFormat, "" );
if( !CV_ARE_SIZES_EQ( src, dst ) )
CV_ERROR( CV_StsUnmatchedSizes, "" );
switch (method)
{
case CV_STDDEV_ADAPTIVE_THRESH:
icvAdaptiveThreshold_StdDev( src, dst, cvRound(maxValue),
type, cvRound( parameters[0] ),
cvRound( parameters[1] ));
break;
default:
CV_ERROR_FROM_STATUS( CV_BADCOEF_ERR );
}
__END__;
}
CV_IMPL void cvDistReg(const CvArr* srcarr,
CvArr* dstarr)
{
CV_FUNCNAME("cvDistReg");
__BEGIN__;
CvMat sstub, *src;
CvMat dstub, *dst;
CvMat* src_dx=0, *src_dy=0, *s=0, *ps=0;
CvMat* dps_x=0, *dps_y=0, *del=0, *ones=0;
CvSize size;
int i, j, iStep_s, iStep_ps;
float* fPtr_s, *fPtr_ps;
float temp_s=0.0f, temp_ps=0.0f;
float flag_s1=0.0f, flag_s2=0.0f, flag_ps1=0.0f, flag_ps2=0.0f;
CV_CALL( src = cvGetMat(srcarr, &sstub ));
CV_CALL( dst = cvGetMat(dstarr, &dstub ));
if( CV_MAT_TYPE(src->type) != CV_32FC1)
CV_ERROR( CV_StsUnsupportedFormat, "Only-32bit, 1-channel input images are supported" );
if( CV_MAT_TYPE(dst->type) != CV_32FC1)
CV_ERROR( CV_StsUnsupportedFormat, "Only-32bit, 1-channel input images are supported" );
if( !CV_ARE_SIZES_EQ( src, dst ))
CV_ERROR( CV_StsUnmatchedSizes, "The input images must have the same size" );
size = cvGetMatSize( src );
src_dx = cvCreateMat(size.height, size.width, CV_32FC1 );
src_dy = cvCreateMat(size.height, size.width, CV_32FC1 );
s = cvCreateMat(size.height, size.width, CV_32FC1 );
ps = cvCreateMat(size.height, size.width, CV_32FC1 );
dps_x = cvCreateMat(size.height, size.width, CV_32FC1 );
dps_y = cvCreateMat(size.height, size.width, CV_32FC1 );
del = cvCreateMat(size.height, size.width, CV_32FC1 );
ones = cvCreateMat(size.height, size.width, CV_32FC1 );
cvSetZero(src_dx);
cvSetZero(src_dy);
cvSetZero(s);
cvSetZero(ps);
cvSetZero(dps_x);
cvSetZero(dps_y);
cvSetZero(del);
cvSet(ones, cvScalar(1.0f));
iStep_s = s->step / sizeof(fPtr_s[0]);
fPtr_s = s->data.fl;
iStep_ps= ps->step/ sizeof(fPtr_ps[0]);
fPtr_ps = ps->data.fl;
cvSobel(src, src_dx, 1, 0, 1);
cvSobel(src, src_dy, 0, 1, 1);
cvMul(src_dx, ones, src_dx, 0.25f);
cvMul(src_dy, ones, src_dy, 0.25f);
cvCalS(src,s);
for (j=0; j<size.height; j++){
for (i=0; i<size.width; i++){
temp_s = fPtr_s[i+iStep_s*j];
if (int(temp_s*10000)>=0 && int(temp_s*10000)<=10000) {
flag_s1 = 1.0f;
} else {
flag_s1 = 0.0f;
}
if (int(temp_s*10000) > 10000) {
flag_s2 = 1.0f;
} else {
flag_s2 = 0.0f;
}
temp_ps = flag_s1*sin(2*PI*temp_s)/2/PI+flag_s2*(temp_s-1.0f);
if (int(temp_ps*10000) == 0) {
flag_ps1 = 0.0f;
} else {
flag_ps1 = 1.0f;
}
if (int(temp_s*10000) == 0) {
flag_ps2 = 0.0f;
} else {
flag_ps2 = 1.0f;
}
fPtr_ps[i+iStep_ps*j] = (flag_ps1*temp_ps+1.0f-flag_ps1)/(flag_ps2*temp_s+1.0f-flag_ps2);
if ((flag_ps2*temp_s+1.0f-flag_ps2)==0){
printf("Something wrong in last: temp_s = %f, flag_ps2 = %f\n", temp_s, flag_ps2);
exit(0);
}
}
}
cvMul(ps, src_dx, dps_x);
cvMul(ps, src_dy, dps_y);
cvSub(dps_x, src_dx, dps_x);
cvSub(dps_y, src_dy, dps_y);
cvCurvature(dps_x, dps_y, dst);
cvLaplace(src,del,1);
cvMul(del, ones, del, 0.2f);
cvAdd(dst, del, dst);
cvReleaseMat(&src_dx);
cvReleaseMat(&src_dy);
cvReleaseMat(&s);
cvReleaseMat(&ps);
cvReleaseMat(&dps_x);
cvReleaseMat(&dps_y);
cvReleaseMat(&del);
cvReleaseMat(&ones);
__END__;
}
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);
}
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*)uarr;
CvMat vstub, *v = (CvMat*)varr;
CvMat tmat;
uchar* tw = 0;
int type, nm, mn;
int buf_size, pix_size;
int t_svd = 0; // special case: a->rows < a->cols
if( !CV_IS_ARR( a ))
CV_CALL( a = cvGetMat( a, &astub ));
if( !CV_IS_ARR( w ))
CV_CALL( w = cvGetMat( w, &wstub ));
if( !CV_ARE_TYPES_EQ( a, w ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
nm = MIN( a->width, a->height );
mn = MAX( a->width, a->height );
if( (w->width == 1 || w->height == 1) &&
CV_IS_ARR_CONT( w->type ) && w->width*w->height == nm )
{
tw = w->data.ptr;
}
else if( !CV_ARE_SIZES_EQ( w, a ))
{
CV_ERROR( CV_StsBadSize, "W must be either continuous vector of "
"size MIN(A->width,A->height) or matrix of "
"the same size as A" );
}
if( u )
{
if( !CV_IS_ARR( u ))
CV_CALL( u = cvGetMat( u, &ustub ));
if( !CV_ARE_TYPES_EQ( a, u ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( u->width != u->height || u->height != a->height )
CV_ERROR( CV_StsUnmatchedSizes, "U matrix must be square and have the same "
"linear size as number of rows in A" );
if( u->data.ptr == a->data.ptr )
CV_ERROR( CV_StsBadArg, "U can not be equal A" );
}
else
{
u = &ustub;
u->data.ptr = 0;
u->step = 0;
}
if( v )
{
if( !CV_IS_ARR( v ))
CV_CALL( v = cvGetMat( v, &vstub ));
if( !CV_ARE_TYPES_EQ( a, v ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( v->width != v->height || v->width != a->width )
CV_ERROR( CV_StsUnmatchedSizes, "V matrix must be square and have the same "
"linear size as number of columns in A" );
if( v->data.ptr == a->data.ptr || v->data.ptr == u->data.ptr )
CV_ERROR( CV_StsBadArg, "V can not be equal U or A" );
}
else
{
v = &vstub;
v->data.ptr = 0;
v->step = 0;
}
type = CV_ARR_TYPE( a->type );
pix_size = icvPixSize[type];
buf_size = nm*2 + mn;
if( a->rows < a->cols )
{
CvMat* t;
CV_SWAP( u, v, t );
flags = (flags & CV_SVD_U_T ? CV_SVD_V_T : 0)|
(flags & CV_SVD_V_T ? CV_SVD_U_T : 0);
t_svd = 1;
}
if( !(flags & CV_SVD_MODIFY_A) )
buf_size += a->width*a->height;
buf_size *= pix_size;
if( buf_size <= CV_MAX_LOCAL_SIZE )
{
buffer = (uchar*)alloca( buf_size );
local_alloc = 1;
}
else
{
CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
}
if( !(flags & CV_SVD_MODIFY_A) )
{
if( !t_svd )
{
cvInitMatHeader( &tmat, a->height, a->width, type,
buffer + (nm*2 + mn)*pix_size );
cvCopy( a, &tmat );
}
else
{
cvInitMatHeader( &tmat, a->width, a->height, type,
buffer + (nm*2 + mn)*pix_size );
cvT( a, &tmat );
}
a = &tmat;
}
if( !tw )
tw = buffer + (nm + mn)*pix_size;
if( type == CV_32FC1 )
{
IPPI_CALL( icvSVD_32f( a->data.fl, a->step/sizeof(float), (float*)tw,
u->data.fl, u->step/sizeof(float),
v->data.fl, v->step/sizeof(float),
icvGetMatSize(a), (float*)buffer ));
}
else if( type == CV_64FC1 )
{
IPPI_CALL( icvSVD_64f( a->data.db, a->step/sizeof(double), (double*)tw,
u->data.db, u->step/sizeof(double),
v->data.db, v->step/sizeof(double),
icvGetMatSize(a), (double*)buffer ));
}
else
{
CV_ERROR( CV_StsUnsupportedFormat, "" );
}
if( tw != w->data.ptr )
{
cvSetZero( w );
if( type == CV_32FC1 )
for( int i = 0; i < nm; i++ )
((float*)(w->data.ptr + i*w->step))[i] = ((float*)tw)[i];
else
for( int i = 0; i < nm; i++ )
((double*)(w->data.ptr + i*w->step))[i] = ((double*)tw)[i];
}
if( u->data.ptr )
{
if( !(flags & CV_SVD_U_T))
cvT( u, u );
CV_CHECK_NANS( u );
}
if( v->data.ptr)
{
if( !(flags & CV_SVD_V_T))
cvT( v, v );
CV_CHECK_NANS( v );
}
CV_CHECK_NANS( w );
__END__;
if( buffer && !local_alloc )
cvFree( (void**)&buffer );
}
void icvConvertPointsHomogenious( const CvMat* src, CvMat* dst )
{
CvMat* temp = 0;
CvMat* denom = 0;
CV_FUNCNAME( "cvConvertPointsHomogenious" );
__BEGIN__;
int i, s_count, s_dims, d_count, d_dims;
CvMat _src, _dst, _ones;
CvMat* ones = 0;
if( !CV_IS_MAT(src) )
CV_ERROR( !src ? CV_StsNullPtr : CV_StsBadArg,
"The input parameter is not a valid matrix" );
if( !CV_IS_MAT(dst) )
CV_ERROR( !dst ? CV_StsNullPtr : CV_StsBadArg,
"The output parameter is not a valid matrix" );
if( src == dst || src->data.ptr == dst->data.ptr )
{
if( src != dst && (!CV_ARE_TYPES_EQ(src, dst) ||
!CV_ARE_SIZES_EQ(src,dst)) )
CV_ERROR( CV_StsBadArg, "Invalid inplace operation" );
EXIT;
}
if( src->rows > src->cols )
{
if( !((src->cols > 1) ^ (CV_MAT_CN(src->type) > 1)) )
CV_ERROR( CV_StsBadSize,
"Either the number of channels or columns or "
"rows must be =1" );
s_dims = CV_MAT_CN(src->type)*src->cols;
s_count = src->rows;
}
else
{
if( !((src->rows > 1) ^ (CV_MAT_CN(src->type) > 1)) )
CV_ERROR( CV_StsBadSize,
"Either the number of channels or columns or "
"rows must be =1" );
s_dims = CV_MAT_CN(src->type)*src->rows;
s_count = src->cols;
}
if( src->rows == 1 || src->cols == 1 )
src = cvReshape( src, &_src, 1, s_count );
if( dst->rows > dst->cols )
{
if( !((dst->cols > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
CV_ERROR( CV_StsBadSize,
"Either the number of channels or columns or "
"rows in the input matrix must be =1" );
d_dims = CV_MAT_CN(dst->type)*dst->cols;
d_count = dst->rows;
}
else
{
if( !((dst->rows > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
CV_ERROR( CV_StsBadSize,
"Either the number of channels or columns or "
"rows in the output matrix must be =1" );
d_dims = CV_MAT_CN(dst->type)*dst->rows;
d_count = dst->cols;
}
if( dst->rows == 1 || dst->cols == 1 )
dst = cvReshape( dst, &_dst, 1, d_count );
if( s_count != d_count )
CV_ERROR( CV_StsUnmatchedSizes, "Both matrices must have the "
"same number of points" );
if( CV_MAT_DEPTH(src->type) < CV_32F || CV_MAT_DEPTH(dst->type) < CV_32F )
CV_ERROR( CV_StsUnsupportedFormat,
"Both matrices must be floating-point "
"(single or double precision)" );
if( s_dims < 2 || s_dims > 4 || d_dims < 2 || d_dims > 4 )
CV_ERROR( CV_StsOutOfRange,
"Both input and output point dimensionality "
"must be 2, 3 or 4" );
if( s_dims < d_dims - 1 || s_dims > d_dims + 1 )
CV_ERROR( CV_StsUnmatchedSizes,
"The dimensionalities of input and output "
"point sets differ too much" );
if( s_dims == d_dims - 1 )
{
if( d_count == dst->rows )
{
ones = cvGetSubRect( dst, &_ones, cvRect( s_dims, 0, 1, d_count ));
dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, s_dims, d_count ));
}
else
{
ones = cvGetSubRect( dst, &_ones, cvRect( 0, s_dims, d_count, 1 ));
dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, d_count, s_dims ));
}
}
if( s_dims <= d_dims )
{
if( src->rows == dst->rows && src->cols == dst->cols )
{
if( CV_ARE_TYPES_EQ( src, dst ) )
cvCopy( src, dst );
else
cvConvert( src, dst );
}
else
{
if( !CV_ARE_TYPES_EQ( src, dst ))
{
CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));
cvConvert( src, temp );
src = temp;
}
cvTranspose( src, dst );
}
if( ones )
cvSet( ones, cvRealScalar(1.) );
}
else
{
int s_plane_stride, s_stride, d_plane_stride, d_stride, elem_size;
if( !CV_ARE_TYPES_EQ( src, dst ))
{
CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));
cvConvert( src, temp );
src = temp;
}
elem_size = CV_ELEM_SIZE(src->type);
if( s_count == src->cols )
s_plane_stride = src->step / elem_size, s_stride = 1;
else
s_stride = src->step / elem_size, s_plane_stride = 1;
if( d_count == dst->cols )
d_plane_stride = dst->step / elem_size, d_stride = 1;
else
d_stride = dst->step / elem_size, d_plane_stride = 1;
CV_CALL( denom = cvCreateMat( 1, d_count, dst->type ));
if( CV_MAT_DEPTH(dst->type) == CV_32F )
{
const float* xs = src->data.fl;
const float* ys = xs + s_plane_stride;
const float* zs = 0;
const float* ws = xs + (s_dims - 1)*s_plane_stride;
float* iw = denom->data.fl;
float* xd = dst->data.fl;
float* yd = xd + d_plane_stride;
float* zd = 0;
if( d_dims == 3 )
{
zs = ys + s_plane_stride;
zd = yd + d_plane_stride;
}
for( i = 0; i < d_count; i++, ws += s_stride )
{
float t = *ws;
iw[i] = t ? t : 1.f;
}
cvDiv( 0, denom, denom );
if( d_dims == 3 )
for( i = 0; i < d_count; i++ )
{
float w = iw[i];
float x = *xs * w, y = *ys * w, z = *zs * w;
xs += s_stride; ys += s_stride; zs += s_stride;
*xd = x; *yd = y; *zd = z;
xd += d_stride; yd += d_stride; zd += d_stride;
}
else
for( i = 0; i < d_count; i++ )
{
float w = iw[i];
float x = *xs * w, y = *ys * w;
xs += s_stride; ys += s_stride;
*xd = x; *yd = y;
xd += d_stride; yd += d_stride;
}
}
else
{
const double* xs = src->data.db;
const double* ys = xs + s_plane_stride;
const double* zs = 0;
const double* ws = xs + (s_dims - 1)*s_plane_stride;
double* iw = denom->data.db;
double* xd = dst->data.db;
double* yd = xd + d_plane_stride;
double* zd = 0;
if( d_dims == 3 )
{
zs = ys + s_plane_stride;
zd = yd + d_plane_stride;
}
for( i = 0; i < d_count; i++, ws += s_stride )
{
double t = *ws;
iw[i] = t ? t : 1.;
}
cvDiv( 0, denom, denom );
if( d_dims == 3 )
for( i = 0; i < d_count; i++ )
{
double w = iw[i];
double x = *xs * w, y = *ys * w, z = *zs * w;
xs += s_stride; ys += s_stride; zs += s_stride;
*xd = x; *yd = y; *zd = z;
xd += d_stride; yd += d_stride; zd += d_stride;
}
else
for( i = 0; i < d_count; i++ )
{
double w = iw[i];
double x = *xs * w, y = *ys * w;
xs += s_stride; ys += s_stride;
*xd = x; *yd = y;
xd += d_stride; yd += d_stride;
}
}
}
__END__;
cvReleaseMat( &denom );
cvReleaseMat( &temp );
}
CV_IMPL CvSeq*
cvSegmentMotion( const CvArr* mhiimg, CvArr* segmask, CvMemStorage* storage,
double timestamp, double seg_thresh )
{
CvSeq* components = 0;
cv::Ptr<CvMat> mask8u;
CvMat mhistub, *mhi = cvGetMat(mhiimg, &mhistub);
CvMat maskstub, *mask = cvGetMat(segmask, &maskstub);
Cv32suf v, comp_idx;
int stub_val, ts;
int x, y;
if( !storage )
CV_Error( CV_StsNullPtr, "NULL memory storage" );
mhi = cvGetMat( mhi, &mhistub );
mask = cvGetMat( mask, &maskstub );
if( CV_MAT_TYPE( mhi->type ) != CV_32FC1 || CV_MAT_TYPE( mask->type ) != CV_32FC1 )
CV_Error( CV_BadDepth, "Both MHI and the destination mask" );
if( !CV_ARE_SIZES_EQ( mhi, mask ))
CV_Error( CV_StsUnmatchedSizes, "" );
mask8u = cvCreateMat( mhi->rows + 2, mhi->cols + 2, CV_8UC1 );
cvZero( mask8u );
cvZero( mask );
components = cvCreateSeq( CV_SEQ_KIND_GENERIC, sizeof(CvSeq),
sizeof(CvConnectedComp), storage );
v.f = (float)timestamp; ts = v.i;
v.f = FLT_MAX*0.1f; stub_val = v.i;
comp_idx.f = 1;
for( y = 0; y < mhi->rows; y++ )
{
int* mhi_row = (int*)(mhi->data.ptr + y*mhi->step);
for( x = 0; x < mhi->cols; x++ )
{
if( mhi_row[x] == 0 )
mhi_row[x] = stub_val;
}
}
for( y = 0; y < mhi->rows; y++ )
{
int* mhi_row = (int*)(mhi->data.ptr + y*mhi->step);
uchar* mask8u_row = mask8u->data.ptr + (y+1)*mask8u->step + 1;
for( x = 0; x < mhi->cols; x++ )
{
if( mhi_row[x] == ts && mask8u_row[x] == 0 )
{
CvConnectedComp comp;
int x1, y1;
CvScalar _seg_thresh = cvRealScalar(seg_thresh);
CvPoint seed = cvPoint(x,y);
cvFloodFill( mhi, seed, cvRealScalar(0), _seg_thresh, _seg_thresh,
&comp, CV_FLOODFILL_MASK_ONLY + 2*256 + 4, mask8u );
for( y1 = 0; y1 < comp.rect.height; y1++ )
{
int* mask_row1 = (int*)(mask->data.ptr +
(comp.rect.y + y1)*mask->step) + comp.rect.x;
uchar* mask8u_row1 = mask8u->data.ptr +
(comp.rect.y + y1+1)*mask8u->step + comp.rect.x+1;
for( x1 = 0; x1 < comp.rect.width; x1++ )
{
if( mask8u_row1[x1] > 1 )
{
mask8u_row1[x1] = 1;
mask_row1[x1] = comp_idx.i;
}
}
}
comp_idx.f++;
cvSeqPush( components, &comp );
}
}
}
for( y = 0; y < mhi->rows; y++ )
{
int* mhi_row = (int*)(mhi->data.ptr + y*mhi->step);
for( x = 0; x < mhi->cols; x++ )
{
if( mhi_row[x] == stub_val )
mhi_row[x] = 0;
}
}
return components;
}
CV_IMPL void
cvCalcMotionGradient( const CvArr* mhiimg, CvArr* maskimg,
CvArr* orientation,
double delta1, double delta2,
int aperture_size )
{
cv::Ptr<CvMat> dX_min, dY_max;
CvMat mhistub, *mhi = cvGetMat(mhiimg, &mhistub);
CvMat maskstub, *mask = cvGetMat(maskimg, &maskstub);
CvMat orientstub, *orient = cvGetMat(orientation, &orientstub);
CvMat dX_min_row, dY_max_row, orient_row, mask_row;
CvSize size;
int x, y;
float gradient_epsilon = 1e-4f * aperture_size * aperture_size;
float min_delta, max_delta;
if( !CV_IS_MASK_ARR( mask ))
CV_Error( CV_StsBadMask, "" );
if( aperture_size < 3 || aperture_size > 7 || (aperture_size & 1) == 0 )
CV_Error( CV_StsOutOfRange, "aperture_size must be 3, 5 or 7" );
if( delta1 <= 0 || delta2 <= 0 )
CV_Error( CV_StsOutOfRange, "both delta's must be positive" );
if( CV_MAT_TYPE( mhi->type ) != CV_32FC1 || CV_MAT_TYPE( orient->type ) != CV_32FC1 )
CV_Error( CV_StsUnsupportedFormat,
"MHI and orientation must be single-channel floating-point images" );
if( !CV_ARE_SIZES_EQ( mhi, mask ) || !CV_ARE_SIZES_EQ( orient, mhi ))
CV_Error( CV_StsUnmatchedSizes, "" );
if( orient->data.ptr == mhi->data.ptr )
CV_Error( CV_StsInplaceNotSupported, "orientation image must be different from MHI" );
if( delta1 > delta2 )
{
double t;
CV_SWAP( delta1, delta2, t );
}
size = cvGetMatSize( mhi );
min_delta = (float)delta1;
max_delta = (float)delta2;
dX_min = cvCreateMat( mhi->rows, mhi->cols, CV_32F );
dY_max = cvCreateMat( mhi->rows, mhi->cols, CV_32F );
// calc Dx and Dy
cvSobel( mhi, dX_min, 1, 0, aperture_size );
cvSobel( mhi, dY_max, 0, 1, aperture_size );
cvGetRow( dX_min, &dX_min_row, 0 );
cvGetRow( dY_max, &dY_max_row, 0 );
cvGetRow( orient, &orient_row, 0 );
cvGetRow( mask, &mask_row, 0 );
// calc gradient
for( y = 0; y < size.height; y++ )
{
dX_min_row.data.ptr = dX_min->data.ptr + y*dX_min->step;
dY_max_row.data.ptr = dY_max->data.ptr + y*dY_max->step;
orient_row.data.ptr = orient->data.ptr + y*orient->step;
mask_row.data.ptr = mask->data.ptr + y*mask->step;
cvCartToPolar( &dX_min_row, &dY_max_row, 0, &orient_row, 1 );
// make orientation zero where the gradient is very small
for( x = 0; x < size.width; x++ )
{
float dY = dY_max_row.data.fl[x];
float dX = dX_min_row.data.fl[x];
if( fabs(dX) < gradient_epsilon && fabs(dY) < gradient_epsilon )
{
mask_row.data.ptr[x] = 0;
orient_row.data.i[x] = 0;
}
else
mask_row.data.ptr[x] = 1;
}
}
cvErode( mhi, dX_min, 0, (aperture_size-1)/2);
cvDilate( mhi, dY_max, 0, (aperture_size-1)/2);
// mask off pixels which have little motion difference in their neighborhood
for( y = 0; y < size.height; y++ )
{
dX_min_row.data.ptr = dX_min->data.ptr + y*dX_min->step;
dY_max_row.data.ptr = dY_max->data.ptr + y*dY_max->step;
mask_row.data.ptr = mask->data.ptr + y*mask->step;
orient_row.data.ptr = orient->data.ptr + y*orient->step;
for( x = 0; x < size.width; x++ )
{
float d0 = dY_max_row.data.fl[x] - dX_min_row.data.fl[x];
if( mask_row.data.ptr[x] == 0 || d0 < min_delta || max_delta < d0 )
{
mask_row.data.ptr[x] = 0;
orient_row.data.i[x] = 0;
}
}
}
}
CV_IMPL void
cvAbsDiff( const void* srcarr1, const void* srcarr2, void* dstarr )
{
static CvFuncTable adiff_tab;
static int inittab = 0;
CV_FUNCNAME( "cvAbsDiff" );
__BEGIN__;
int coi1 = 0, coi2 = 0, coi3 = 0;
CvMat srcstub1, *src1 = (CvMat*)srcarr1;
CvMat srcstub2, *src2 = (CvMat*)srcarr2;
CvMat dststub, *dst = (CvMat*)dstarr;
int src1_step, src2_step, dst_step;
CvSize size;
int type;
if( !inittab )
{
icvInitAbsDiffTable( &adiff_tab );
inittab = 1;
}
CV_CALL( src1 = cvGetMat( src1, &srcstub1, &coi1 ));
CV_CALL( src2 = cvGetMat( src2, &srcstub2, &coi2 ));
CV_CALL( dst = cvGetMat( dst, &dststub, &coi3 ));
if( coi1 != 0 || coi2 != 0 || coi3 != 0 )
CV_ERROR( CV_BadCOI, "" );
if( !CV_ARE_SIZES_EQ( src1, src2 ) )
CV_ERROR_FROM_CODE( CV_StsUnmatchedSizes );
size = icvGetMatSize( src1 );
type = CV_MAT_TYPE(src1->type);
if( !CV_ARE_SIZES_EQ( src1, dst ))
CV_ERROR_FROM_CODE( CV_StsUnmatchedSizes );
if( !CV_ARE_TYPES_EQ( src1, src2 ))
CV_ERROR_FROM_CODE( CV_StsUnmatchedFormats );
if( !CV_ARE_TYPES_EQ( src1, dst ))
CV_ERROR_FROM_CODE( CV_StsUnmatchedFormats );
size.width *= CV_MAT_CN( type );
src1_step = src1->step;
src2_step = src2->step;
dst_step = dst->step;
if( CV_IS_MAT_CONT( src1->type & src2->type & dst->type ))
{
size.width *= size.height;
size.height = 1;
src1_step = src2_step = dst_step = CV_STUB_STEP;
}
{
CvFunc2D_3A func = (CvFunc2D_3A)
(adiff_tab.fn_2d[CV_MAT_DEPTH(type)]);
if( !func )
CV_ERROR( CV_StsUnsupportedFormat, "" );
IPPI_CALL( func( src1->data.ptr, src1_step, src2->data.ptr, src2_step,
dst->data.ptr, dst_step, size ));
}
__END__;
}
CV_IMPL void
cvPyrMeanShiftFiltering( const CvArr* srcarr, CvArr* dstarr,
double sp0, double sr, int max_level,
CvTermCriteria termcrit )
{
const int cn = 3;
const int MAX_LEVELS = 8;
CvMat* src_pyramid[MAX_LEVELS+1];
CvMat* dst_pyramid[MAX_LEVELS+1];
CvMat* mask0 = 0;
int i, j, level;
//uchar* submask = 0;
#define cdiff(ofs0) (tab[c0-dptr[ofs0]+255] + \
tab[c1-dptr[(ofs0)+1]+255] + tab[c2-dptr[(ofs0)+2]+255] >= isr22)
memset( src_pyramid, 0, sizeof(src_pyramid) );
memset( dst_pyramid, 0, sizeof(dst_pyramid) );
CV_FUNCNAME( "cvPyrMeanShiftFiltering" );
__BEGIN__;
double sr2 = sr * sr;
int isr2 = cvRound(sr2), isr22 = MAX(isr2,16);
int tab[768];
CvMat sstub0, *src0;
CvMat dstub0, *dst0;
CV_CALL( src0 = cvGetMat( srcarr, &sstub0 ));
CV_CALL( dst0 = cvGetMat( dstarr, &dstub0 ));
if( CV_MAT_TYPE(src0->type) != CV_8UC3 )
CV_ERROR( CV_StsUnsupportedFormat, "Only 8-bit, 3-channel images are supported" );
if( !CV_ARE_TYPES_EQ( src0, dst0 ))
CV_ERROR( CV_StsUnmatchedFormats, "The input and output images must have the same type" );
if( !CV_ARE_SIZES_EQ( src0, dst0 ))
CV_ERROR( CV_StsUnmatchedSizes, "The input and output images must have the same size" );
if( (unsigned)max_level > (unsigned)MAX_LEVELS )
CV_ERROR( CV_StsOutOfRange, "The number of pyramid levels is too large or negative" );
if( !(termcrit.type & CV_TERMCRIT_ITER) )
termcrit.max_iter = 5;
termcrit.max_iter = MAX(termcrit.max_iter,1);
termcrit.max_iter = MIN(termcrit.max_iter,100);
if( !(termcrit.type & CV_TERMCRIT_EPS) )
termcrit.epsilon = 1.f;
termcrit.epsilon = MAX(termcrit.epsilon, 0.f);
for( i = 0; i < 768; i++ )
tab[i] = (i - 255)*(i - 255);
// 1. construct pyramid
src_pyramid[0] = src0;
dst_pyramid[0] = dst0;
for( level = 1; level <= max_level; level++ )
{
CV_CALL( src_pyramid[level] = cvCreateMat( (src_pyramid[level-1]->rows+1)/2,
(src_pyramid[level-1]->cols+1)/2, src_pyramid[level-1]->type ));
CV_CALL( dst_pyramid[level] = cvCreateMat( src_pyramid[level]->rows,
src_pyramid[level]->cols, src_pyramid[level]->type ));
CV_CALL( cvPyrDown( src_pyramid[level-1], src_pyramid[level] ));
//CV_CALL( cvResize( src_pyramid[level-1], src_pyramid[level], CV_INTER_AREA ));
}
CV_CALL( mask0 = cvCreateMat( src0->rows, src0->cols, CV_8UC1 ));
//CV_CALL( submask = (uchar*)cvAlloc( (sp+2)*(sp+2) ));
// 2. apply meanshift, starting from the pyramid top (i.e. the smallest layer)
for( level = max_level; level >= 0; level-- )
{
CvMat* src = src_pyramid[level];
CvSize size = cvGetMatSize(src);
uchar* sptr = src->data.ptr;
int sstep = src->step;
uchar* mask = 0;
int mstep = 0;
uchar* dptr;
int dstep;
float sp = (float)(sp0 / (1 << level));
sp = MAX( sp, 1 );
if( level < max_level )
{
CvSize size1 = cvGetMatSize(dst_pyramid[level+1]);
CvMat m = cvMat( size.height, size.width, CV_8UC1, mask0->data.ptr );
dstep = dst_pyramid[level+1]->step;
dptr = dst_pyramid[level+1]->data.ptr + dstep + cn;
mstep = m.step;
mask = m.data.ptr + mstep;
//cvResize( dst_pyramid[level+1], dst_pyramid[level], CV_INTER_CUBIC );
cvPyrUp( dst_pyramid[level+1], dst_pyramid[level] );
cvZero( &m );
for( i = 1; i < size1.height-1; i++, dptr += dstep - (size1.width-2)*3, mask += mstep*2 )
{
for( j = 1; j < size1.width-1; j++, dptr += cn )
{
int c0 = dptr[0], c1 = dptr[1], c2 = dptr[2];
mask[j*2 - 1] = cdiff(-3) || cdiff(3) || cdiff(-dstep-3) || cdiff(-dstep) ||
cdiff(-dstep+3) || cdiff(dstep-3) || cdiff(dstep) || cdiff(dstep+3);
}
}
cvDilate( &m, &m, 0, 1 );
mask = m.data.ptr;
}
dptr = dst_pyramid[level]->data.ptr;
dstep = dst_pyramid[level]->step;
for( i = 0; i < size.height; i++, sptr += sstep - size.width*3,
dptr += dstep - size.width*3,
mask += mstep )
{
for( j = 0; j < size.width; j++, sptr += 3, dptr += 3 )
{
int x0 = j, y0 = i, x1, y1, iter;
int c0, c1, c2;
if( mask && !mask[j] )
continue;
c0 = sptr[0], c1 = sptr[1], c2 = sptr[2];
// iterate meanshift procedure
for( iter = 0; iter < termcrit.max_iter; iter++ )
{
uchar* ptr;
int x, y, count = 0;
int minx, miny, maxx, maxy;
int s0 = 0, s1 = 0, s2 = 0, sx = 0, sy = 0;
double icount;
int stop_flag;
//mean shift: process pixels in window (p-sigmaSp)x(p+sigmaSp)
minx = cvRound(x0 - sp); minx = MAX(minx, 0);
miny = cvRound(y0 - sp); miny = MAX(miny, 0);
maxx = cvRound(x0 + sp); maxx = MIN(maxx, size.width-1);
maxy = cvRound(y0 + sp); maxy = MIN(maxy, size.height-1);
ptr = sptr + (miny - i)*sstep + (minx - j)*3;
for( y = miny; y <= maxy; y++, ptr += sstep - (maxx-minx+1)*3 )
{
int row_count = 0;
x = minx;
for( ; x + 3 <= maxx; x += 4, ptr += 12 )
{
int t0 = ptr[0], t1 = ptr[1], t2 = ptr[2];
if( tab[t0-c0+255] + tab[t1-c1+255] + tab[t2-c2+255] <= isr2 )
{
s0 += t0; s1 += t1; s2 += t2;
sx += x; row_count++;
}
t0 = ptr[3], t1 = ptr[4], t2 = ptr[5];
if( tab[t0-c0+255] + tab[t1-c1+255] + tab[t2-c2+255] <= isr2 )
{
s0 += t0; s1 += t1; s2 += t2;
sx += x+1; row_count++;
}
t0 = ptr[6], t1 = ptr[7], t2 = ptr[8];
if( tab[t0-c0+255] + tab[t1-c1+255] + tab[t2-c2+255] <= isr2 )
{
s0 += t0; s1 += t1; s2 += t2;
sx += x+2; row_count++;
}
t0 = ptr[9], t1 = ptr[10], t2 = ptr[11];
if( tab[t0-c0+255] + tab[t1-c1+255] + tab[t2-c2+255] <= isr2 )
{
s0 += t0; s1 += t1; s2 += t2;
sx += x+3; row_count++;
}
}
for( ; x <= maxx; x++, ptr += 3 )
{
int t0 = ptr[0], t1 = ptr[1], t2 = ptr[2];
if( tab[t0-c0+255] + tab[t1-c1+255] + tab[t2-c2+255] <= isr2 )
{
s0 += t0; s1 += t1; s2 += t2;
sx += x; row_count++;
}
}
count += row_count;
sy += y*row_count;
}
if( count == 0 )
break;
icount = 1./count;
x1 = cvRound(sx*icount);
y1 = cvRound(sy*icount);
s0 = cvRound(s0*icount);
s1 = cvRound(s1*icount);
s2 = cvRound(s2*icount);
stop_flag = (x0 == x1 && y0 == y1) || abs(x1-x0) + abs(y1-y0) +
tab[s0 - c0 + 255] + tab[s1 - c1 + 255] +
tab[s2 - c2 + 255] <= termcrit.epsilon;
x0 = x1; y0 = y1;
c0 = s0; c1 = s1; c2 = s2;
if( stop_flag )
break;
}
dptr[0] = (uchar)c0;
dptr[1] = (uchar)c1;
dptr[2] = (uchar)c2;
}
}
}
__END__;
for( i = 1; i <= MAX_LEVELS; i++ )
{
cvReleaseMat( &src_pyramid[i] );
cvReleaseMat( &dst_pyramid[i] );
}
cvReleaseMat( &mask0 );
}
CV_IMPL void
cvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ,
CvMat *matrixQx, CvMat *matrixQy, CvMat *matrixQz,
CvPoint3D64f *eulerAngles)
{
CV_FUNCNAME("cvRQDecomp3x3");
__BEGIN__;
double _M[3][3], _R[3][3], _Q[3][3];
CvMat M = cvMat(3, 3, CV_64F, _M);
CvMat R = cvMat(3, 3, CV_64F, _R);
CvMat Q = cvMat(3, 3, CV_64F, _Q);
double z, c, s;
/* Validate parameters. */
CV_ASSERT( CV_IS_MAT(matrixM) && CV_IS_MAT(matrixR) && CV_IS_MAT(matrixQ) &&
matrixM->cols == 3 && matrixM->rows == 3 &&
CV_ARE_SIZES_EQ(matrixM, matrixR) && CV_ARE_SIZES_EQ(matrixM, matrixQ));
cvConvert(matrixM, &M);
{
/* Find Givens rotation Q_x for x axis (left multiplication). */
/*
( 1 0 0 )
Qx = ( 0 c s ), c = m33/sqrt(m32^2 + m33^2), s = m32/sqrt(m32^2 + m33^2)
( 0 -s c )
*/
s = _M[2][1];
c = _M[2][2];
z = 1./sqrt(c * c + s * s + DBL_EPSILON);
c *= z;
s *= z;
double _Qx[3][3] = { {1, 0, 0}, {0, c, s}, {0, -s, c} };
CvMat Qx = cvMat(3, 3, CV_64F, _Qx);
cvMatMul(&M, &Qx, &R);
assert(fabs(_R[2][1]) < FLT_EPSILON);
_R[2][1] = 0;
/* Find Givens rotation for y axis. */
/*
( c 0 s )
Qy = ( 0 1 0 ), c = m33/sqrt(m31^2 + m33^2), s = m31/sqrt(m31^2 + m33^2)
(-s 0 c )
*/
s = _R[2][0];
c = _R[2][2];
z = 1./sqrt(c * c + s * s + DBL_EPSILON);
c *= z;
s *= z;
double _Qy[3][3] = { {c, 0, s}, {0, 1, 0}, {-s, 0, c} };
CvMat Qy = cvMat(3, 3, CV_64F, _Qy);
cvMatMul(&R, &Qy, &M);
assert(fabs(_M[2][0]) < FLT_EPSILON);
_M[2][0] = 0;
/* Find Givens rotation for z axis. */
/*
( c s 0 )
Qz = (-s c 0 ), c = m22/sqrt(m21^2 + m22^2), s = m21/sqrt(m21^2 + m22^2)
( 0 0 1 )
*/
s = _M[1][0];
c = _M[1][1];
z = 1./sqrt(c * c + s * s + DBL_EPSILON);
c *= z;
s *= z;
double _Qz[3][3] = { {c, s, 0}, {-s, c, 0}, {0, 0, 1} };
CvMat Qz = cvMat(3, 3, CV_64F, _Qz);
cvMatMul(&M, &Qz, &R);
assert(fabs(_R[1][0]) < FLT_EPSILON);
_R[1][0] = 0;
// Solve the decomposition ambiguity.
// Diagonal entries of R, except the last one, shall be positive.
// Further rotate R by 180 degree if necessary
if( _R[0][0] < 0 )
{
if( _R[1][1] < 0 )
{
// rotate around z for 180 degree, i.e. a rotation matrix of
// [-1, 0, 0],
// [ 0, -1, 0],
// [ 0, 0, 1]
_R[0][0] *= -1;
_R[0][1] *= -1;
_R[1][1] *= -1;
_Qz[0][0] *= -1;
_Qz[0][1] *= -1;
_Qz[1][0] *= -1;
_Qz[1][1] *= -1;
}
else
{
// rotate around y for 180 degree, i.e. a rotation matrix of
// [-1, 0, 0],
// [ 0, 1, 0],
// [ 0, 0, -1]
_R[0][0] *= -1;
_R[0][2] *= -1;
_R[1][2] *= -1;
_R[2][2] *= -1;
cvTranspose( &Qz, &Qz );
_Qy[0][0] *= -1;
_Qy[0][2] *= -1;
_Qy[2][0] *= -1;
_Qy[2][2] *= -1;
}
}
else if( _R[1][1] < 0 )
{
// ??? for some reason, we never get here ???
// rotate around x for 180 degree, i.e. a rotation matrix of
// [ 1, 0, 0],
// [ 0, -1, 0],
// [ 0, 0, -1]
_R[0][1] *= -1;
_R[0][2] *= -1;
_R[1][1] *= -1;
_R[1][2] *= -1;
_R[2][2] *= -1;
cvTranspose( &Qz, &Qz );
cvTranspose( &Qy, &Qy );
_Qx[1][1] *= -1;
_Qx[1][2] *= -1;
_Qx[2][1] *= -1;
_Qx[2][2] *= -1;
}
// calculate the euler angle
if( eulerAngles )
{
eulerAngles->x = acos(_Qx[1][1]) * (_Qx[1][2] >= 0 ? 1 : -1) * (180.0 / CV_PI);
eulerAngles->y = acos(_Qy[0][0]) * (_Qy[0][2] >= 0 ? 1 : -1) * (180.0 / CV_PI);
eulerAngles->z = acos(_Qz[0][0]) * (_Qz[0][1] >= 0 ? 1 : -1) * (180.0 / CV_PI);
}
/* Calulate orthogonal matrix. */
/*
Q = QzT * QyT * QxT
*/
cvGEMM( &Qz, &Qy, 1, 0, 0, &M, CV_GEMM_A_T + CV_GEMM_B_T );
cvGEMM( &M, &Qx, 1, 0, 0, &Q, CV_GEMM_B_T );
/* Save R and Q matrices. */
cvConvert( &R, matrixR );
cvConvert( &Q, matrixQ );
if( matrixQx )
cvConvert(&Qx, matrixQx);
if( matrixQy )
cvConvert(&Qy, matrixQy);
if( matrixQz )
cvConvert(&Qz, matrixQz);
}
__END__;
}
void cvBGCodeBookUpdate( CvBGCodeBookModel* model, const CvArr* _image,
CvRect roi, const CvArr* _mask )
{
CV_FUNCNAME( "cvBGCodeBookUpdate" );
__BEGIN__;
CvMat stub, *image = cvGetMat( _image, &stub );
CvMat mstub, *mask = _mask ? cvGetMat( _mask, &mstub ) : 0;
int i, x, y, T;
int nblocks;
uchar cb0, cb1, cb2;
CvBGCodeBookElem* freeList;
CV_ASSERT( model && CV_MAT_TYPE(image->type) == CV_8UC3 &&
(!mask || (CV_IS_MASK_ARR(mask) && CV_ARE_SIZES_EQ(image, mask))) );
if( roi.x == 0 && roi.y == 0 && roi.width == 0 && roi.height == 0 )
{
roi.width = image->cols;
roi.height = image->rows;
}
else
CV_ASSERT( (unsigned)roi.x < (unsigned)image->cols &&
(unsigned)roi.y < (unsigned)image->rows &&
roi.width >= 0 && roi.height >= 0 &&
roi.x + roi.width <= image->cols &&
roi.y + roi.height <= image->rows );
if( image->cols != model->size.width || image->rows != model->size.height )
{
cvClearMemStorage( model->storage );
model->freeList = 0;
cvFree( &model->cbmap );
int bufSz = image->cols*image->rows*sizeof(model->cbmap[0]);
model->cbmap = (CvBGCodeBookElem**)cvAlloc(bufSz);
memset( model->cbmap, 0, bufSz );
model->size = cvSize(image->cols, image->rows);
}
icvInitSatTab();
cb0 = model->cbBounds[0];
cb1 = model->cbBounds[1];
cb2 = model->cbBounds[2];
T = ++model->t;
freeList = model->freeList;
nblocks = (int)((model->storage->block_size - sizeof(CvMemBlock))/sizeof(*freeList));
nblocks = MIN( nblocks, 1024 );
CV_ASSERT( nblocks > 0 );
for( y = 0; y < roi.height; y++ )
{
const uchar* p = image->data.ptr + image->step*(y + roi.y) + roi.x*3;
const uchar* m = mask ? mask->data.ptr + mask->step*(y + roi.y) + roi.x : 0;
CvBGCodeBookElem** cb = model->cbmap + image->cols*(y + roi.y) + roi.x;
for( x = 0; x < roi.width; x++, p += 3, cb++ )
{
CvBGCodeBookElem *e, *found = 0;
uchar p0, p1, p2, l0, l1, l2, h0, h1, h2;
int negRun;
if( m && m[x] == 0 )
continue;
p0 = p[0]; p1 = p[1]; p2 = p[2];
l0 = SAT_8U(p0 - cb0); l1 = SAT_8U(p1 - cb1); l2 = SAT_8U(p2 - cb2);
h0 = SAT_8U(p0 + cb0); h1 = SAT_8U(p1 + cb1); h2 = SAT_8U(p2 + cb2);
for( e = *cb; e != 0; e = e->next )
{
if( e->learnMin[0] <= p0 && p0 <= e->learnMax[0] &&
e->learnMin[1] <= p1 && p1 <= e->learnMax[1] &&
e->learnMin[2] <= p2 && p2 <= e->learnMax[2] )
{
e->tLastUpdate = T;
e->boxMin[0] = MIN(e->boxMin[0], p0);
e->boxMax[0] = MAX(e->boxMax[0], p0);
e->boxMin[1] = MIN(e->boxMin[1], p1);
e->boxMax[1] = MAX(e->boxMax[1], p1);
e->boxMin[2] = MIN(e->boxMin[2], p2);
e->boxMax[2] = MAX(e->boxMax[2], p2);
// no need to use SAT_8U for updated learnMin[i] & learnMax[i] here,
// as the bounding li & hi are already within 0..255.
if( e->learnMin[0] > l0 ) e->learnMin[0]--;
if( e->learnMax[0] < h0 ) e->learnMax[0]++;
if( e->learnMin[1] > l1 ) e->learnMin[1]--;
if( e->learnMax[1] < h1 ) e->learnMax[1]++;
if( e->learnMin[2] > l2 ) e->learnMin[2]--;
if( e->learnMax[2] < h2 ) e->learnMax[2]++;
found = e;
break;
}
negRun = T - e->tLastUpdate;
e->stale = MAX( e->stale, negRun );
}
for( ; e != 0; e = e->next )
{
negRun = T - e->tLastUpdate;
e->stale = MAX( e->stale, negRun );
}
if( !found )
{
if( !freeList )
{
freeList = (CvBGCodeBookElem*)cvMemStorageAlloc(model->storage,
nblocks*sizeof(*freeList));
for( i = 0; i < nblocks-1; i++ )
freeList[i].next = &freeList[i+1];
freeList[nblocks-1].next = 0;
}
e = freeList;
freeList = freeList->next;
e->learnMin[0] = l0; e->learnMax[0] = h0;
e->learnMin[1] = l1; e->learnMax[1] = h1;
e->learnMin[2] = l2; e->learnMax[2] = h2;
e->boxMin[0] = e->boxMax[0] = p0;
e->boxMin[1] = e->boxMax[1] = p1;
e->boxMin[2] = e->boxMax[2] = p2;
e->tLastUpdate = T;
e->stale = 0;
e->next = *cb;
*cb = e;
}
}
}
model->freeList = freeList;
__END__;
}
CV_IMPL void
cvRunningAvg( const void* arrY, void* arrU,
double alpha, const void* maskarr )
{
static CvFuncTable acc_tab;
static CvBigFuncTable accmask_tab;
static int inittab = 0;
CV_FUNCNAME( "cvRunningAvg" );
__BEGIN__;
int coi1, coi2;
int type;
int mat_step, sum_step, mask_step = 0;
CvSize size;
CvMat stub, *mat = (CvMat*)arrY;
CvMat sumstub, *sum = (CvMat*)arrU;
CvMat maskstub, *mask = (CvMat*)maskarr;
if( !inittab )
{
icvInitAddWeightedTable( &acc_tab, &accmask_tab );
inittab = 1;
}
CV_CALL( mat = cvGetMat( mat, &stub, &coi1 ));
CV_CALL( sum = cvGetMat( sum, &sumstub, &coi2 ));
if( coi1 != 0 || coi2 != 0 )
CV_ERROR( CV_BadCOI, "" );
if( !CV_ARE_CNS_EQ( mat, sum ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( CV_MAT_DEPTH( sum->type ) != CV_32F )
CV_ERROR( CV_BadDepth, "" );
if( !CV_ARE_SIZES_EQ( mat, sum ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
size = cvGetMatSize( mat );
type = CV_MAT_TYPE( mat->type );
mat_step = mat->step;
sum_step = sum->step;
if( !mask )
{
CvAddWeightedFunc func = (CvAddWeightedFunc)acc_tab.fn_2d[CV_MAT_DEPTH(type)];
if( !func )
CV_ERROR( CV_StsUnsupportedFormat, "" );
size.width *= CV_MAT_CN(type);
if( CV_IS_MAT_CONT( mat->type & sum->type ))
{
size.width *= size.height;
mat_step = sum_step = CV_STUB_STEP;
size.height = 1;
}
IPPI_CALL( func( mat->data.ptr, mat_step,
sum->data.ptr, sum_step, size, (float)alpha ));
}
else
{
CvAddWeightedMaskFunc func = (CvAddWeightedMaskFunc)accmask_tab.fn_2d[type];
if( !func )
CV_ERROR( CV_StsUnsupportedFormat, "" );
CV_CALL( mask = cvGetMat( mask, &maskstub ));
if( !CV_IS_MASK_ARR( mask ))
CV_ERROR( CV_StsBadMask, "" );
if( !CV_ARE_SIZES_EQ( mat, mask ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
mask_step = mask->step;
if( CV_IS_MAT_CONT( mat->type & sum->type & mask->type ))
{
size.width *= size.height;
mat_step = sum_step = mask_step = CV_STUB_STEP;
size.height = 1;
}
IPPI_CALL( func( mat->data.ptr, mat_step, mask->data.ptr, mask_step,
sum->data.ptr, sum_step, size, (float)alpha ));
}
__END__;
}
CV_IMPL void
cvMultiplyAcc( const void* arrA, const void* arrB,
void* acc, const void* maskarr )
{
static CvFuncTable acc_tab;
static CvBigFuncTable accmask_tab;
static int inittab = 0;
CV_FUNCNAME( "cvMultiplyAcc" );
__BEGIN__;
int coi1, coi2, coi3;
int type;
int mat1_step, mat2_step, sum_step, mask_step = 0;
CvSize size;
CvMat stub1, *mat1 = (CvMat*)arrA;
CvMat stub2, *mat2 = (CvMat*)arrB;
CvMat sumstub, *sum = (CvMat*)acc;
CvMat maskstub, *mask = (CvMat*)maskarr;
if( !inittab )
{
icvInitAddProductTable( &acc_tab, &accmask_tab );
inittab = 1;
}
CV_CALL( mat1 = cvGetMat( mat1, &stub1, &coi1 ));
CV_CALL( mat2 = cvGetMat( mat2, &stub2, &coi2 ));
CV_CALL( sum = cvGetMat( sum, &sumstub, &coi3 ));
if( coi1 != 0 || coi2 != 0 || coi3 != 0 )
CV_ERROR( CV_BadCOI, "" );
if( !CV_ARE_CNS_EQ( mat1, mat2 ) || !CV_ARE_CNS_EQ( mat1, sum ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( CV_MAT_DEPTH( sum->type ) != CV_32F )
CV_ERROR( CV_BadDepth, "" );
if( !CV_ARE_SIZES_EQ( mat1, sum ) || !CV_ARE_SIZES_EQ( mat2, sum ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
size = cvGetMatSize( mat1 );
type = CV_MAT_TYPE( mat1->type );
mat1_step = mat1->step;
mat2_step = mat2->step;
sum_step = sum->step;
if( !mask )
{
CvFunc2D_3A func = (CvFunc2D_3A)acc_tab.fn_2d[CV_MAT_DEPTH(type)];
if( !func )
CV_ERROR( CV_StsUnsupportedFormat, "" );
size.width *= CV_MAT_CN(type);
if( CV_IS_MAT_CONT( mat1->type & mat2->type & sum->type ))
{
size.width *= size.height;
mat1_step = mat2_step = sum_step = CV_STUB_STEP;
size.height = 1;
}
IPPI_CALL( func( mat1->data.ptr, mat1_step, mat2->data.ptr, mat2_step,
sum->data.ptr, sum_step, size ));
}
else
{
CvFunc2D_4A func = (CvFunc2D_4A)accmask_tab.fn_2d[type];
if( !func )
CV_ERROR( CV_StsUnsupportedFormat, "" );
CV_CALL( mask = cvGetMat( mask, &maskstub ));
if( !CV_IS_MASK_ARR( mask ))
CV_ERROR( CV_StsBadMask, "" );
if( !CV_ARE_SIZES_EQ( mat1, mask ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
mask_step = mask->step;
if( CV_IS_MAT_CONT( mat1->type & mat2->type & sum->type & mask->type ))
{
size.width *= size.height;
mat1_step = mat2_step = sum_step = mask_step = CV_STUB_STEP;
size.height = 1;
}
IPPI_CALL( func( mat1->data.ptr, mat1_step, mat2->data.ptr, mat2_step,
mask->data.ptr, mask_step,
sum->data.ptr, sum_step, size ));
}
__END__;
}
CV_IMPL void
cvAcc( const void* arr, void* sumarr, const void* maskarr )
{
static CvFuncTable acc_tab;
static CvBigFuncTable accmask_tab;
static int inittab = 0;
CV_FUNCNAME( "cvAcc" );
__BEGIN__;
int type, sumdepth;
int mat_step, sum_step, mask_step = 0;
CvSize size;
CvMat stub, *mat = (CvMat*)arr;
CvMat sumstub, *sum = (CvMat*)sumarr;
CvMat maskstub, *mask = (CvMat*)maskarr;
if( !inittab )
{
icvInitAddTable( &acc_tab, &accmask_tab );
inittab = 1;
}
if( !CV_IS_MAT( mat ) || !CV_IS_MAT( sum ))
{
int coi1 = 0, coi2 = 0;
CV_CALL( mat = cvGetMat( mat, &stub, &coi1 ));
CV_CALL( sum = cvGetMat( sum, &sumstub, &coi2 ));
if( coi1 + coi2 != 0 )
CV_ERROR( CV_BadCOI, "" );
}
if( CV_MAT_DEPTH( sum->type ) != CV_32F )
CV_ERROR( CV_BadDepth, "" );
if( !CV_ARE_CNS_EQ( mat, sum ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
sumdepth = CV_MAT_DEPTH( sum->type );
if( sumdepth != CV_32F && (maskarr != 0 || sumdepth != CV_64F))
CV_ERROR( CV_BadDepth, "Bad accumulator type" );
if( !CV_ARE_SIZES_EQ( mat, sum ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
size = cvGetMatSize( mat );
type = CV_MAT_TYPE( mat->type );
mat_step = mat->step;
sum_step = sum->step;
if( !mask )
{
CvFunc2D_2A func=(CvFunc2D_2A)acc_tab.fn_2d[CV_MAT_DEPTH(type)];
if( !func )
CV_ERROR( CV_StsUnsupportedFormat, "Unsupported type combination" );
size.width *= CV_MAT_CN(type);
if( CV_IS_MAT_CONT( mat->type & sum->type ))
{
size.width *= size.height;
mat_step = sum_step = CV_STUB_STEP;
size.height = 1;
}
IPPI_CALL( func( mat->data.ptr, mat_step, sum->data.ptr, sum_step, size ));
}
else
{
CvFunc2D_3A func = (CvFunc2D_3A)accmask_tab.fn_2d[type];
if( !func )
CV_ERROR( CV_StsUnsupportedFormat, "" );
CV_CALL( mask = cvGetMat( mask, &maskstub ));
if( !CV_IS_MASK_ARR( mask ))
CV_ERROR( CV_StsBadMask, "" );
if( !CV_ARE_SIZES_EQ( mat, mask ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
mask_step = mask->step;
if( CV_IS_MAT_CONT( mat->type & sum->type & mask->type ))
{
size.width *= size.height;
mat_step = sum_step = mask_step = CV_STUB_STEP;
size.height = 1;
}
IPPI_CALL( func( mat->data.ptr, mat_step, mask->data.ptr, mask_step,
sum->data.ptr, sum_step, size ));
}
__END__;
}
CV_IMPL void
cvPreCornerDetect( const void* srcarr, void* dstarr, int aperture_size )
{
CvSepFilter dx_filter, dy_filter, d2x_filter, d2y_filter, dxy_filter;
CvMat *Dx = 0, *Dy = 0, *D2x = 0, *D2y = 0, *Dxy = 0;
CvMat *tempsrc = 0;
int buf_size = 1 << 12;
CV_FUNCNAME( "cvPreCornerDetect" );
__BEGIN__;
int i, j, y, dst_y = 0, max_dy, delta = 0;
int temp_step = 0, d_step;
uchar* shifted_ptr = 0;
int depth, d_depth;
int stage = CV_START;
CvSobelFixedIPPFunc ipp_sobel_vert = 0, ipp_sobel_horiz = 0,
ipp_sobel_vert_second = 0, ipp_sobel_horiz_second = 0,
ipp_sobel_cross = 0;
CvSize el_size, size, stripe_size;
int aligned_width;
CvPoint el_anchor;
double factor;
CvMat stub, *src = (CvMat*)srcarr;
CvMat dststub, *dst = (CvMat*)dstarr;
bool use_ipp = false;
CV_CALL( src = cvGetMat( srcarr, &stub ));
CV_CALL( dst = cvGetMat( dst, &dststub ));
if( CV_MAT_TYPE(src->type) != CV_8UC1 && CV_MAT_TYPE(src->type) != CV_32FC1 ||
CV_MAT_TYPE(dst->type) != CV_32FC1 )
CV_ERROR( CV_StsUnsupportedFormat, "Input must be 8uC1 or 32fC1, output must be 32fC1" );
if( !CV_ARE_SIZES_EQ( src, dst ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
if( aperture_size == CV_SCHARR )
CV_ERROR( CV_StsOutOfRange, "CV_SCHARR is not supported by this function" );
if( aperture_size < 3 || aperture_size > 7 || !(aperture_size & 1) )
CV_ERROR( CV_StsOutOfRange,
"Derivative filter aperture size must be 3, 5 or 7" );
depth = CV_MAT_DEPTH(src->type);
d_depth = depth == CV_8U ? CV_16S : CV_32F;
size = cvGetMatSize(src);
aligned_width = cvAlign(size.width, 4);
el_size = cvSize( aperture_size, aperture_size );
el_anchor = cvPoint( aperture_size/2, aperture_size/2 );
if( aperture_size <= 5 && icvFilterSobelVert_8u16s_C1R_p )
{
if( depth == CV_8U )
{
ipp_sobel_vert = icvFilterSobelVert_8u16s_C1R_p;
ipp_sobel_horiz = icvFilterSobelHoriz_8u16s_C1R_p;
ipp_sobel_vert_second = icvFilterSobelVertSecond_8u16s_C1R_p;
ipp_sobel_horiz_second = icvFilterSobelHorizSecond_8u16s_C1R_p;
ipp_sobel_cross = icvFilterSobelCross_8u16s_C1R_p;
}
else if( depth == CV_32F )
{
ipp_sobel_vert = icvFilterSobelVert_32f_C1R_p;
ipp_sobel_horiz = icvFilterSobelHoriz_32f_C1R_p;
ipp_sobel_vert_second = icvFilterSobelVertSecond_32f_C1R_p;
ipp_sobel_horiz_second = icvFilterSobelHorizSecond_32f_C1R_p;
ipp_sobel_cross = icvFilterSobelCross_32f_C1R_p;
}
}
if( ipp_sobel_vert && ipp_sobel_horiz && ipp_sobel_vert_second &&
ipp_sobel_horiz_second && ipp_sobel_cross )
{
CV_CALL( tempsrc = icvIPPFilterInit( src, buf_size, el_size ));
shifted_ptr = tempsrc->data.ptr + el_anchor.y*tempsrc->step +
el_anchor.x*CV_ELEM_SIZE(depth);
temp_step = tempsrc->step ? tempsrc->step : CV_STUB_STEP;
max_dy = tempsrc->rows - aperture_size + 1;
use_ipp = true;
}
else
{
ipp_sobel_vert = ipp_sobel_horiz = 0;
ipp_sobel_vert_second = ipp_sobel_horiz_second = ipp_sobel_cross = 0;
dx_filter.init_deriv( size.width, depth, d_depth, 1, 0, aperture_size );
dy_filter.init_deriv( size.width, depth, d_depth, 0, 1, aperture_size );
d2x_filter.init_deriv( size.width, depth, d_depth, 2, 0, aperture_size );
d2y_filter.init_deriv( size.width, depth, d_depth, 0, 2, aperture_size );
dxy_filter.init_deriv( size.width, depth, d_depth, 1, 1, aperture_size );
max_dy = buf_size / src->cols;
max_dy = MAX( max_dy, aperture_size );
}
CV_CALL( Dx = cvCreateMat( max_dy, aligned_width, d_depth ));
CV_CALL( Dy = cvCreateMat( max_dy, aligned_width, d_depth ));
CV_CALL( D2x = cvCreateMat( max_dy, aligned_width, d_depth ));
CV_CALL( D2y = cvCreateMat( max_dy, aligned_width, d_depth ));
CV_CALL( Dxy = cvCreateMat( max_dy, aligned_width, d_depth ));
Dx->cols = Dy->cols = D2x->cols = D2y->cols = Dxy->cols = size.width;
if( !use_ipp )
max_dy -= aperture_size - 1;
d_step = Dx->step ? Dx->step : CV_STUB_STEP;
stripe_size = size;
factor = 1 << (aperture_size - 1);
if( depth == CV_8U )
factor *= 255;
factor = 1./(factor * factor * factor);
aperture_size = aperture_size * 10 + aperture_size;
for( y = 0; y < size.height; y += delta )
{
if( !use_ipp )
{
delta = MIN( size.height - y, max_dy );
CvRect roi = cvRect(0,y,size.width,delta);
CvPoint origin=cvPoint(0,0);
if( y + delta == size.height )
stage = stage & CV_START ? CV_START + CV_END : CV_END;
dx_filter.process(src,Dx,roi,origin,stage);
dy_filter.process(src,Dy,roi,origin,stage);
d2x_filter.process(src,D2x,roi,origin,stage);
d2y_filter.process(src,D2y,roi,origin,stage);
stripe_size.height = dxy_filter.process(src,Dxy,roi,origin,stage);
}
else
{
delta = icvIPPFilterNextStripe( src, tempsrc, y, el_size, el_anchor );
stripe_size.height = delta;
IPPI_CALL( ipp_sobel_vert( shifted_ptr, temp_step,
Dx->data.ptr, d_step, stripe_size, aperture_size ));
IPPI_CALL( ipp_sobel_horiz( shifted_ptr, temp_step,
Dy->data.ptr, d_step, stripe_size, aperture_size ));
IPPI_CALL( ipp_sobel_vert_second( shifted_ptr, temp_step,
D2x->data.ptr, d_step, stripe_size, aperture_size ));
IPPI_CALL( ipp_sobel_horiz_second( shifted_ptr, temp_step,
D2y->data.ptr, d_step, stripe_size, aperture_size ));
IPPI_CALL( ipp_sobel_cross( shifted_ptr, temp_step,
Dxy->data.ptr, d_step, stripe_size, aperture_size ));
}
for( i = 0; i < stripe_size.height; i++, dst_y++ )
{
float* dstdata = (float*)(dst->data.ptr + dst_y*dst->step);
if( d_depth == CV_16S )
{
const short* dxdata = (const short*)(Dx->data.ptr + i*Dx->step);
const short* dydata = (const short*)(Dy->data.ptr + i*Dy->step);
const short* d2xdata = (const short*)(D2x->data.ptr + i*D2x->step);
const short* d2ydata = (const short*)(D2y->data.ptr + i*D2y->step);
const short* dxydata = (const short*)(Dxy->data.ptr + i*Dxy->step);
for( j = 0; j < stripe_size.width; j++ )
{
double dx = dxdata[j];
double dx2 = dx * dx;
double dy = dydata[j];
double dy2 = dy * dy;
dstdata[j] = (float)(factor*(dx2*d2ydata[j] + dy2*d2xdata[j] - 2*dx*dy*dxydata[j]));
}
}
else
{
const float* dxdata = (const float*)(Dx->data.ptr + i*Dx->step);
const float* dydata = (const float*)(Dy->data.ptr + i*Dy->step);
const float* d2xdata = (const float*)(D2x->data.ptr + i*D2x->step);
const float* d2ydata = (const float*)(D2y->data.ptr + i*D2y->step);
const float* dxydata = (const float*)(Dxy->data.ptr + i*Dxy->step);
for( j = 0; j < stripe_size.width; j++ )
{
double dx = dxdata[j];
double dy = dydata[j];
dstdata[j] = (float)(factor*(dx*dx*d2ydata[j] + dy*dy*d2xdata[j] - 2*dx*dy*dxydata[j]));
}
}
}
stage = CV_MIDDLE;
}
__END__;
cvReleaseMat( &Dx );
cvReleaseMat( &Dy );
cvReleaseMat( &D2x );
cvReleaseMat( &D2y );
cvReleaseMat( &Dxy );
cvReleaseMat( &tempsrc );
}
CV_IMPL void cvDrlse_edge(CvArr * srcphi,
CvArr * srcgrad,
CvArr * dstarr,
double lambda,
double mu,
double alfa,
double epsilon,
int timestep,
int iter)
{
CV_FUNCNAME( "cvDrlse_edge" );
__BEGIN__;
CvMat sstub1, sstub2, *phi, *grad;
CvMat dstub, *dst;
CvMat *gradx=0, *grady=0, *phi_0=0, *phix=0, *phiy=0;
CvMat *s=0, *Nx=0, *Ny=0, *curvature=0, *distRegTerm=0;
CvMat *diracPhi=0, *areaTerm=0, *edgeTerm=0;
CvMat *temp1=0, *temp2=0, *temp3=0, *ones=0;
CvSize size;
int i;
CV_CALL( phi = cvGetMat(srcphi, &sstub1 ));
CV_CALL( grad = cvGetMat(srcgrad, &sstub2 ));
CV_CALL( dst = cvGetMat(dstarr, &dstub));
if( CV_MAT_TYPE(phi->type) != CV_32FC1)
CV_ERROR( CV_StsUnsupportedFormat, "Only-32bit, 1-channel input images are supported" );
if( CV_MAT_TYPE(grad->type) != CV_32FC1)
CV_ERROR( CV_StsUnsupportedFormat, "Only-32bit, 1-channel input images are supported" );
if( !CV_ARE_SIZES_EQ( phi, grad ))
CV_ERROR( CV_StsUnmatchedSizes, "The input images must have the same size" );
size = cvGetMatSize( phi );
//Initialization
gradx = cvCreateMat(size.height, size.width, CV_32FC1 );
grady = cvCreateMat(size.height, size.width, CV_32FC1 );
phi_0 = cvCreateMat(size.height, size.width, CV_32FC1 );
phix = cvCreateMat(size.height, size.width, CV_32FC1 );
phiy = cvCreateMat(size.height, size.width, CV_32FC1 );
Nx = cvCreateMat(size.height, size.width, CV_32FC1 );
Ny = cvCreateMat(size.height, size.width, CV_32FC1 );
s = cvCreateMat(size.height, size.width, CV_32FC1 );
curvature= cvCreateMat(size.height, size.width, CV_32FC1 );
distRegTerm= cvCreateMat(size.height, size.width, CV_32FC1 );
diracPhi = cvCreateMat(size.height, size.width, CV_32FC1 );
areaTerm = cvCreateMat(size.height, size.width, CV_32FC1 );
edgeTerm = cvCreateMat(size.height, size.width, CV_32FC1 );
temp1 = cvCreateMat(size.height, size.width, CV_32FC1 );
temp2 = cvCreateMat(size.height, size.width, CV_32FC1 );
temp3 = cvCreateMat(size.height, size.width, CV_32FC1 );
ones = cvCreateMat(size.height, size.width, CV_32FC1 );
cvSetZero(gradx);
cvSetZero(grady);
cvSetZero(phix);
cvSetZero(phiy);
cvSetZero(Nx);
cvSetZero(Ny);
cvSetZero(s);
cvSetZero(curvature);
cvSetZero(distRegTerm);
cvSetZero(diracPhi);
cvSetZero(areaTerm);
cvSetZero(edgeTerm);
cvSetZero(temp1);
cvSetZero(temp2);
cvSetZero(temp3);
cvSet(ones, cvScalar(1.0f));
//--------------BEGIN----------------------
cvSobel(grad, gradx, 1, 0, 1);
cvSobel(grad, grady, 0, 1, 1);
cvMul(gradx, ones, gradx, 0.25f);
cvMul(grady, ones, grady, 0.25f);
cvCopy(phi, dst);
for(i=0; i<iter; i++){
cvNeumannBoundCond(dst, dst);
cvSobel(dst, phix, 1, 0, 1);
cvSobel(dst, phiy, 0, 1, 1);
cvCalS(dst,s);
cvDiv(phix, s, Nx, 0.25f);
cvDiv(phiy, s, Ny, 0.25f);
cvCurvature(Nx, Ny, curvature);
cvDistReg(dst, distRegTerm);
cvDirac(dst, diracPhi, epsilon); //Compute driacPhi;
cvMul(diracPhi, grad, areaTerm); //Compute areaTerm
cvMul(gradx, Nx, gradx); //------------------//
cvMul(grady, Ny, grady); // Computing //
cvAdd(gradx, grady, temp1); // //
cvMul(diracPhi, temp1, temp2); // edgeTerm //
cvMul(areaTerm, curvature, temp3); // //
cvAdd(temp2, temp3, edgeTerm); //------------------//
cvMul(distRegTerm, ones, distRegTerm, mu); // distRegTerm = mu * distRegTerm
cvMul(edgeTerm, ones, edgeTerm, lambda); // edgeTerm = lambda * edgeTerm
cvMul(areaTerm, ones, areaTerm, alfa); // areaTerm = alfa * areaTerm
cvAdd(distRegTerm, edgeTerm, temp1);
cvAdd(temp1, areaTerm, temp2); // (distRegTerm + edgeTerm + areaTerm)
cvMul(temp2, ones, temp2, double(timestep)); // timestep * (distRegTerm + edgeTerm + areaTerm)
cvAdd(dst, temp2, dst); // phi = phi + timestep * (distRegTerm + edgeTerm + areaTerm)
}
//----------------END------------------------
// Clean up
cvReleaseMat(&ones);
cvReleaseMat(&phi_0);
cvReleaseMat(&gradx);
cvReleaseMat(&grady);
cvReleaseMat(&phix);
cvReleaseMat(&phiy);
cvReleaseMat(&Nx);
cvReleaseMat(&Ny);
cvReleaseMat(&s);
cvReleaseMat(&curvature);
cvReleaseMat(&distRegTerm);
cvReleaseMat(&diracPhi);
cvReleaseMat(&areaTerm);
cvReleaseMat(&edgeTerm);
cvReleaseMat(&temp1);
cvReleaseMat(&temp2);
cvReleaseMat(&temp3);
__END__;
}
CV_IMPL void
cvWatershed( const CvArr* srcarr, CvArr* dstarr )
{
const int IN_QUEUE = -2;
const int WSHED = -1;
const int NQ = 256;
CvMemStorage* storage = 0;
CV_FUNCNAME( "cvWatershed" );
__BEGIN__;
CvMat sstub, *src;
CvMat dstub, *dst;
CvSize size;
CvWSNode* free_node = 0, *node;
CvWSQueue q[NQ];
int active_queue;
int i, j;
int db, dg, dr;
int* mask;
uchar* img;
int mstep, istep;
int subs_tab[513];
// MAX(a,b) = b + MAX(a-b,0)
#define ws_max(a,b) ((b) + subs_tab[(a)-(b)+NQ])
// MIN(a,b) = a - MAX(a-b,0)
#define ws_min(a,b) ((a) - subs_tab[(a)-(b)+NQ])
#define ws_push(idx,mofs,iofs) \
{ \
if( !free_node ) \
CV_CALL( free_node = icvAllocWSNodes( storage ));\
node = free_node; \
free_node = free_node->next;\
node->next = 0; \
node->mask_ofs = mofs; \
node->img_ofs = iofs; \
if( q[idx].last ) \
q[idx].last->next=node; \
else \
q[idx].first = node; \
q[idx].last = node; \
}
#define ws_pop(idx,mofs,iofs) \
{ \
node = q[idx].first; \
q[idx].first = node->next; \
if( !node->next ) \
q[idx].last = 0; \
node->next = free_node; \
free_node = node; \
mofs = node->mask_ofs; \
iofs = node->img_ofs; \
}
#define c_diff(ptr1,ptr2,diff) \
{ \
db = abs((ptr1)[0] - (ptr2)[0]);\
dg = abs((ptr1)[1] - (ptr2)[1]);\
dr = abs((ptr1)[2] - (ptr2)[2]);\
diff = ws_max(db,dg); \
diff = ws_max(diff,dr); \
assert( 0 <= diff && diff <= 255 ); \
}
CV_CALL( src = cvGetMat( srcarr, &sstub ));
CV_CALL( dst = cvGetMat( dstarr, &dstub ));
if( CV_MAT_TYPE(src->type) != CV_8UC3 )
CV_ERROR( CV_StsUnsupportedFormat, "Only 8-bit, 3-channel input images are supported" );
if( CV_MAT_TYPE(dst->type) != CV_32SC1 )
CV_ERROR( CV_StsUnsupportedFormat,
"Only 32-bit, 1-channel output images are supported" );
if( !CV_ARE_SIZES_EQ( src, dst ))
CV_ERROR( CV_StsUnmatchedSizes, "The input and output images must have the same size" );
size = cvGetMatSize(src);
CV_CALL( storage = cvCreateMemStorage() );
istep = src->step;
img = src->data.ptr;
mstep = dst->step / sizeof(mask[0]);
mask = dst->data.i;
memset( q, 0, NQ*sizeof(q[0]) );
for( i = 0; i < 256; i++ )
subs_tab[i] = 0;
for( i = 256; i <= 512; i++ )
subs_tab[i] = i - 256;
// draw a pixel-wide border of dummy "watershed" (i.e. boundary) pixels
for( j = 0; j < size.width; j++ )
mask[j] = mask[j + mstep*(size.height-1)] = WSHED;
// initial phase: put all the neighbor pixels of each marker to the ordered queue -
// determine the initial boundaries of the basins
for( i = 1; i < size.height-1; i++ )
{
img += istep; mask += mstep;
mask[0] = mask[size.width-1] = WSHED;
for( j = 1; j < size.width-1; j++ )
{
int* m = mask + j;
if( m[0] < 0 ) m[0] = 0;
if( m[0] == 0 && (m[-1] > 0 || m[1] > 0 || m[-mstep] > 0 || m[mstep] > 0) )
{
uchar* ptr = img + j*3;
int idx = 256, t;
if( m[-1] > 0 )
c_diff( ptr, ptr - 3, idx );
if( m[1] > 0 )
{
c_diff( ptr, ptr + 3, t );
idx = ws_min( idx, t );
}
if( m[-mstep] > 0 )
{
c_diff( ptr, ptr - istep, t );
idx = ws_min( idx, t );
}
if( m[mstep] > 0 )
{
c_diff( ptr, ptr + istep, t );
idx = ws_min( idx, t );
}
assert( 0 <= idx && idx <= 255 );
ws_push( idx, i*mstep + j, i*istep + j*3 );
m[0] = IN_QUEUE;
}
}
}
// find the first non-empty queue
for( i = 0; i < NQ; i++ )
if( q[i].first )
break;
// if there is no markers, exit immediately
if( i == NQ )
EXIT;
active_queue = i;
img = src->data.ptr;
mask = dst->data.i;
// recursively fill the basins
for(;;)
{
int mofs, iofs;
int lab = 0, t;
int* m;
uchar* ptr;
if( q[active_queue].first == 0 )
{
for( i = active_queue+1; i < NQ; i++ )
if( q[i].first )
break;
if( i == NQ )
break;
active_queue = i;
}
ws_pop( active_queue, mofs, iofs );
m = mask + mofs;
ptr = img + iofs;
t = m[-1];
if( t > 0 ) lab = t;
t = m[1];
if( t > 0 )
{
if( lab == 0 ) lab = t;
else if( t != lab ) lab = WSHED;
}
t = m[-mstep];
if( t > 0 )
{
if( lab == 0 ) lab = t;
else if( t != lab ) lab = WSHED;
}
t = m[mstep];
if( t > 0 )
{
if( lab == 0 ) lab = t;
else if( t != lab ) lab = WSHED;
}
assert( lab != 0 );
m[0] = lab;
if( lab == WSHED )
continue;
if( m[-1] == 0 )
{
c_diff( ptr, ptr - 3, t );
ws_push( t, mofs - 1, iofs - 3 );
active_queue = ws_min( active_queue, t );
m[-1] = IN_QUEUE;
}
if( m[1] == 0 )
{
c_diff( ptr, ptr + 3, t );
ws_push( t, mofs + 1, iofs + 3 );
active_queue = ws_min( active_queue, t );
m[1] = IN_QUEUE;
}
if( m[-mstep] == 0 )
{
c_diff( ptr, ptr - istep, t );
ws_push( t, mofs - mstep, iofs - istep );
active_queue = ws_min( active_queue, t );
m[-mstep] = IN_QUEUE;
}
if( m[mstep] == 0 )
{
c_diff( ptr, ptr + 3, t );
ws_push( t, mofs + mstep, iofs + istep );
active_queue = ws_min( active_queue, t );
m[mstep] = IN_QUEUE;
}
}
__END__;
cvReleaseMemStorage( &storage );
}
void OptFlowEMD::calculate_flow(IplImage* imageT, IplImage* imageTMinus1, IplImage* velx, IplImage* vely, IplImage* abs){
#ifdef __CV_BEGIN__
__CV_BEGIN__
#else
__BEGIN__
#endif
CV_FUNCNAME( "OptFlowGenGrad::calculate_flow" );
CvMat stubA, *srcA = (CvMat*)imageT; // stubA takes the new header data for the matrix according to ROI
CvMat stubB, *srcB = (CvMat*)imageTMinus1;
CvMat stubx, *vel_x = (CvMat*)velx;
CvMat stuby, *vel_y = (CvMat*)vely;
CvMat stubAbs, *abs_ = NULL;
if (abs != NULL)
abs_ = (CvMat*)abs;
// see GetMat function doc: This returns a matrix header with the current image ROI!
// this gives basically a view on the ROI, stubA takes the header data of the matrix
// srcA is pointed to this new 'augmented' data-header
CV_CALL( srcA = cvGetMat( srcA, &stubA ));
CV_CALL( srcB = cvGetMat( srcB, &stubB ));
CV_CALL( vel_x = cvGetMat( vel_x, &stubx ));
CV_CALL( vel_y = cvGetMat( vel_y, &stuby ));
if (abs_ != NULL)
CV_CALL( abs_ = cvGetMat ( abs_, &stubAbs ));
if( !CV_ARE_TYPES_EQ( srcA, srcB ))
CV_ERROR( CV_StsUnmatchedFormats, "Source images have different formats" );
if( !CV_ARE_TYPES_EQ( vel_x, vel_y ))
CV_ERROR( CV_StsUnmatchedFormats, "Destination images have different formats" );
if (abs_ != NULL)
if (!CV_ARE_TYPES_EQ( vel_x, abs_))
CV_ERROR( CV_StsUnmatchedFormats, "Destination images have different formats" );
if( !CV_ARE_SIZES_EQ( srcA, srcB ) ||
!CV_ARE_SIZES_EQ( vel_x, vel_y ) ||
!CV_ARE_SIZES_EQ( srcA, vel_x ))
CV_ERROR( CV_StsUnmatchedSizes, "Some images have different sizes" );
if(abs_ != NULL)
if (!CV_ARE_SIZES_EQ( srcA, abs_))
CV_ERROR( CV_StsUnmatchedSizes, "Some images have different sizes" );
if( CV_MAT_TYPE( srcA->type ) != CV_8UC1)
CV_ERROR( CV_StsUnsupportedFormat, "Source images must have 8uC1 type");
if( CV_MAT_TYPE( vel_x->type ) != CV_32FC1 )
CV_ERROR( CV_StsUnsupportedFormat, "Destination images must have 32fC1 type" );
if( srcA->step != srcB->step ||
vel_x->step != vel_y->step)
CV_ERROR( CV_BadStep, "source and destination images have different step" );
if (abs_ != NULL)
if (vel_x->step != abs_->step)
CV_ERROR( CV_BadStep, "source and destination images have different step" );
if (abs_ != NULL){
IPPI_CALL( calcOptFlowEMD( (uchar*)srcA->data.ptr, (uchar*)srcB->data.ptr, srcA->step, cvGetMatSize( srcA ),
vel_x->data.fl, vel_y->data.fl, vel_x->step,
abs_->data.fl));
}
else{
IPPI_CALL( calcOptFlowEMD( (uchar*)srcA->data.ptr, (uchar*)srcB->data.ptr, srcA->step, cvGetMatSize( srcA ),
vel_x->data.fl, vel_y->data.fl, vel_x->step,
NULL));
}
#ifdef __CV_END__
__CV_END__
#else
__END__
#endif
}
CV_IMPL void
cvAbsDiffS( const void* srcarr, void* dstarr, CvScalar scalar )
{
static CvFuncTable adiffs_tab;
static int inittab = 0;
CV_FUNCNAME( "cvAbsDiffS" );
__BEGIN__;
int coi1 = 0, coi2 = 0;
int type, sctype;
CvMat srcstub, *src = (CvMat*)srcarr;
CvMat dststub, *dst = (CvMat*)dstarr;
int src_step = src->step;
int dst_step = dst->step;
double buf[12];
CvSize size;
if( !inittab )
{
icvInitAbsDiffCTable( &adiffs_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, "" );
if( !CV_ARE_TYPES_EQ(src, dst) )
CV_ERROR_FROM_CODE( CV_StsUnmatchedFormats );
if( !CV_ARE_SIZES_EQ(src, dst) )
CV_ERROR_FROM_CODE( CV_StsUnmatchedSizes );
sctype = type = CV_MAT_TYPE( src->type );
if( CV_MAT_DEPTH(type) < CV_32S )
sctype = (type & CV_MAT_CN_MASK) | CV_32SC1;
size = icvGetMatSize( src );
size.width *= CV_MAT_CN( type );
src_step = src->step;
dst_step = dst->step;
if( CV_IS_MAT_CONT( src->type & dst->type ))
{
size.width *= size.height;
size.height = 1;
src_step = dst_step = CV_STUB_STEP;
}
CV_CALL( cvScalarToRawData( &scalar, buf, sctype, 1 ));
{
CvFunc2D_2A1P func = (CvFunc2D_2A1P)
(adiffs_tab.fn_2d[CV_MAT_DEPTH(type)]);
if( !func )
CV_ERROR( CV_StsUnsupportedFormat, "" );
IPPI_CALL( func( src->data.ptr, src_step, dst->data.ptr,
dst_step, size, buf ));
}
__END__;
}
int ImageReranker::cvFindHomography( const CvMat* objectPoints, const CvMat* imagePoints,
CvMat* __H, int method, double ransacReprojThreshold,
CvMat* mask )
{
const double confidence = 0.995;
const int maxIters = 400;
const double defaultRANSACReprojThreshold = 3;
bool result = false;
Ptr<CvMat> m, M, tempMask;
double H[9];
CvMat matH = cvMat( 3, 3, CV_64FC1, H );
int count;
CV_Assert( CV_IS_MAT(imagePoints) && CV_IS_MAT(objectPoints) );
count = MAX(imagePoints->cols, imagePoints->rows);
CV_Assert( count >= 4 );
if( ransacReprojThreshold <= 0 )
ransacReprojThreshold = defaultRANSACReprojThreshold;
m = cvCreateMat( 1, count, CV_64FC2 );
cvConvertPointsHomogeneous( imagePoints, m );
M = cvCreateMat( 1, count, CV_64FC2 );
cvConvertPointsHomogeneous( objectPoints, M );
if( mask )
{
CV_Assert( CV_IS_MASK_ARR(mask) && CV_IS_MAT_CONT(mask->type) &&
(mask->rows == 1 || mask->cols == 1) &&
mask->rows*mask->cols == count );
}
if( mask || count > 4 )
tempMask = cvCreateMat( 1, count, CV_8U );
if( !tempMask.empty() )
cvSet( tempMask, cvScalarAll(1.) );
CvHomographyEstimator estimator(4);
if( count == 4 )
method = 0;
assert(method == CV_RANSAC);
result = estimator.runRANSAC( M, m, &matH, tempMask, ransacReprojThreshold, confidence, maxIters);
if( result && count > 4 )
{
icvCompressPoints( (CvPoint2D64f*)M->data.ptr, tempMask->data.ptr, 1, count );
count = icvCompressPoints( (CvPoint2D64f*)m->data.ptr, tempMask->data.ptr, 1, count );
M->cols = m->cols = count;
if( method == CV_RANSAC )
estimator.runKernel( M, m, &matH );
estimator.refine( M, m, &matH, 10 );
}
if( result )
cvConvert( &matH, __H );
if( mask && tempMask )
{
if( CV_ARE_SIZES_EQ(mask, tempMask) )
cvCopy( tempMask, mask );
else
cvTranspose( tempMask, mask );
}
return (int)result;
}
CV_IMPL double
cvCalcGlobalOrientation( const void* orientation, const void* maskimg, const void* mhiimg,
double curr_mhi_timestamp, double mhi_duration )
{
int hist_size = 12;
cv::Ptr<CvHistogram> hist;
CvMat mhistub, *mhi = cvGetMat(mhiimg, &mhistub);
CvMat maskstub, *mask = cvGetMat(maskimg, &maskstub);
CvMat orientstub, *orient = cvGetMat(orientation, &orientstub);
void* _orient;
float _ranges[] = { 0, 360 };
float* ranges = _ranges;
int base_orient;
float shift_orient = 0, shift_weight = 0;
float a, b, fbase_orient;
float delbound;
CvMat mhi_row, mask_row, orient_row;
int x, y, mhi_rows, mhi_cols;
if( !CV_IS_MASK_ARR( mask ))
CV_Error( CV_StsBadMask, "" );
if( CV_MAT_TYPE( mhi->type ) != CV_32FC1 || CV_MAT_TYPE( orient->type ) != CV_32FC1 )
CV_Error( CV_StsUnsupportedFormat,
"MHI and orientation must be single-channel floating-point images" );
if( !CV_ARE_SIZES_EQ( mhi, mask ) || !CV_ARE_SIZES_EQ( orient, mhi ))
CV_Error( CV_StsUnmatchedSizes, "" );
if( mhi_duration <= 0 )
CV_Error( CV_StsOutOfRange, "MHI duration must be positive" );
if( orient->data.ptr == mhi->data.ptr )
CV_Error( CV_StsInplaceNotSupported, "orientation image must be different from MHI" );
// calculate histogram of different orientation values
hist = cvCreateHist( 1, &hist_size, CV_HIST_ARRAY, &ranges );
_orient = orient;
cvCalcArrHist( &_orient, hist, 0, mask );
// find the maximum index (the dominant orientation)
cvGetMinMaxHistValue( hist, 0, 0, 0, &base_orient );
fbase_orient = base_orient*360.f/hist_size;
// override timestamp with the maximum value in MHI
cvMinMaxLoc( mhi, 0, &curr_mhi_timestamp, 0, 0, mask );
// find the shift relative to the dominant orientation as weighted sum of relative angles
a = (float)(254. / 255. / mhi_duration);
b = (float)(1. - curr_mhi_timestamp * a);
delbound = (float)(curr_mhi_timestamp - mhi_duration);
mhi_rows = mhi->rows;
mhi_cols = mhi->cols;
if( CV_IS_MAT_CONT( mhi->type & mask->type & orient->type ))
{
mhi_cols *= mhi_rows;
mhi_rows = 1;
}
cvGetRow( mhi, &mhi_row, 0 );
cvGetRow( mask, &mask_row, 0 );
cvGetRow( orient, &orient_row, 0 );
/*
a = 254/(255*dt)
b = 1 - t*a = 1 - 254*t/(255*dur) =
(255*dt - 254*t)/(255*dt) =
(dt - (t - dt)*254)/(255*dt);
--------------------------------------------------------
ax + b = 254*x/(255*dt) + (dt - (t - dt)*254)/(255*dt) =
(254*x + dt - (t - dt)*254)/(255*dt) =
((x - (t - dt))*254 + dt)/(255*dt) =
(((x - (t - dt))/dt)*254 + 1)/255 = (((x - low_time)/dt)*254 + 1)/255
*/
for( y = 0; y < mhi_rows; y++ )
{
mhi_row.data.ptr = mhi->data.ptr + mhi->step*y;
mask_row.data.ptr = mask->data.ptr + mask->step*y;
orient_row.data.ptr = orient->data.ptr + orient->step*y;
for( x = 0; x < mhi_cols; x++ )
if( mask_row.data.ptr[x] != 0 && mhi_row.data.fl[x] > delbound )
{
/*
orient in 0..360, base_orient in 0..360
-> (rel_angle = orient - base_orient) in -360..360.
rel_angle is translated to -180..180
*/
float weight = mhi_row.data.fl[x] * a + b;
float rel_angle = orient_row.data.fl[x] - fbase_orient;
rel_angle += (rel_angle < -180 ? 360 : 0);
rel_angle += (rel_angle > 180 ? -360 : 0);
if( fabs(rel_angle) < 45 )
{
shift_orient += weight * rel_angle;
shift_weight += weight;
}
}
}
// add the dominant orientation and the relative shift
if( shift_weight == 0 )
shift_weight = 0.01f;
fbase_orient += shift_orient / shift_weight;
fbase_orient -= (fbase_orient < 360 ? 0 : 360);
fbase_orient += (fbase_orient >= 0 ? 0 : 360);
return fbase_orient;
}
CV_IMPL CvScalar
cvAvg( const void* img, const void* maskarr )
{
CvScalar mean = {{0,0,0,0}};
static CvBigFuncTable mean_tab;
static CvFuncTable meancoi_tab;
static int inittab = 0;
CV_FUNCNAME("cvAvg");
__BEGIN__;
CvSize size;
double scale;
if( !maskarr )
{
CV_CALL( mean = cvSum(img));
size = cvGetSize( img );
size.width *= size.height;
scale = size.width ? 1./size.width : 0;
mean.val[0] *= scale;
mean.val[1] *= scale;
mean.val[2] *= scale;
mean.val[3] *= scale;
}
else
{
int type, coi = 0;
int mat_step, mask_step;
CvMat stub, maskstub, *mat = (CvMat*)img, *mask = (CvMat*)maskarr;
if( !inittab )
{
icvInitMeanMRTable( &mean_tab );
icvInitMeanCnCMRTable( &meancoi_tab );
inittab = 1;
}
if( !CV_IS_MAT(mat) )
CV_CALL( mat = cvGetMat( mat, &stub, &coi ));
if( !CV_IS_MAT(mask) )
CV_CALL( mask = cvGetMat( mask, &maskstub ));
if( !CV_IS_MASK_ARR(mask) )
CV_ERROR( CV_StsBadMask, "" );
if( !CV_ARE_SIZES_EQ( mat, mask ) )
CV_ERROR( CV_StsUnmatchedSizes, "" );
type = CV_MAT_TYPE( mat->type );
size = cvGetMatSize( mat );
mat_step = mat->step;
mask_step = mask->step;
if( CV_IS_MAT_CONT( mat->type & mask->type ))
{
size.width *= size.height;
size.height = 1;
mat_step = mask_step = CV_STUB_STEP;
}
if( CV_MAT_CN(type) == 1 || coi == 0 )
{
CvFunc2D_2A1P func;
if( CV_MAT_CN(type) > 4 )
CV_ERROR( CV_StsOutOfRange, "The input array must have at most 4 channels unless COI is set" );
func = (CvFunc2D_2A1P)(mean_tab.fn_2d[type]);
if( !func )
CV_ERROR( CV_StsBadArg, cvUnsupportedFormat );
IPPI_CALL( func( mat->data.ptr, mat_step, mask->data.ptr,
mask_step, size, mean.val ));
}
else
{
CvFunc2DnC_2A1P func = (CvFunc2DnC_2A1P)(
meancoi_tab.fn_2d[CV_MAT_DEPTH(type)]);
if( !func )
CV_ERROR( CV_StsBadArg, cvUnsupportedFormat );
IPPI_CALL( func( mat->data.ptr, mat_step, mask->data.ptr,
mask_step, size, CV_MAT_CN(type), coi, mean.val ));
}
}
__END__;
return mean;
}
/* motion templates */
CV_IMPL void
cvUpdateMotionHistory( const void* silhouette, void* mhimg,
double timestamp, double mhi_duration )
{
CvMat silhstub, *silh = cvGetMat(silhouette, &silhstub);
CvMat mhistub, *mhi = cvGetMat(mhimg, &mhistub);
if( !CV_IS_MASK_ARR( silh ))
CV_Error( CV_StsBadMask, "" );
if( CV_MAT_TYPE( mhi->type ) != CV_32FC1 )
CV_Error( CV_StsUnsupportedFormat, "" );
if( !CV_ARE_SIZES_EQ( mhi, silh ))
CV_Error( CV_StsUnmatchedSizes, "" );
CvSize size = cvGetMatSize( mhi );
int mhi_step = mhi->step;
int silh_step = silh->step;
if( CV_IS_MAT_CONT( mhi->type & silh->type ))
{
size.width *= size.height;
mhi_step = silh_step = CV_STUB_STEP;
size.height = 1;
}
float ts = (float)timestamp;
float delbound = (float)(timestamp - mhi_duration);
int x, y;
#if CV_SSE2
volatile bool useSIMD = cv::checkHardwareSupport(CV_CPU_SSE2);
#endif
for( y = 0; y < size.height; y++ )
{
const uchar* silhData = silh->data.ptr + silh->step*y;
float* mhiData = (float*)(mhi->data.ptr + mhi->step*y);
x = 0;
#if CV_SSE2
if( useSIMD )
{
__m128 ts4 = _mm_set1_ps(ts), db4 = _mm_set1_ps(delbound);
for( ; x <= size.width - 8; x += 8 )
{
__m128i z = _mm_setzero_si128();
__m128i s = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(silhData + x)), z);
__m128 s0 = _mm_cvtepi32_ps(_mm_unpacklo_epi16(s, z)), s1 = _mm_cvtepi32_ps(_mm_unpackhi_epi16(s, z));
__m128 v0 = _mm_loadu_ps(mhiData + x), v1 = _mm_loadu_ps(mhiData + x + 4);
__m128 fz = _mm_setzero_ps();
v0 = _mm_and_ps(v0, _mm_cmpge_ps(v0, db4));
v1 = _mm_and_ps(v1, _mm_cmpge_ps(v1, db4));
__m128 m0 = _mm_and_ps(_mm_xor_ps(v0, ts4), _mm_cmpneq_ps(s0, fz));
__m128 m1 = _mm_and_ps(_mm_xor_ps(v1, ts4), _mm_cmpneq_ps(s1, fz));
v0 = _mm_xor_ps(v0, m0);
v1 = _mm_xor_ps(v1, m1);
_mm_storeu_ps(mhiData + x, v0);
_mm_storeu_ps(mhiData + x + 4, v1);
}
}
#endif
for( ; x < size.width; x++ )
{
float val = mhiData[x];
val = silhData[x] ? ts : val < delbound ? 0 : val;
mhiData[x] = val;
}
}
}
CV_IMPL void
cvMinMaxLoc( const void* img, double* _minVal, double* _maxVal,
CvPoint* _minLoc, CvPoint* _maxLoc, const void* mask )
{
static CvFuncTable minmax_tab, minmaxcoi_tab;
static CvFuncTable minmaxmask_tab, minmaxmaskcoi_tab;
static int inittab = 0;
CV_FUNCNAME("cvMinMaxLoc");
__BEGIN__;
int type, depth, cn, coi = 0;
int mat_step, mask_step = 0;
CvSize size;
CvMat stub, maskstub, *mat = (CvMat*)img, *matmask = (CvMat*)mask;
CvPoint minLoc, maxLoc;
double minVal = 0, maxVal = 0;
if( !inittab )
{
icvInitMinMaxIndxC1RTable( &minmax_tab );
icvInitMinMaxIndxCnCRTable( &minmaxcoi_tab );
icvInitMinMaxIndxC1MRTable( &minmaxmask_tab );
icvInitMinMaxIndxCnCMRTable( &minmaxmaskcoi_tab );
inittab = 1;
}
CV_CALL( mat = cvGetMat( mat, &stub, &coi ));
type = CV_MAT_TYPE( mat->type );
depth = CV_MAT_DEPTH( type );
cn = CV_MAT_CN( type );
size = cvGetMatSize( mat );
if( cn > 1 && coi == 0 )
CV_ERROR( CV_StsBadArg, "" );
mat_step = mat->step;
if( !mask )
{
if( size.height == 1 )
mat_step = CV_STUB_STEP;
if( CV_MAT_CN(type) == 1 || coi == 0 )
{
CvFunc2D_1A4P func = (CvFunc2D_1A4P)(minmax_tab.fn_2d[depth]);
if( !func )
CV_ERROR( CV_StsBadArg, cvUnsupportedFormat );
IPPI_CALL( func( mat->data.ptr, mat_step, size,
&minVal, &maxVal, &minLoc, &maxLoc ));
}
else
{
CvFunc2DnC_1A4P func = (CvFunc2DnC_1A4P)(minmaxcoi_tab.fn_2d[depth]);
if( !func )
CV_ERROR( CV_StsBadArg, cvUnsupportedFormat );
IPPI_CALL( func( mat->data.ptr, mat_step, size, cn, coi,
&minVal, &maxVal, &minLoc, &maxLoc ));
}
}
else
{
CV_CALL( matmask = cvGetMat( matmask, &maskstub ));
if( !CV_IS_MASK_ARR( matmask ))
CV_ERROR( CV_StsBadMask, "" );
if( !CV_ARE_SIZES_EQ( mat, matmask ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
mask_step = matmask->step;
if( size.height == 1 )
mat_step = mask_step = CV_STUB_STEP;
if( CV_MAT_CN(type) == 1 || coi == 0 )
{
CvFunc2D_2A4P func = (CvFunc2D_2A4P)(minmaxmask_tab.fn_2d[depth]);
if( !func )
CV_ERROR( CV_StsBadArg, cvUnsupportedFormat );
IPPI_CALL( func( mat->data.ptr, mat_step, matmask->data.ptr,
mask_step, size,
&minVal, &maxVal, &minLoc, &maxLoc ));
}
else
{
CvFunc2DnC_2A4P func = (CvFunc2DnC_2A4P)(minmaxmaskcoi_tab.fn_2d[depth]);
if( !func )
CV_ERROR( CV_StsBadArg, cvUnsupportedFormat );
IPPI_CALL( func( mat->data.ptr, mat_step,
matmask->data.ptr, mask_step, size, cn, coi,
&minVal, &maxVal, &minLoc, &maxLoc ));
}
}
if( depth < CV_32S || depth == CV_32F )
{
minVal = *(float*)&minVal;
maxVal = *(float*)&maxVal;
}
if( _minVal )
*_minVal = minVal;
if( _maxVal )
*_maxVal = maxVal;
if( _minLoc )
*_minLoc = minLoc;
if( _maxLoc )
*_maxLoc = maxLoc;
__END__;
}
