/* 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__;
}
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__;
}
/* Returns number of corresponding points */
int icvFindCorrForGivenPoints( IplImage *image1,/* Image 1 */
IplImage *image2,/* Image 2 */
CvMat *points1,
CvMat *pntStatus1,
CvMat *points2,
CvMat *pntStatus2,
int useFilter,/*Use fundamental matrix to filter points */
double threshold)/* Threshold for good points in filter */
{
int resNumCorrPoints = 0;
CvPoint2D32f* cornerPoints1 = 0;
CvPoint2D32f* cornerPoints2 = 0;
char* status = 0;
float* errors = 0;
CvMat* tmpPoints1 = 0;
CvMat* tmpPoints2 = 0;
CvMat* pStatus = 0;
IplImage *grayImage1 = 0;
IplImage *grayImage2 = 0;
IplImage *pyrImage1 = 0;
IplImage *pyrImage2 = 0;
CV_FUNCNAME( "icvFindCorrForGivenPoints" );
__BEGIN__;
/* Test input data for errors */
/* Test for null pointers */
if( image1 == 0 || image2 == 0 ||
points1 == 0 || points2 == 0 ||
pntStatus1 == 0 || pntStatus2 == 0)
{
CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
}
/* Test image size */
int w,h;
w = image1->width;
h = image1->height;
if( w <= 0 || h <= 0)
{
CV_ERROR( CV_StsOutOfRange, "Size of image1 must be > 0" );
}
if( image2->width != w || image2->height != h )
{
CV_ERROR( CV_StsUnmatchedSizes, "Size of images must be the same" );
}
/* Test for matrices */
if( !CV_IS_MAT(points1) || !CV_IS_MAT(points2) ||
!CV_IS_MAT(pntStatus1) || !CV_IS_MAT(pntStatus2) )
{
CV_ERROR( CV_StsUnsupportedFormat, "Input parameters (points and status) must be a matrices" );
}
/* Test type of status matrices */
if( !CV_IS_MASK_ARR(pntStatus1) || !CV_IS_MASK_ARR(pntStatus2) )
{
CV_ERROR( CV_StsUnsupportedFormat, "Statuses must be a mask arrays" );
}
/* Test number of points */
int numPoints;
numPoints = points1->cols;
if( numPoints <= 0 )
{
CV_ERROR( CV_StsOutOfRange, "Number of points1 must be > 0" );
}
if( points2->cols != numPoints || pntStatus1->cols != numPoints || pntStatus2->cols != numPoints )
{
CV_ERROR( CV_StsUnmatchedSizes, "Number of points and statuses must be the same" );
}
if( points1->rows != 2 || points2->rows != 2 )
{
CV_ERROR( CV_StsOutOfRange, "Number of points coordinates must be 2" );
}
if( pntStatus1->rows != 1 || pntStatus2->rows != 1 )
{
CV_ERROR( CV_StsOutOfRange, "Status must be a matrix 1xN" );
}
/* ----- End test ----- */
/* Compute number of visible points on image1 */
int numVisPoints;
numVisPoints = cvCountNonZero(pntStatus1);
if( numVisPoints > 0 )
{
/* Create temporary images */
/* We must use iplImage againts hughgui images */
/*
CvvImage grayImage1;
CvvImage grayImage2;
CvvImage pyrImage1;
CvvImage pyrImage2;
*/
/* Create Ipl images */
CV_CALL( grayImage1 = cvCreateImage(cvSize(w,h),8,1) );
CV_CALL( grayImage2 = cvCreateImage(cvSize(w,h),8,1) );
CV_CALL( pyrImage1 = cvCreateImage(cvSize(w,h),8,1) );
CV_CALL( pyrImage2 = cvCreateImage(cvSize(w,h),8,1) );
CV_CALL( cornerPoints1 = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f)*numVisPoints) );
CV_CALL( cornerPoints2 = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f)*numVisPoints) );
CV_CALL( status = (char*)cvAlloc( sizeof(char)*numVisPoints) );
CV_CALL( errors = (float*)cvAlloc( 2 * sizeof(float)*numVisPoints) );
int i;
for( i = 0; i < numVisPoints; i++ )
{
status[i] = 1;
}
/* !!! Need test creation errors */
/*
if( !grayImage1.Create(w,h,8)) EXIT;
if( !grayImage2.Create(w,h,8)) EXIT;
if( !pyrImage1. Create(w,h,8)) EXIT;
if( !pyrImage2. Create(w,h,8)) EXIT;
*/
cvCvtColor(image1,grayImage1,CV_BGR2GRAY);
cvCvtColor(image2,grayImage2,CV_BGR2GRAY);
/*
grayImage1.CopyOf(image1,0);
grayImage2.CopyOf(image2,0);
*/
/* Copy points good points from input data */
uchar *stat1 = pntStatus1->data.ptr;
uchar *stat2 = pntStatus2->data.ptr;
int curr = 0;
for( i = 0; i < numPoints; i++ )
{
if( stat1[i] )
{
cornerPoints1[curr].x = (float)cvmGet(points1,0,i);
cornerPoints1[curr].y = (float)cvmGet(points1,1,i);
curr++;
}
}
/* Define number of levels of pyramid */
cvCalcOpticalFlowPyrLK( grayImage1, grayImage2,
pyrImage1, pyrImage2,
cornerPoints1, cornerPoints2,
numVisPoints, cvSize(10,10), 3,
status, errors,
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03),
0/*CV_LKFLOW_PYR_A_READY*/ );
memset(stat2,0,sizeof(uchar)*numPoints);
int currVis = 0;
int totalCorns = 0;
/* Copy new points and set status */
/* stat1 may not be the same as stat2 */
for( i = 0; i < numPoints; i++ )
{
if( stat1[i] )
{
if( status[currVis] && errors[currVis] < 1000 )
{
stat2[i] = 1;
cvmSet(points2,0,i,cornerPoints2[currVis].x);
cvmSet(points2,1,i,cornerPoints2[currVis].y);
totalCorns++;
}
currVis++;
}
}
resNumCorrPoints = totalCorns;
/* Filter points using RANSAC */
if( useFilter )
{
resNumCorrPoints = 0;
/* Use RANSAC filter for found points */
if( totalCorns > 7 )
{
/* Create array with good points only */
CV_CALL( tmpPoints1 = cvCreateMat(2,totalCorns,CV_64F) );
CV_CALL( tmpPoints2 = cvCreateMat(2,totalCorns,CV_64F) );
/* Copy just good points */
int currPoint = 0;
for( i = 0; i < numPoints; i++ )
{
if( stat2[i] )
{
cvmSet(tmpPoints1,0,currPoint,cvmGet(points1,0,i));
cvmSet(tmpPoints1,1,currPoint,cvmGet(points1,1,i));
cvmSet(tmpPoints2,0,currPoint,cvmGet(points2,0,i));
cvmSet(tmpPoints2,1,currPoint,cvmGet(points2,1,i));
currPoint++;
}
}
/* Compute fundamental matrix */
CvMat fundMatr;
double fundMatr_dat[9];
fundMatr = cvMat(3,3,CV_64F,fundMatr_dat);
CV_CALL( pStatus = cvCreateMat(1,totalCorns,CV_32F) );
int num = cvFindFundamentalMat(tmpPoints1,tmpPoints2,&fundMatr,CV_FM_RANSAC,threshold,0.99,pStatus);
if( num > 0 )
{
int curr = 0;
/* Set final status for points2 */
for( i = 0; i < numPoints; i++ )
{
if( stat2[i] )
{
if( cvmGet(pStatus,0,curr) == 0 )
{
stat2[i] = 0;
}
curr++;
}
}
resNumCorrPoints = curr;
}
}
}
}
__END__;
/* Free allocated memory */
cvFree(&cornerPoints1);
cvFree(&cornerPoints2);
cvFree(&status);
cvFree(&errors);
cvFree(&tmpPoints1);
cvFree(&tmpPoints2);
cvReleaseMat( &pStatus );
cvReleaseImage( &grayImage1 );
cvReleaseImage( &grayImage2 );
cvReleaseImage( &pyrImage1 );
cvReleaseImage( &pyrImage2 );
return resNumCorrPoints;
}
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 void
cvFloodFill( CvArr* arr, CvPoint seed_point,
CvScalar newVal, CvScalar lo_diff, CvScalar up_diff,
CvConnectedComp* comp, int flags, CvArr* maskarr )
{
cv::Ptr<CvMat> tempMask;
std::vector<CvFFillSegment> buffer;
if( comp )
memset( comp, 0, sizeof(*comp) );
int i, type, depth, cn, is_simple;
int buffer_size, connectivity = flags & 255;
union {
uchar b[4];
int i[4];
float f[4];
double _[4];
} nv_buf;
nv_buf._[0] = nv_buf._[1] = nv_buf._[2] = nv_buf._[3] = 0;
struct { cv::Vec3b b; cv::Vec3i i; cv::Vec3f f; } ld_buf, ud_buf;
CvMat stub, *img = cvGetMat(arr, &stub);
CvMat maskstub, *mask = (CvMat*)maskarr;
CvSize size;
type = CV_MAT_TYPE( img->type );
depth = CV_MAT_DEPTH(type);
cn = CV_MAT_CN(type);
if( connectivity == 0 )
connectivity = 4;
else if( connectivity != 4 && connectivity != 8 )
CV_Error( CV_StsBadFlag, "Connectivity must be 4, 0(=4) or 8" );
is_simple = mask == 0 && (flags & CV_FLOODFILL_MASK_ONLY) == 0;
for( i = 0; i < cn; i++ )
{
if( lo_diff.val[i] < 0 || up_diff.val[i] < 0 )
CV_Error( CV_StsBadArg, "lo_diff and up_diff must be non-negative" );
is_simple &= fabs(lo_diff.val[i]) < DBL_EPSILON && fabs(up_diff.val[i]) < DBL_EPSILON;
}
size = cvGetMatSize( img );
if( (unsigned)seed_point.x >= (unsigned)size.width ||
(unsigned)seed_point.y >= (unsigned)size.height )
CV_Error( CV_StsOutOfRange, "Seed point is outside of image" );
cvScalarToRawData( &newVal, &nv_buf, type, 0 );
buffer_size = MAX( size.width, size.height ) * 2;
buffer.resize( buffer_size );
if( is_simple )
{
int elem_size = CV_ELEM_SIZE(type);
const uchar* seed_ptr = img->data.ptr + img->step*seed_point.y + elem_size*seed_point.x;
for(i = 0; i < elem_size; i++)
if (seed_ptr[i] != nv_buf.b[i])
break;
if (i != elem_size)
{
if( type == CV_8UC1 )
icvFloodFill_CnIR(img->data.ptr, img->step, size, seed_point, nv_buf.b[0],
comp, flags, &buffer);
else if( type == CV_8UC3 )
icvFloodFill_CnIR(img->data.ptr, img->step, size, seed_point, cv::Vec3b(nv_buf.b),
comp, flags, &buffer);
else if( type == CV_32SC1 )
icvFloodFill_CnIR(img->data.ptr, img->step, size, seed_point, nv_buf.i[0],
comp, flags, &buffer);
else if( type == CV_32FC1 )
icvFloodFill_CnIR(img->data.ptr, img->step, size, seed_point, nv_buf.f[0],
comp, flags, &buffer);
else if( type == CV_32SC3 )
icvFloodFill_CnIR(img->data.ptr, img->step, size, seed_point, cv::Vec3i(nv_buf.i),
comp, flags, &buffer);
else if( type == CV_32FC3 )
icvFloodFill_CnIR(img->data.ptr, img->step, size, seed_point, cv::Vec3f(nv_buf.f),
comp, flags, &buffer);
else
CV_Error( CV_StsUnsupportedFormat, "" );
return;
}
}
if( !mask )
{
/* created mask will be 8-byte aligned */
tempMask = cvCreateMat( size.height + 2, (size.width + 9) & -8, CV_8UC1 );
mask = tempMask;
}
else
{
mask = cvGetMat( mask, &maskstub );
if( !CV_IS_MASK_ARR( mask ))
CV_Error( CV_StsBadMask, "" );
if( mask->width != size.width + 2 || mask->height != size.height + 2 )
CV_Error( CV_StsUnmatchedSizes, "mask must be 2 pixel wider "
"and 2 pixel taller than filled image" );
}
int width = tempMask ? mask->step : size.width + 2;
uchar* mask_row = mask->data.ptr + mask->step;
memset( mask_row - mask->step, 1, width );
for( i = 1; i <= size.height; i++, mask_row += mask->step )
{
if( tempMask )
memset( mask_row, 0, width );
mask_row[0] = mask_row[size.width+1] = (uchar)1;
}
memset( mask_row, 1, width );
if( depth == CV_8U )
for( i = 0; i < cn; i++ )
{
int t = cvFloor(lo_diff.val[i]);
ld_buf.b[i] = CV_CAST_8U(t);
t = cvFloor(up_diff.val[i]);
ud_buf.b[i] = CV_CAST_8U(t);
}
else if( depth == CV_32S )
for( i = 0; i < cn; i++ )
{
int t = cvFloor(lo_diff.val[i]);
ld_buf.i[i] = t;
t = cvFloor(up_diff.val[i]);
ud_buf.i[i] = t;
}
else if( depth == CV_32F )
for( i = 0; i < cn; i++ )
{
ld_buf.f[i] = (float)lo_diff.val[i];
ud_buf.f[i] = (float)up_diff.val[i];
}
else
CV_Error( CV_StsUnsupportedFormat, "" );
if( type == CV_8UC1 )
icvFloodFillGrad_CnIR<uchar, int, Diff8uC1>(
img->data.ptr, img->step, mask->data.ptr, mask->step,
size, seed_point, nv_buf.b[0],
Diff8uC1(ld_buf.b[0], ud_buf.b[0]),
comp, flags, &buffer);
else if( type == CV_8UC3 )
icvFloodFillGrad_CnIR<cv::Vec3b, cv::Vec3i, Diff8uC3>(
img->data.ptr, img->step, mask->data.ptr, mask->step,
size, seed_point, cv::Vec3b(nv_buf.b),
Diff8uC3(ld_buf.b, ud_buf.b),
comp, flags, &buffer);
else if( type == CV_32SC1 )
icvFloodFillGrad_CnIR<int, int, Diff32sC1>(
img->data.ptr, img->step, mask->data.ptr, mask->step,
size, seed_point, nv_buf.i[0],
Diff32sC1(ld_buf.i[0], ud_buf.i[0]),
comp, flags, &buffer);
else if( type == CV_32SC3 )
icvFloodFillGrad_CnIR<cv::Vec3i, cv::Vec3i, Diff32sC3>(
img->data.ptr, img->step, mask->data.ptr, mask->step,
size, seed_point, cv::Vec3i(nv_buf.i),
Diff32sC3(ld_buf.i, ud_buf.i),
comp, flags, &buffer);
else if( type == CV_32FC1 )
icvFloodFillGrad_CnIR<float, float, Diff32fC1>(
img->data.ptr, img->step, mask->data.ptr, mask->step,
size, seed_point, nv_buf.f[0],
Diff32fC1(ld_buf.f[0], ud_buf.f[0]),
comp, flags, &buffer);
else if( type == CV_32FC3 )
icvFloodFillGrad_CnIR<cv::Vec3f, cv::Vec3f, Diff32fC3>(
img->data.ptr, img->step, mask->data.ptr, mask->step,
size, seed_point, cv::Vec3f(nv_buf.f),
Diff32fC3(ld_buf.f, ud_buf.f),
comp, flags, &buffer);
else
CV_Error(CV_StsUnsupportedFormat, "");
}
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;
}
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
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
cvFloodFill( CvArr* arr, CvPoint seed_point,
CvScalar newVal, CvScalar lo_diff, CvScalar up_diff,
CvConnectedComp* comp, int flags, CvArr* maskarr )
{
static void* ffill_tab[4];
static void* ffillgrad_tab[4];
static int inittab = 0;
CvMat* tempMask = 0;
CvFFillSegment* buffer = 0;
CV_FUNCNAME( "cvFloodFill" );
if( comp )
memset( comp, 0, sizeof(*comp) );
__BEGIN__;
int i, type, depth, cn, is_simple, idx;
int buffer_size, connectivity = flags & 255;
double nv_buf[4] = {0,0,0,0};
union { uchar b[4]; float f[4]; } ld_buf, ud_buf;
CvMat stub, *img = (CvMat*)arr;
CvMat maskstub, *mask = (CvMat*)maskarr;
CvSize size;
if( !inittab )
{
icvInitFloodFill( ffill_tab, ffillgrad_tab );
inittab = 1;
}
CV_CALL( img = cvGetMat( img, &stub ));
type = CV_MAT_TYPE( img->type );
depth = CV_MAT_DEPTH(type);
cn = CV_MAT_CN(type);
idx = type == CV_8UC1 || type == CV_8UC3 ? 0 :
type == CV_32FC1 || type == CV_32FC3 ? 1 : -1;
if( idx < 0 )
CV_ERROR( CV_StsUnsupportedFormat, "" );
if( connectivity == 0 )
connectivity = 4;
else if( connectivity != 4 && connectivity != 8 )
CV_ERROR( CV_StsBadFlag, "Connectivity must be 4, 0(=4) or 8" );
is_simple = mask == 0 && (flags & CV_FLOODFILL_MASK_ONLY) == 0;
for( i = 0; i < cn; i++ )
{
if( lo_diff.val[i] < 0 || up_diff.val[i] < 0 )
CV_ERROR( CV_StsBadArg, "lo_diff and up_diff must be non-negative" );
is_simple &= fabs(lo_diff.val[i]) < DBL_EPSILON && fabs(up_diff.val[i]) < DBL_EPSILON;
}
size = cvGetMatSize( img );
if( (unsigned)seed_point.x >= (unsigned)size.width ||
(unsigned)seed_point.y >= (unsigned)size.height )
CV_ERROR( CV_StsOutOfRange, "Seed point is outside of image" );
cvScalarToRawData( &newVal, &nv_buf, type, 0 );
buffer_size = MAX( size.width, size.height )*2;
CV_CALL( buffer = (CvFFillSegment*)cvAlloc( buffer_size*sizeof(buffer[0])));
if( is_simple )
{
CvFloodFillFunc func = (CvFloodFillFunc)ffill_tab[idx];
if( !func )
CV_ERROR( CV_StsUnsupportedFormat, "" );
IPPI_CALL( func( img->data.ptr, img->step, size,
seed_point, &nv_buf, comp, flags,
buffer, buffer_size, cn ));
}
else
{
CvFloodFillGradFunc func = (CvFloodFillGradFunc)ffillgrad_tab[idx];
if( !func )
CV_ERROR( CV_StsUnsupportedFormat, "" );
if( !mask )
{
/* created mask will be 8-byte aligned */
tempMask = cvCreateMat( size.height + 2, (size.width + 9) & -8, CV_8UC1 );
mask = tempMask;
}
else
{
CV_CALL( mask = cvGetMat( mask, &maskstub ));
if( !CV_IS_MASK_ARR( mask ))
CV_ERROR( CV_StsBadMask, "" );
if( mask->width != size.width + 2 || mask->height != size.height + 2 )
CV_ERROR( CV_StsUnmatchedSizes, "mask must be 2 pixel wider "
"and 2 pixel taller than filled image" );
}
{
int width = tempMask ? mask->step : size.width + 2;
uchar* mask_row = mask->data.ptr + mask->step;
memset( mask_row - mask->step, 1, width );
for( i = 1; i <= size.height; i++, mask_row += mask->step )
{
if( tempMask )
memset( mask_row, 0, width );
mask_row[0] = mask_row[size.width+1] = (uchar)1;
}
memset( mask_row, 1, width );
}
if( depth == CV_8U )
for( i = 0; i < cn; i++ )
{
int t = cvFloor(lo_diff.val[i]);
ld_buf.b[i] = CV_CAST_8U(t);
t = cvFloor(up_diff.val[i]);
ud_buf.b[i] = CV_CAST_8U(t);
}
else
for( i = 0; i < cn; i++ )
{
ld_buf.f[i] = (float)lo_diff.val[i];
ud_buf.f[i] = (float)up_diff.val[i];
}
IPPI_CALL( func( img->data.ptr, img->step, mask->data.ptr, mask->step,
size, seed_point, &nv_buf, ld_buf.f, ud_buf.f,
comp, flags, buffer, buffer_size, cn ));
}
__END__;
cvFree( &buffer );
cvReleaseMat( &tempMask );
}
DMZ_INTERNAL CvLinePolar llcv_hough(const CvArr *src_image, IplImage *dx, IplImage *dy, float rho, float theta, int threshold, float theta_min, float theta_max, bool vertical, float gradient_angle_threshold) {
CvMat img_stub, *img = (CvMat*)src_image;
img = cvGetMat(img, &img_stub);
CvMat dx_stub, *dx_mat = (CvMat*)dx;
dx_mat = cvGetMat(dx_mat, &dx_stub);
CvMat dy_stub, *dy_mat = (CvMat*)dy;
dy_mat = cvGetMat(dy_mat, &dy_stub);
if(!CV_IS_MASK_ARR(img)) {
CV_Error(CV_StsBadArg, "The source image must be 8-bit, single-channel");
}
if(rho <= 0 || theta <= 0 || threshold <= 0) {
CV_Error(CV_StsOutOfRange, "rho, theta and threshold must be positive");
}
if(theta_max < theta_min + theta) {
CV_Error(CV_StsBadArg, "theta + theta_min (param1) must be <= theta_max (param2)");
}
cv::AutoBuffer<int> _accum;
cv::AutoBuffer<int> _tabSin, _tabCos;
const uchar* image;
int step, width, height;
int numangle, numrho;
float ang;
int r, n;
int i, j;
float irho = 1 / rho;
float scale;
CV_Assert( CV_IS_MAT(img) && CV_MAT_TYPE(img->type) == CV_8UC1 );
image = img->data.ptr;
step = img->step;
width = img->cols;
height = img->rows;
const uint8_t *dx_mat_ptr = (uint8_t *)(dx_mat->data.ptr);
int dx_step = dx_mat->step;
const uint8_t *dy_mat_ptr = (uint8_t *)(dy_mat->data.ptr);
int dy_step = dy_mat->step;
numangle = cvRound((theta_max - theta_min) / theta);
numrho = cvRound(((width + height) * 2 + 1) / rho);
_accum.allocate((numangle+2) * (numrho+2));
_tabSin.allocate(numangle);
_tabCos.allocate(numangle);
int *accum = _accum;
int *tabSin = _tabSin, *tabCos = _tabCos;
memset(accum, 0, sizeof(accum[0]) * (numangle + 2) * (numrho + 2));
#define FIXED_POINT_EXPONENT 10
#define FIXED_POINT_MULTIPLIER (1 << FIXED_POINT_EXPONENT)
for(ang = theta_min, n = 0; n < numangle; ang += theta, n++) {
tabSin[n] = (int)floorf(FIXED_POINT_MULTIPLIER * sinf(ang) * irho);
tabCos[n] = (int)floorf(FIXED_POINT_MULTIPLIER * cosf(ang) * irho);
}
float slope_bound_a, slope_bound_b;
if(vertical) {
slope_bound_a = tanf((float)TO_RADIANS(180 - gradient_angle_threshold));
slope_bound_b = tanf((float)TO_RADIANS(180 + gradient_angle_threshold));
} else {
slope_bound_a = tanf((float)TO_RADIANS(90 - gradient_angle_threshold));
slope_bound_b = tanf((float)TO_RADIANS(90 + gradient_angle_threshold));
}
// stage 1. fill accumulator
for(i = 0; i < height; i++) {
int16_t *dx_row_ptr = (int16_t *)(dx_mat_ptr + i * dx_step);
int16_t *dy_row_ptr = (int16_t *)(dy_mat_ptr + i * dy_step);
for(j = 0; j < width; j++) {
if(image[i * step + j] != 0) {
int16_t del_x = dx_row_ptr[j];
int16_t del_y = dy_row_ptr[j];
bool use_pixel = false;
if(dmz_likely(del_x != 0)) { // avoid div by 0
float slope = (float)del_y / (float)del_x;
if(vertical) {
if(slope >= slope_bound_a && slope <= slope_bound_b) {
use_pixel = true;
}
} else {
if(slope >= slope_bound_a || slope <= slope_bound_b) {
use_pixel = true;
}
}
} else {
use_pixel = !vertical;
}
if(use_pixel) {
for(n = 0; n < numangle; n++) {
r = (j * tabCos[n] + i * tabSin[n]) >> FIXED_POINT_EXPONENT;
r += (numrho - 1) / 2;
accum[(n+1) * (numrho+2) + r+1]++;
}
}
}
}
}
/* Create feature points on image and return number of them. Array points fills by found points */
int icvCreateFeaturePoints(IplImage *image, CvMat *points, CvMat *status)
{
int foundFeaturePoints = 0;
IplImage *grayImage = 0;
IplImage *eigImage = 0;
IplImage *tmpImage = 0;
CvPoint2D32f *cornerPoints = 0;
CV_FUNCNAME( "icvFeatureCreatePoints" );
__BEGIN__;
/* Test for errors */
if( image == 0 || points == 0 )
{
CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
}
/* Test image size */
int w,h;
w = image->width;
h = image->height;
if( w <= 0 || h <= 0)
{
CV_ERROR( CV_StsOutOfRange, "Size of image must be > 0" );
}
/* Test for matrices */
if( !CV_IS_MAT(points) )
{
CV_ERROR( CV_StsUnsupportedFormat, "Input parameter points must be a matrix" );
}
int needNumPoints;
needNumPoints = points->cols;
if( needNumPoints <= 0 )
{
CV_ERROR( CV_StsOutOfRange, "Number of need points must be > 0" );
}
if( points->rows != 2 )
{
CV_ERROR( CV_StsOutOfRange, "Number of point coordinates must be == 2" );
}
if( status != 0 )
{
/* If status matrix exist test it for correct */
if( !CV_IS_MASK_ARR(status) )
{
CV_ERROR( CV_StsUnsupportedFormat, "Statuses must be a mask arrays" );
}
if( status->cols != needNumPoints )
{
CV_ERROR( CV_StsUnmatchedSizes, "Size of points and statuses must be the same" );
}
if( status->rows !=1 )
{
CV_ERROR( CV_StsUnsupportedFormat, "Number of rows of status must be 1" );
}
}
/* Create temporary images */
CV_CALL( grayImage = cvCreateImage(cvSize(w,h), 8,1) );
CV_CALL( eigImage = cvCreateImage(cvSize(w,h),32,1) );
CV_CALL( tmpImage = cvCreateImage(cvSize(w,h),32,1) );
/* Create points */
CV_CALL( cornerPoints = (CvPoint2D32f*)cvAlloc( sizeof(CvPoint2D32f) * needNumPoints) );
int foundNum;
double quality;
double minDist;
cvCvtColor(image,grayImage, CV_BGR2GRAY);
foundNum = needNumPoints;
quality = 0.01;
minDist = 5;
cvGoodFeaturesToTrack(grayImage, eigImage, tmpImage, cornerPoints, &foundNum, quality, minDist);
/* Copy found points to result */
int i;
for( i = 0; i < foundNum; i++ )
{
cvmSet(points,0,i,cornerPoints[i].x);
cvmSet(points,1,i,cornerPoints[i].y);
}
/* Set status if need */
if( status )
{
for( i = 0; i < foundNum; i++ )
{
status->data.ptr[i] = 1;
}
for( i = foundNum; i < needNumPoints; i++ )
{
status->data.ptr[i] = 0;
}
}
foundFeaturePoints = foundNum;
__END__;
/* Free allocated memory */
cvReleaseImage(&grayImage);
cvReleaseImage(&eigImage);
cvReleaseImage(&tmpImage);
cvFree(&cornerPoints);
return foundFeaturePoints;
}
/*-------------------------------------------------------------------------------------*/
int icvRemoveDoublePoins( CvMat *oldPoints,/* Points on prev image */
CvMat *newPoints,/* New points */
CvMat *oldStatus,/* Status for old points */
CvMat *newStatus,
CvMat *origStatus,
float threshold)/* Status for new points */
{
CvMemStorage* storage = 0;
CvSubdiv2D* subdiv = 0;
CvSeq* seq = 0;
int originalPoints = 0;
CV_FUNCNAME( "icvRemoveDoublePoins" );
__BEGIN__;
/* Test input data */
if( oldPoints == 0 || newPoints == 0 ||
oldStatus == 0 || newStatus == 0 || origStatus == 0 )
{
CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
}
if( !CV_IS_MAT(oldPoints) || !CV_IS_MAT(newPoints) )
{
CV_ERROR( CV_StsUnsupportedFormat, "Input parameters points must be a matrices" );
}
if( !CV_IS_MASK_ARR(oldStatus) || !CV_IS_MASK_ARR(newStatus) || !CV_IS_MASK_ARR(origStatus) )
{
CV_ERROR( CV_StsUnsupportedFormat, "Input parameters statuses must be a mask array" );
}
int oldNumPoints;
oldNumPoints = oldPoints->cols;
if( oldNumPoints < 0 )
{
CV_ERROR( CV_StsOutOfRange, "Number of oldPoints must be >= 0" );
}
if( oldStatus->cols != oldNumPoints )
{
CV_ERROR( CV_StsUnmatchedSizes, "Number of old Points and old Statuses must be the same" );
}
int newNumPoints;
newNumPoints = newPoints->cols;
if( newNumPoints < 0 )
{
CV_ERROR( CV_StsOutOfRange, "Number of newPoints must be >= 0" );
}
if( newStatus->cols != newNumPoints )
{
CV_ERROR( CV_StsUnmatchedSizes, "Number of new Points and new Statuses must be the same" );
}
if( origStatus->cols != newNumPoints )
{
CV_ERROR( CV_StsUnmatchedSizes, "Number of new Points and new original Status must be the same" );
}
if( oldPoints->rows != 2)
{
CV_ERROR( CV_StsOutOfRange, "OldPoints must have 2 coordinates >= 0" );
}
if( newPoints->rows != 2)
{
CV_ERROR( CV_StsOutOfRange, "NewPoints must have 2 coordinates >= 0" );
}
if( oldStatus->rows != 1 || newStatus->rows != 1 || origStatus->rows != 1 )
{
CV_ERROR( CV_StsOutOfRange, "Statuses must have 1 row" );
}
/* we have points on image and wants add new points */
/* use subdivision for find nearest points */
/* Define maximum and minimum X and Y */
float minX,minY;
float maxX,maxY;
minX = minY = FLT_MAX;
maxX = maxY = FLT_MIN;
int i;
for( i = 0; i < oldNumPoints; i++ )
{
if( oldStatus->data.ptr[i] )
{
float x = (float)cvmGet(oldPoints,0,i);
float y = (float)cvmGet(oldPoints,1,i);
if( x < minX )
minX = x;
if( x > maxX )
maxX = x;
if( y < minY )
minY = y;
if( y > maxY )
maxY = y;
}
}
for( i = 0; i < newNumPoints; i++ )
{
if( newStatus->data.ptr[i] )
{
float x = (float)cvmGet(newPoints,0,i);
float y = (float)cvmGet(newPoints,1,i);
if( x < minX )
minX = x;
if( x > maxX )
maxX = x;
if( y < minY )
minY = y;
if( y > maxY )
maxY = y;
}
}
/* Creare subdivision for old image */
storage = cvCreateMemStorage(0);
// subdiv = cvCreateSubdivDelaunay2D( cvRect( 0, 0, size.width, size.height ), storage );
subdiv = cvCreateSubdivDelaunay2D( cvRect( cvRound(minX)-5, cvRound(minY)-5, cvRound(maxX-minX)+10, cvRound(maxY-minY)+10 ), storage );
seq = cvCreateSeq( 0, sizeof(*seq), sizeof(CvPoint2D32f), storage );
/* Insert each point from first image */
for( i = 0; i < oldNumPoints; i++ )
{
/* Add just exist points */
if( oldStatus->data.ptr[i] )
{
CvPoint2D32f pt;
pt.x = (float)cvmGet(oldPoints,0,i);
pt.y = (float)cvmGet(oldPoints,1,i);
CvSubdiv2DPoint* point;
point = cvSubdivDelaunay2DInsert( subdiv, pt );
}
}
/* Find nearest points */
/* for each new point */
int flag;
for( i = 0; i < newNumPoints; i++ )
{
flag = 0;
/* Test just exist points */
if( newStatus->data.ptr[i] )
{
flag = 1;
/* Let this is a good point */
//originalPoints++;
CvPoint2D32f pt;
pt.x = (float)cvmGet(newPoints,0,i);
pt.y = (float)cvmGet(newPoints,1,i);
CvSubdiv2DPoint* point = cvFindNearestPoint2D( subdiv, pt );
if( point )
{
/* Test distance of found nearest point */
double minDistance = icvSqDist2D32f( pt, point->pt );
if( minDistance < threshold*threshold )
{
/* Point is double. Turn it off */
/* Set status */
//newStatus->data.ptr[i] = 0;
/* No this is a double point */
//originalPoints--;
flag = 0;
}
}
}
originalPoints += flag;
origStatus->data .ptr[i] = (uchar)flag;
}
__END__;
cvReleaseMemStorage( &storage );
return originalPoints;
}
/*-------------------------------------------------------------------------------------*/
int icvGrowPointsAndStatus(CvMat **oldPoints,CvMat **oldStatus,CvMat *addPoints,CvMat *addStatus,int addCreateNum)
{
/* Add to existing points and status arrays new points or just grow */
CvMat *newOldPoint = 0;
CvMat *newOldStatus = 0;
int newTotalNumber = 0;
CV_FUNCNAME( "icvGrowPointsAndStatus" );
__BEGIN__;
/* Test for errors */
if( oldPoints == 0 || oldStatus == 0 )
{
CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
}
if( *oldPoints == 0 || *oldStatus == 0 )
{
CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
}
if( !CV_IS_MAT(*oldPoints))
{
CV_ERROR( CV_StsUnsupportedFormat, "oldPoints must be a pointer to a matrix" );
}
if( !CV_IS_MASK_ARR(*oldStatus))
{
CV_ERROR( CV_StsUnsupportedFormat, "oldStatus must be a pointer to a mask array" );
}
int oldNum;
oldNum = (*oldPoints)->cols;
if( oldNum < 1 )
{
CV_ERROR( CV_StsOutOfRange, "Number of old points must be > 0" );
}
/* Define if need number of add points */
int addNum;
addNum = 0;
if( addPoints != 0 && addStatus != 0 )
{/* We have aditional points */
if( CV_IS_MAT(addPoints) && CV_IS_MASK_ARR(addStatus) )
{
addNum = addPoints->cols;
if( addStatus->cols != addNum )
{
CV_ERROR( CV_StsOutOfRange, "Number of add points and statuses must be the same" );
}
}
}
/* */
int numCoord;
numCoord = (*oldPoints)->rows;
newTotalNumber = oldNum + addNum + addCreateNum;
if( newTotalNumber )
{
/* Free allocated memory */
newOldPoint = cvCreateMat(numCoord,newTotalNumber,CV_64F);
newOldStatus = cvCreateMat(1,newTotalNumber,CV_8S);
/* Copy old values to */
int i;
/* Clear all values */
cvZero(newOldPoint);
cvZero(newOldStatus);
for( i = 0; i < oldNum; i++ )
{
int currCoord;
for( currCoord = 0; currCoord < numCoord; currCoord++ )
{
cvmSet(newOldPoint,currCoord,i,cvmGet(*oldPoints,currCoord,i));
}
newOldStatus->data.ptr[i] = (*oldStatus)->data.ptr[i];
}
/* Copy additional points and statuses */
if( addNum )
{
for( i = 0; i < addNum; i++ )
{
int currCoord;
for( currCoord = 0; currCoord < numCoord; currCoord++ )
{
cvmSet(newOldPoint,currCoord,i+oldNum,cvmGet(addPoints,currCoord,i));
}
newOldStatus->data.ptr[i+oldNum] = addStatus->data.ptr[i];
//cvmSet(newOldStatus,0,i,cvmGet(addStatus,0,i));
}
}
/* Delete previous data */
cvReleaseMat(oldPoints);
cvReleaseMat(oldStatus);
/* copy pointers */
*oldPoints = newOldPoint;
*oldStatus = newOldStatus;
}
__END__;
return newTotalNumber;
}
static int
icvFindContoursInInterval( const CvArr* src,
/*int minValue, int maxValue,*/
CvMemStorage* storage,
CvSeq** result,
int contourHeaderSize )
{
int count = 0;
CvMemStorage* storage00 = 0;
CvMemStorage* storage01 = 0;
CvSeq* first = 0;
CV_FUNCNAME( "icvFindContoursInInterval" );
__BEGIN__;
int i, j, k, n;
uchar* src_data = 0;
int img_step = 0;
CvSize img_size;
int connect_flag;
int lower_total;
int upper_total;
int all_total;
CvSeq* runs;
CvLinkedRunPoint tmp;
CvLinkedRunPoint* tmp_prev;
CvLinkedRunPoint* upper_line = 0;
CvLinkedRunPoint* lower_line = 0;
CvLinkedRunPoint* last_elem;
CvLinkedRunPoint* upper_run = 0;
CvLinkedRunPoint* lower_run = 0;
CvLinkedRunPoint* prev_point = 0;
CvSeqWriter writer_ext;
CvSeqWriter writer_int;
CvSeqWriter writer;
CvSeqReader reader;
CvSeq* external_contours;
CvSeq* internal_contours;
CvSeq* prev = 0;
if( !storage )
CV_ERROR( CV_StsNullPtr, "NULL storage pointer" );
if( !result )
CV_ERROR( CV_StsNullPtr, "NULL double CvSeq pointer" );
if( contourHeaderSize < (int)sizeof(CvContour))
CV_ERROR( CV_StsBadSize, "Contour header size must be >= sizeof(CvContour)" );
CV_CALL( storage00 = cvCreateChildMemStorage(storage));
CV_CALL( storage01 = cvCreateChildMemStorage(storage));
{
CvMat stub, *mat;
CV_CALL( mat = cvGetMat( src, &stub ));
if( !CV_IS_MASK_ARR(mat))
CV_ERROR( CV_StsBadArg, "Input array must be 8uC1 or 8sC1" );
src_data = mat->data.ptr;
img_step = mat->step;
img_size = cvGetMatSize( mat );
}
// Create temporary sequences
runs = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvLinkedRunPoint), storage00 );
cvStartAppendToSeq( runs, &writer );
cvStartWriteSeq( 0, sizeof(CvSeq), sizeof(CvLinkedRunPoint*), storage01, &writer_ext );
cvStartWriteSeq( 0, sizeof(CvSeq), sizeof(CvLinkedRunPoint*), storage01, &writer_int );
tmp_prev = &(tmp);
tmp_prev->next = 0;
tmp_prev->link = 0;
// First line. None of runs is binded
tmp.pt.y = 0;
i = 0;
CV_WRITE_SEQ_ELEM( tmp, writer );
upper_line = (CvLinkedRunPoint*)CV_GET_WRITTEN_ELEM( writer );
tmp_prev = upper_line;
for( j = 0; j < img_size.width; )
{
for( ; j < img_size.width && !ICV_IS_COMPONENT_POINT(src_data[j]); j++ )
;
if( j == img_size.width )
break;
tmp.pt.x = j;
CV_WRITE_SEQ_ELEM( tmp, writer );
tmp_prev->next = (CvLinkedRunPoint*)CV_GET_WRITTEN_ELEM( writer );
tmp_prev = tmp_prev->next;
for( ; j < img_size.width && ICV_IS_COMPONENT_POINT(src_data[j]); j++ )
;
tmp.pt.x = j-1;
CV_WRITE_SEQ_ELEM( tmp, writer );
tmp_prev->next = (CvLinkedRunPoint*)CV_GET_WRITTEN_ELEM( writer );
tmp_prev->link = tmp_prev->next;
// First point of contour
CV_WRITE_SEQ_ELEM( tmp_prev, writer_ext );
tmp_prev = tmp_prev->next;
}
cvFlushSeqWriter( &writer );
upper_line = upper_line->next;
upper_total = runs->total - 1;
last_elem = tmp_prev;
tmp_prev->next = 0;
for( i = 1; i < img_size.height; i++ )
{
//------// Find runs in next line
src_data += img_step;
tmp.pt.y = i;
all_total = runs->total;
for( j = 0; j < img_size.width; )
{
for( ; j < img_size.width && !ICV_IS_COMPONENT_POINT(src_data[j]); j++ )
;
if( j == img_size.width ) break;
tmp.pt.x = j;
CV_WRITE_SEQ_ELEM( tmp, writer );
tmp_prev->next = (CvLinkedRunPoint*)CV_GET_WRITTEN_ELEM( writer );
tmp_prev = tmp_prev->next;
for( ; j < img_size.width && ICV_IS_COMPONENT_POINT(src_data[j]); j++ )
;
tmp.pt.x = j-1;
CV_WRITE_SEQ_ELEM( tmp, writer );
tmp_prev = tmp_prev->next = (CvLinkedRunPoint*)CV_GET_WRITTEN_ELEM( writer );
}//j
cvFlushSeqWriter( &writer );
lower_line = last_elem->next;
lower_total = runs->total - all_total;
last_elem = tmp_prev;
tmp_prev->next = 0;
//------//
//------// Find links between runs of lower_line and upper_line
upper_run = upper_line;
lower_run = lower_line;
connect_flag = ICV_SINGLE;
for( k = 0, n = 0; k < upper_total/2 && n < lower_total/2; )
{
switch( connect_flag )
{
case ICV_SINGLE:
if( upper_run->next->pt.x < lower_run->next->pt.x )
{
if( upper_run->next->pt.x >= lower_run->pt.x -1 )
{
lower_run->link = upper_run;
connect_flag = ICV_CONNECTING_ABOVE;
prev_point = upper_run->next;
}
else
upper_run->next->link = upper_run;
k++;
upper_run = upper_run->next->next;
}
else
{
if( upper_run->pt.x <= lower_run->next->pt.x +1 )
{
lower_run->link = upper_run;
connect_flag = ICV_CONNECTING_BELOW;
prev_point = lower_run->next;
}
else
{
lower_run->link = lower_run->next;
// First point of contour
CV_WRITE_SEQ_ELEM( lower_run, writer_ext );
}
n++;
lower_run = lower_run->next->next;
}
break;
case ICV_CONNECTING_ABOVE:
if( upper_run->pt.x > lower_run->next->pt.x +1 )
{
prev_point->link = lower_run->next;
connect_flag = ICV_SINGLE;
n++;
lower_run = lower_run->next->next;
}
else
{
prev_point->link = upper_run;
if( upper_run->next->pt.x < lower_run->next->pt.x )
{
k++;
prev_point = upper_run->next;
upper_run = upper_run->next->next;
}
else
{
connect_flag = ICV_CONNECTING_BELOW;
prev_point = lower_run->next;
n++;
lower_run = lower_run->next->next;
}
}
break;
case ICV_CONNECTING_BELOW:
if( lower_run->pt.x > upper_run->next->pt.x +1 )
{
upper_run->next->link = prev_point;
connect_flag = ICV_SINGLE;
k++;
upper_run = upper_run->next->next;
}
else
{
// First point of contour
CV_WRITE_SEQ_ELEM( lower_run, writer_int );
lower_run->link = prev_point;
if( lower_run->next->pt.x < upper_run->next->pt.x )
{
n++;
prev_point = lower_run->next;
lower_run = lower_run->next->next;
}
else
{
connect_flag = ICV_CONNECTING_ABOVE;
k++;
prev_point = upper_run->next;
upper_run = upper_run->next->next;
}
}
break;
}
}// k, n
for( ; n < lower_total/2; n++ )
{
if( connect_flag != ICV_SINGLE )
{
prev_point->link = lower_run->next;
connect_flag = ICV_SINGLE;
lower_run = lower_run->next->next;
continue;
}
lower_run->link = lower_run->next;
//First point of contour
CV_WRITE_SEQ_ELEM( lower_run, writer_ext );
lower_run = lower_run->next->next;
}
for( ; k < upper_total/2; k++ )
{
if( connect_flag != ICV_SINGLE )
{
upper_run->next->link = prev_point;
connect_flag = ICV_SINGLE;
upper_run = upper_run->next->next;
continue;
}
upper_run->next->link = upper_run;
upper_run = upper_run->next->next;
}
upper_line = lower_line;
upper_total = lower_total;
}//i
upper_run = upper_line;
//the last line of image
for( k = 0; k < upper_total/2; k++ )
{
upper_run->next->link = upper_run;
upper_run = upper_run->next->next;
}
//------//
//------//Find end read contours
external_contours = cvEndWriteSeq( &writer_ext );
internal_contours = cvEndWriteSeq( &writer_int );
for( k = 0; k < 2; k++ )
{
CvSeq* contours = k == 0 ? external_contours : internal_contours;
cvStartReadSeq( contours, &reader );
for( j = 0; j < contours->total; j++, count++ )
{
CvLinkedRunPoint* p_temp;
CvLinkedRunPoint* p00;
CvLinkedRunPoint* p01;
CvSeq* contour;
CV_READ_SEQ_ELEM( p00, reader );
p01 = p00;
if( !p00->link )
continue;
cvStartWriteSeq( CV_SEQ_ELTYPE_POINT | CV_SEQ_POLYLINE | CV_SEQ_FLAG_CLOSED,
contourHeaderSize, sizeof(CvPoint), storage, &writer );
do
{
CV_WRITE_SEQ_ELEM( p00->pt, writer );
p_temp = p00;
p00 = p00->link;
p_temp->link = 0;
}
while( p00 != p01 );
contour = cvEndWriteSeq( &writer );
cvBoundingRect( contour, 1 );
if( k != 0 )
contour->flags |= CV_SEQ_FLAG_HOLE;
if( !first )
prev = first = contour;
else
{
contour->h_prev = prev;
prev = prev->h_next = contour;
}
}
}
__END__;
if( !first )
count = -1;
if( result )
*result = first;
cvReleaseMemStorage(&storage00);
cvReleaseMemStorage(&storage01);
return count;
}
CV_IMPL void
cvCalcMotionGradient( const CvArr* mhiimg, CvArr* maskimg,
CvArr* orientation,
double delta1, double delta2,
int aperture_size )
{
CvMat *dX_min = 0, *dY_max = 0;
IplConvKernel* el = 0;
CV_FUNCNAME( "cvCalcMotionGradient" );
__BEGIN__;
CvMat mhistub, *mhi = (CvMat*)mhiimg;
CvMat maskstub, *mask = (CvMat*)maskimg;
CvMat orientstub, *orient = (CvMat*)orientation;
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;
CV_CALL( mhi = cvGetMat( mhi, &mhistub ));
CV_CALL( mask = cvGetMat( mask, &maskstub ));
CV_CALL( orient = cvGetMat( orient, &orientstub ));
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;
CV_CALL( dX_min = cvCreateMat( mhi->rows, mhi->cols, CV_32F ));
CV_CALL( dY_max = cvCreateMat( mhi->rows, mhi->cols, CV_32F ));
/* calc Dx and Dy */
CV_CALL( cvSobel( mhi, dX_min, 1, 0, aperture_size ));
CV_CALL( 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;
}
}
CV_CALL( el = cvCreateStructuringElementEx( aperture_size, aperture_size,
aperture_size/2, aperture_size/2, CV_SHAPE_RECT ));
cvErode( mhi, dX_min, el );
cvDilate( mhi, dY_max, el );
/* 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;
}
}
}
__END__;
cvReleaseMat( &dX_min );
cvReleaseMat( &dY_max );
cvReleaseStructuringElement( &el );
}
/*
Initializes scanner structure.
Prepare image for scanning ( clear borders and convert all pixels to 0-1.
*/
CV_IMPL CvContourScanner
cvStartFindContours( void* _img, CvMemStorage* storage,
int header_size, int mode,
int method, CvPoint offset )
{
int y;
int step;
CvSize size;
uchar *img = 0;
CvContourScanner scanner = 0;
CvMat stub, *mat = (CvMat*)_img;
CV_FUNCNAME( "cvStartFindContours" );
__BEGIN__;
if( !storage )
CV_ERROR( CV_StsNullPtr, "" );
CV_CALL( mat = cvGetMat( mat, &stub ));
if( !CV_IS_MASK_ARR( mat ))
CV_ERROR( CV_StsUnsupportedFormat, "[Start]FindContours support only 8uC1 images" );
size = cvSize( mat->width, mat->height );
step = mat->step;
img = (uchar*)(mat->data.ptr);
if( method < 0 || method > CV_CHAIN_APPROX_TC89_KCOS )
CV_ERROR_FROM_STATUS( CV_BADRANGE_ERR );
if( header_size < (int) (method == CV_CHAIN_CODE ? sizeof( CvChain ) : sizeof( CvContour )))
CV_ERROR_FROM_STATUS( CV_BADSIZE_ERR );
scanner = (CvContourScanner)cvAlloc( sizeof( *scanner ));
if( !scanner )
CV_ERROR_FROM_STATUS( CV_OUTOFMEM_ERR );
memset( scanner, 0, sizeof( *scanner ));
scanner->storage1 = scanner->storage2 = storage;
scanner->img0 = (char *) img;
scanner->img = (char *) (img + step);
scanner->img_step = step;
scanner->img_size.width = size.width - 1; /* exclude rightest column */
scanner->img_size.height = size.height - 1; /* exclude bottomost row */
scanner->mode = mode;
scanner->offset = offset;
scanner->pt.x = scanner->pt.y = 1;
scanner->lnbd.x = 0;
scanner->lnbd.y = 1;
scanner->nbd = 2;
scanner->mode = (int) mode;
scanner->frame_info.contour = &(scanner->frame);
scanner->frame_info.is_hole = 1;
scanner->frame_info.next = 0;
scanner->frame_info.parent = 0;
scanner->frame_info.rect = cvRect( 0, 0, size.width, size.height );
scanner->l_cinfo = 0;
scanner->subst_flag = 0;
scanner->frame.flags = CV_SEQ_FLAG_HOLE;
scanner->approx_method2 = scanner->approx_method1 = method;
if( method == CV_CHAIN_APPROX_TC89_L1 || method == CV_CHAIN_APPROX_TC89_KCOS )
scanner->approx_method1 = CV_CHAIN_CODE;
if( scanner->approx_method1 == CV_CHAIN_CODE )
{
scanner->seq_type1 = CV_SEQ_CHAIN_CONTOUR;
scanner->header_size1 = scanner->approx_method1 == scanner->approx_method2 ?
header_size : sizeof( CvChain );
scanner->elem_size1 = sizeof( char );
}
else
{
scanner->seq_type1 = CV_SEQ_POLYGON;
scanner->header_size1 = scanner->approx_method1 == scanner->approx_method2 ?
header_size : sizeof( CvContour );
scanner->elem_size1 = sizeof( CvPoint );
}
scanner->header_size2 = header_size;
if( scanner->approx_method2 == CV_CHAIN_CODE )
{
scanner->seq_type2 = scanner->seq_type1;
scanner->elem_size2 = scanner->elem_size1;
}
else
{
scanner->seq_type2 = CV_SEQ_POLYGON;
scanner->elem_size2 = sizeof( CvPoint );
}
scanner->seq_type1 = scanner->approx_method1 == CV_CHAIN_CODE ?
CV_SEQ_CHAIN_CONTOUR : CV_SEQ_POLYGON;
scanner->seq_type2 = scanner->approx_method2 == CV_CHAIN_CODE ?
CV_SEQ_CHAIN_CONTOUR : CV_SEQ_POLYGON;
cvSaveMemStoragePos( storage, &(scanner->initial_pos) );
if( method > CV_CHAIN_APPROX_SIMPLE )
{
scanner->storage1 = cvCreateChildMemStorage( scanner->storage2 );
}
if( mode > CV_RETR_LIST )
{
scanner->cinfo_storage = cvCreateChildMemStorage( scanner->storage2 );
scanner->cinfo_set = cvCreateSet( 0, sizeof( CvSet ), sizeof( _CvContourInfo ),
scanner->cinfo_storage );
if( scanner->cinfo_storage == 0 || scanner->cinfo_set == 0 )
CV_ERROR_FROM_STATUS( CV_OUTOFMEM_ERR );
}
/* make zero borders */
memset( img, 0, size.width );
memset( img + step * (size.height - 1), 0, size.width );
for( y = 1, img += step; y < size.height - 1; y++, img += step )
{
img[0] = img[size.width - 1] = 0;
}
/* converts all pixels to 0 or 1 */
cvThreshold( mat, mat, 0, 1, CV_THRESH_BINARY );
CV_CHECK();
__END__;
if( cvGetErrStatus() < 0 )
cvFree( &scanner );
return scanner;
}
/* 8-bit grayscale distance transform function */
static void
icvDistanceATS_L1_8u( const CvMat* src, CvMat* dst )
{
int width = src->cols, height = src->rows;
int a;
uchar lut[256];
int x, y;
const uchar *sbase = src->data.ptr;
uchar *dbase = dst->data.ptr;
int srcstep = src->step;
int dststep = dst->step;
CV_Assert( CV_IS_MASK_ARR( src ) && CV_MAT_TYPE( dst->type ) == CV_8UC1 );
CV_Assert( CV_ARE_SIZES_EQ( src, dst ));
////////////////////// forward scan ////////////////////////
for( x = 0; x < 256; x++ )
lut[x] = CV_CAST_8U(x+1);
//init first pixel to max (we're going to be skipping it)
dbase[0] = (uchar)(sbase[0] == 0 ? 0 : 255);
//first row (scan west only, skip first pixel)
for( x = 1; x < width; x++ )
dbase[x] = (uchar)(sbase[x] == 0 ? 0 : lut[dbase[x-1]]);
for( y = 1; y < height; y++ )
{
sbase += srcstep;
dbase += dststep;
//for left edge, scan north only
a = sbase[0] == 0 ? 0 : lut[dbase[-dststep]];
dbase[0] = (uchar)a;
for( x = 1; x < width; x++ )
{
a = sbase[x] == 0 ? 0 : lut[MIN(a, dbase[x - dststep])];
dbase[x] = (uchar)a;
}
}
////////////////////// backward scan ///////////////////////
a = dbase[width-1];
// do last row east pixel scan here (skip bottom right pixel)
for( x = width - 2; x >= 0; x-- )
{
a = lut[a];
dbase[x] = (uchar)(CV_CALC_MIN_8U(a, dbase[x]));
}
// right edge is the only error case
for( y = height - 2; y >= 0; y-- )
{
dbase -= dststep;
// do right edge
a = lut[dbase[width-1+dststep]];
dbase[width-1] = (uchar)(MIN(a, dbase[width-1]));
for( x = width - 2; x >= 0; x-- )
{
int b = dbase[x+dststep];
a = lut[MIN(a, b)];
dbase[x] = (uchar)(MIN(a, dbase[x]));
}
}
}
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__;
}
/* Wrapper function for distance transform group */
CV_IMPL void
cvDistTransform( const void* srcarr, void* dstarr,
int distType, int maskSize,
const float *mask,
void* labelsarr )
{
cv::Ptr<CvMat> temp;
cv::Ptr<CvMat> src_copy;
cv::Ptr<CvMemStorage> st;
float _mask[5] = {0};
CvMat srcstub, *src = (CvMat*)srcarr;
CvMat dststub, *dst = (CvMat*)dstarr;
CvMat lstub, *labels = (CvMat*)labelsarr;
CvSize size;
//CvIPPDistTransFunc ipp_func = 0;
//CvIPPDistTransFunc2 ipp_inp_func = 0;
src = cvGetMat( src, &srcstub );
dst = cvGetMat( dst, &dststub );
if( !CV_IS_MASK_ARR( src ) || (CV_MAT_TYPE( dst->type ) != CV_32FC1 &&
(CV_MAT_TYPE(dst->type) != CV_8UC1 || distType != CV_DIST_L1 || labels)) )
CV_Error( CV_StsUnsupportedFormat,
"source image must be 8uC1 and the distance map must be 32fC1 "
"(or 8uC1 in case of simple L1 distance transform)" );
if( !CV_ARE_SIZES_EQ( src, dst ))
CV_Error( CV_StsUnmatchedSizes, "the source and the destination images must be of the same size" );
if( maskSize != CV_DIST_MASK_3 && maskSize != CV_DIST_MASK_5 && maskSize != CV_DIST_MASK_PRECISE )
CV_Error( CV_StsBadSize, "Mask size should be 3 or 5 or 0 (presize)" );
if( distType == CV_DIST_C || distType == CV_DIST_L1 )
maskSize = !labels ? CV_DIST_MASK_3 : CV_DIST_MASK_5;
else if( distType == CV_DIST_L2 && labels )
maskSize = CV_DIST_MASK_5;
if( maskSize == CV_DIST_MASK_PRECISE )
{
icvTrueDistTrans( src, dst );
return;
}
if( labels )
{
labels = cvGetMat( labels, &lstub );
if( CV_MAT_TYPE( labels->type ) != CV_32SC1 )
CV_Error( CV_StsUnsupportedFormat, "the output array of labels must be 32sC1" );
if( !CV_ARE_SIZES_EQ( labels, dst ))
CV_Error( CV_StsUnmatchedSizes, "the array of labels has a different size" );
if( maskSize == CV_DIST_MASK_3 )
CV_Error( CV_StsNotImplemented,
"3x3 mask can not be used for \"labeled\" distance transform. Use 5x5 mask" );
}
if( distType == CV_DIST_C || distType == CV_DIST_L1 || distType == CV_DIST_L2 )
{
icvGetDistanceTransformMask( (distType == CV_DIST_C ? 0 :
distType == CV_DIST_L1 ? 1 : 2) + maskSize*10, _mask );
}
else if( distType == CV_DIST_USER )
{
if( !mask )
CV_Error( CV_StsNullPtr, "" );
memcpy( _mask, mask, (maskSize/2 + 1)*sizeof(float));
}
/*if( !labels )
{
if( CV_MAT_TYPE(dst->type) == CV_32FC1 )
ipp_func = (CvIPPDistTransFunc)(maskSize == CV_DIST_MASK_3 ?
icvDistanceTransform_3x3_8u32f_C1R_p : icvDistanceTransform_5x5_8u32f_C1R_p);
else if( src->data.ptr != dst->data.ptr )
ipp_func = (CvIPPDistTransFunc)icvDistanceTransform_3x3_8u_C1R_p;
else
ipp_inp_func = icvDistanceTransform_3x3_8u_C1IR_p;
}*/
size = cvGetMatSize(src);
/*if( (ipp_func || ipp_inp_func) && src->cols >= 4 && src->rows >= 2 )
{
int _imask[3];
_imask[0] = cvRound(_mask[0]);
_imask[1] = cvRound(_mask[1]);
_imask[2] = cvRound(_mask[2]);
if( ipp_func )
{
IPPI_CALL( ipp_func( src->data.ptr, src->step,
dst->data.fl, dst->step, size,
CV_MAT_TYPE(dst->type) == CV_8UC1 ?
(void*)_imask : (void*)_mask ));
}
else
{
IPPI_CALL( ipp_inp_func( src->data.ptr, src->step, size, _imask ));
}
}
else*/ if( CV_MAT_TYPE(dst->type) == CV_8UC1 )
{
icvDistanceATS_L1_8u( src, dst );
}
else
{
int border = maskSize == CV_DIST_MASK_3 ? 1 : 2;
temp = cvCreateMat( size.height + border*2, size.width + border*2, CV_32SC1 );
if( !labels )
{
CvDistTransFunc func = maskSize == CV_DIST_MASK_3 ?
icvDistanceTransform_3x3_C1R :
icvDistanceTransform_5x5_C1R;
func( src->data.ptr, src->step, temp->data.i, temp->step,
dst->data.fl, dst->step, size, _mask );
}
else
{
CvSeq *contours = 0;
CvPoint top_left = {0,0}, bottom_right = {size.width-1,size.height-1};
int label;
st = cvCreateMemStorage();
src_copy = cvCreateMat( size.height, size.width, src->type );
cvCmpS( src, 0, src_copy, CV_CMP_EQ );
cvFindContours( src_copy, st, &contours, sizeof(CvContour),
CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
cvZero( labels );
for( label = 1; contours != 0; contours = contours->h_next, label++ )
{
CvScalar area_color = cvScalarAll(label);
cvDrawContours( labels, contours, area_color, area_color, -255, -1, 8 );
}
cvCopy( src, src_copy );
cvRectangle( src_copy, top_left, bottom_right, cvScalarAll(255), 1, 8 );
icvDistanceTransformEx_5x5_C1R( src_copy->data.ptr, src_copy->step, temp->data.i, temp->step,
dst->data.fl, dst->step, labels->data.i, labels->step, size, _mask );
}
}
}
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 void
cvGoodFeaturesToTrack( const void* image, void* eigImage, void* tempImage,
CvPoint2D32f* corners, int *corner_count,
double quality_level, double min_distance,
const void* maskImage, int block_size,
int use_harris, double harris_k )
{
CvMat* _eigImg = 0;
CvMat* _tmpImg = 0;
CV_FUNCNAME( "cvGoodFeaturesToTrack" );
__BEGIN__;
double max_val = 0;
int max_count = 0;
int count = 0;
int x, y, i, k = 0;
int min_dist;
/* when selecting points, use integer coordinates */
CvPoint *ptr = (CvPoint *) corners;
/* process floating-point images using integer arithmetics */
int *eig_data = 0;
int *tmp_data = 0;
int **ptr_data = 0;
uchar *mask_data = 0;
int mask_step = 0;
CvSize size;
int coi1 = 0, coi2 = 0, coi3 = 0;
CvMat stub, *img = (CvMat*)image;
CvMat eig_stub, *eig = (CvMat*)eigImage;
CvMat tmp_stub, *tmp = (CvMat*)tempImage;
CvMat mask_stub, *mask = (CvMat*)maskImage;
if( corner_count )
{
max_count = *corner_count;
*corner_count = 0;
}
CV_CALL( img = cvGetMat( img, &stub, &coi1 ));
if( eig )
{
CV_CALL( eig = cvGetMat( eig, &eig_stub, &coi2 ));
}
else
{
CV_CALL( _eigImg = cvCreateMat( img->rows, img->cols, CV_32FC1 ));
eig = _eigImg;
}
if( tmp )
{
CV_CALL( tmp = cvGetMat( tmp, &tmp_stub, &coi3 ));
}
else
{
CV_CALL( _tmpImg = cvCreateMat( img->rows, img->cols, CV_32FC1 ));
tmp = _tmpImg;
}
if( mask )
{
CV_CALL( mask = cvGetMat( mask, &mask_stub ));
if( !CV_IS_MASK_ARR( mask ))
{
CV_ERROR( CV_StsBadMask, "" );
}
}
if( coi1 != 0 || coi2 != 0 || coi3 != 0 )
CV_ERROR( CV_BadCOI, "" );
if( CV_MAT_CN(img->type) != 1 ||
CV_MAT_CN(eig->type) != 1 ||
CV_MAT_CN(tmp->type) != 1 )
CV_ERROR( CV_BadNumChannels, cvUnsupportedFormat );
if( CV_MAT_DEPTH(tmp->type) != CV_32F ||
CV_MAT_DEPTH(eig->type) != CV_32F )
CV_ERROR( CV_BadDepth, cvUnsupportedFormat );
if( !corners || !corner_count )
CV_ERROR( CV_StsNullPtr, "" );
if( max_count <= 0 )
CV_ERROR( CV_StsBadArg, "maximal corners number is non positive" );
if( quality_level <= 0 || min_distance < 0 )
CV_ERROR( CV_StsBadArg, "quality level or min distance are non positive" );
if( use_harris )
{
CV_CALL( cvCornerHarris( img, eig, block_size, 3, harris_k ));
}
else
{
CV_CALL( cvCornerMinEigenVal( img, eig, block_size, 3 ));
}
CV_CALL( cvMinMaxLoc( eig, 0, &max_val, 0, 0, mask ));
CV_CALL( cvThreshold( eig, eig, max_val * quality_level,
0, CV_THRESH_TOZERO ));
CV_CALL( cvDilate( eig, tmp ));
min_dist = cvRound( min_distance * min_distance );
size = cvGetMatSize( img );
ptr_data = (int**)(tmp->data.ptr);
eig_data = (int*)(eig->data.ptr);
tmp_data = (int*)(tmp->data.ptr);
if( mask )
{
mask_data = (uchar*)(mask->data.ptr);
mask_step = mask->step;
}
/* collect list of pointers to features - put them into temporary image */
for( y = 1, k = 0; y < size.height - 1; y++ )
{
(char*&)eig_data += eig->step;
(char*&)tmp_data += tmp->step;
mask_data += mask_step;
for( x = 1; x < size.width - 1; x++ )
{
int val = eig_data[x];
if( val != 0 && val == tmp_data[x] && (!mask || mask_data[x]) )
ptr_data[k++] = eig_data + x;
}
}
icvSortFeatures( ptr_data, k, 0 );
/* select the strongest features */
for( i = 0; i < k; i++ )
{
int j = count, ofs = (int)((uchar*)(ptr_data[i]) - eig->data.ptr);
y = ofs / eig->step;
x = (ofs - y * eig->step)/sizeof(float);
if( min_dist != 0 )
{
for( j = 0; j < count; j++ )
{
int dx = x - ptr[j].x;
int dy = y - ptr[j].y;
int dist = dx * dx + dy * dy;
if( dist < min_dist )
break;
}
}
if( j == count )
{
ptr[count].x = x;
ptr[count].y = y;
if( ++count >= max_count )
break;
}
}
/* convert points to floating-point format */
for( i = 0; i < count; i++ )
{
assert( (unsigned)ptr[i].x < (unsigned)size.width &&
(unsigned)ptr[i].y < (unsigned)size.height );
corners[i].x = (float)ptr[i].x;
corners[i].y = (float)ptr[i].y;
}
*corner_count = count;
__END__;
cvReleaseMat( &_eigImg );
cvReleaseMat( &_tmpImg );
}
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__;
}
int cvBGCodeBookDiff( const CvBGCodeBookModel* model, const CvArr* _image,
CvArr* _fgmask, CvRect roi )
{
int maskCount = -1;
CV_FUNCNAME( "cvBGCodeBookDiff" );
__BEGIN__;
CvMat stub, *image = cvGetMat( _image, &stub );
CvMat mstub, *mask = cvGetMat( _fgmask, &mstub );
int x, y;
uchar m0, m1, m2, M0, M1, M2;
CV_ASSERT( model && CV_MAT_TYPE(image->type) == CV_8UC3 &&
image->cols == model->size.width && image->rows == model->size.height &&
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 );
m0 = model->modMin[0]; M0 = model->modMax[0];
m1 = model->modMin[1]; M1 = model->modMax[1];
m2 = model->modMin[2]; M2 = model->modMax[2];
maskCount = roi.height*roi.width;
for( y = 0; y < roi.height; y++ )
{
const uchar* p = image->data.ptr + image->step*(y + roi.y) + roi.x*3;
uchar* m = mask->data.ptr + mask->step*(y + roi.y) + roi.x;
CvBGCodeBookElem** cb = model->cbmap + image->cols*(y + roi.y) + roi.x;
for( x = 0; x < roi.width; x++, p += 3, cb++ )
{
CvBGCodeBookElem *e;
uchar p0 = p[0], p1 = p[1], p2 = p[2];
int l0 = p0 + m0, l1 = p1 + m1, l2 = p2 + m2;
int h0 = p0 - M0, h1 = p1 - M1, h2 = p2 - M2;
m[x] = (uchar)255;
for( e = *cb; e != 0; e = e->next )
{
if( e->boxMin[0] <= l0 && h0 <= e->boxMax[0] &&
e->boxMin[1] <= l1 && h1 <= e->boxMax[1] &&
e->boxMin[2] <= l2 && h2 <= e->boxMax[2] )
{
m[x] = 0;
maskCount--;
break;
}
}
}
}
__END__;
return maskCount;
}
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;
}
}
}
}
void cvBGCodeBookClearStale( CvBGCodeBookModel* model, int staleThresh,
CvRect roi, const CvArr* _mask )
{
CV_FUNCNAME( "cvBGCodeBookClearStale" );
__BEGIN__;
CvMat mstub, *mask = _mask ? cvGetMat( _mask, &mstub ) : 0;
int x, y, T;
CvBGCodeBookElem* freeList;
CV_ASSERT( model && (!mask || (CV_IS_MASK_ARR(mask) &&
mask->cols == model->size.width && mask->rows == model->size.height)) );
if( roi.x == 0 && roi.y == 0 && roi.width == 0 && roi.height == 0 )
{
roi.width = model->size.width;
roi.height = model->size.height;
}
else
CV_ASSERT( (unsigned)roi.x < (unsigned)mask->cols &&
(unsigned)roi.y < (unsigned)mask->rows &&
roi.width >= 0 && roi.height >= 0 &&
roi.x + roi.width <= mask->cols &&
roi.y + roi.height <= mask->rows );
icvInitSatTab();
freeList = model->freeList;
T = model->t;
for( y = 0; y < roi.height; y++ )
{
const uchar* m = mask ? mask->data.ptr + mask->step*(y + roi.y) + roi.x : 0;
CvBGCodeBookElem** cb = model->cbmap + model->size.width*(y + roi.y) + roi.x;
for( x = 0; x < roi.width; x++, cb++ )
{
CvBGCodeBookElem *e, first, *prev = &first;
if( m && m[x] == 0 )
continue;
for( first.next = e = *cb; e != 0; e = prev->next )
{
if( e->stale > staleThresh )
{
prev->next = e->next;
e->next = freeList;
freeList = e;
}
else
{
e->stale = 0;
e->tLastUpdate = T;
prev = e;
}
}
*cb = first.next;
}
}
model->freeList = freeList;
__END__;
}
/* 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;
}
}
}
/* Wrapper function for distance transform group */
CV_IMPL void
cvDistTransform( const void* srcarr, void* dstarr,
int distType, int maskSize,
const float *mask,
void* labelsarr, int labelType )
{
float _mask[5] = {0};
CvMat srcstub, *src = (CvMat*)srcarr;
CvMat dststub, *dst = (CvMat*)dstarr;
CvMat lstub, *labels = (CvMat*)labelsarr;
src = cvGetMat( src, &srcstub );
dst = cvGetMat( dst, &dststub );
if( !CV_IS_MASK_ARR( src ) || (CV_MAT_TYPE( dst->type ) != CV_32FC1 &&
(CV_MAT_TYPE(dst->type) != CV_8UC1 || distType != CV_DIST_L1 || labels)) )
CV_Error( CV_StsUnsupportedFormat,
"source image must be 8uC1 and the distance map must be 32fC1 "
"(or 8uC1 in case of simple L1 distance transform)" );
if( !CV_ARE_SIZES_EQ( src, dst ))
CV_Error( CV_StsUnmatchedSizes, "the source and the destination images must be of the same size" );
if( maskSize != CV_DIST_MASK_3 && maskSize != CV_DIST_MASK_5 && maskSize != CV_DIST_MASK_PRECISE )
CV_Error( CV_StsBadSize, "Mask size should be 3 or 5 or 0 (presize)" );
if( distType == CV_DIST_C || distType == CV_DIST_L1 )
maskSize = !labels ? CV_DIST_MASK_3 : CV_DIST_MASK_5;
else if( distType == CV_DIST_L2 && labels )
maskSize = CV_DIST_MASK_5;
if( maskSize == CV_DIST_MASK_PRECISE )
{
icvTrueDistTrans( src, dst );
return;
}
if( labels )
{
labels = cvGetMat( labels, &lstub );
if( CV_MAT_TYPE( labels->type ) != CV_32SC1 )
CV_Error( CV_StsUnsupportedFormat, "the output array of labels must be 32sC1" );
if( !CV_ARE_SIZES_EQ( labels, dst ))
CV_Error( CV_StsUnmatchedSizes, "the array of labels has a different size" );
if( maskSize == CV_DIST_MASK_3 )
CV_Error( CV_StsNotImplemented,
"3x3 mask can not be used for \"labeled\" distance transform. Use 5x5 mask" );
}
if( distType == CV_DIST_C || distType == CV_DIST_L1 || distType == CV_DIST_L2 )
{
icvGetDistanceTransformMask( (distType == CV_DIST_C ? 0 :
distType == CV_DIST_L1 ? 1 : 2) + maskSize*10, _mask );
}
else if( distType == CV_DIST_USER )
{
if( !mask )
CV_Error( CV_StsNullPtr, "" );
memcpy( _mask, mask, (maskSize/2 + 1)*sizeof(float));
}
CvSize size = cvGetMatSize(src);
if( CV_MAT_TYPE(dst->type) == CV_8UC1 )
{
icvDistanceATS_L1_8u( src, dst );
}
else
{
int border = maskSize == CV_DIST_MASK_3 ? 1 : 2;
cv::Ptr<CvMat> temp = cvCreateMat( size.height + border*2, size.width + border*2, CV_32SC1 );
if( !labels )
{
CvDistTransFunc func = maskSize == CV_DIST_MASK_3 ?
icvDistanceTransform_3x3_C1R :
icvDistanceTransform_5x5_C1R;
func( src->data.ptr, src->step, temp->data.i, temp->step,
dst->data.fl, dst->step, size, _mask );
}
else
{
cvZero( labels );
if( labelType == CV_DIST_LABEL_CCOMP )
{
CvSeq *contours = 0;
cv::Ptr<CvMemStorage> st = cvCreateMemStorage();
cv::Ptr<CvMat> src_copy = cvCreateMat( size.height+border*2, size.width+border*2, src->type );
cvCopyMakeBorder(src, src_copy, cvPoint(border, border), IPL_BORDER_CONSTANT, cvScalarAll(255));
cvCmpS( src_copy, 0, src_copy, CV_CMP_EQ );
cvFindContours( src_copy, st, &contours, sizeof(CvContour),
CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE, cvPoint(-border, -border));
for( int label = 1; contours != 0; contours = contours->h_next, label++ )
{
CvScalar area_color = cvScalarAll(label);
cvDrawContours( labels, contours, area_color, area_color, -255, -1, 8 );
}
}
else
{
int k = 1;
for( int i = 0; i < src->rows; i++ )
{
const uchar* srcptr = src->data.ptr + src->step*i;
int* labelptr = (int*)(labels->data.ptr + labels->step*i);
for( int j = 0; j < src->cols; j++ )
if( srcptr[j] == 0 )
labelptr[j] = k++;
}
}
icvDistanceTransformEx_5x5_C1R( src->data.ptr, src->step, temp->data.i, temp->step,
dst->data.fl, dst->step, labels->data.i, labels->step, size, _mask );
}
}
}
