/* external contour area function */
CV_IMPL double
cvContourArea( const void *array, CvSlice slice, int oriented )
{
double area = 0;
CvContour contour_header;
CvSeq* contour = 0;
CvSeqBlock block;
if( CV_IS_SEQ( array ))
{
contour = (CvSeq*)array;
if( !CV_IS_SEQ_POLYLINE( contour ))
CV_Error( CV_StsBadArg, "Unsupported sequence type" );
}
else
{
contour = cvPointSeqFromMat( CV_SEQ_KIND_CURVE, array, &contour_header, &block );
}
if( cvSliceLength( slice, contour ) == contour->total )
{
IPPI_CALL( icvContourArea( contour, &area ));
}
else
{
if( CV_SEQ_ELTYPE( contour ) != CV_32SC2 )
CV_Error( CV_StsUnsupportedFormat,
"Only curves with integer coordinates are supported in case of contour slice" );
IPPI_CALL( icvContourSecArea( contour, slice, &area ));
}
return oriented ? area : fabs(area);
}
/* external contour area function */
CV_IMPL double
cvContourArea( const void *array, CvSlice slice, int oriented )
{
double area = 0;
CvContour contour_header;
CvSeq* contour = 0;
CvSeqBlock block;
if( CV_IS_SEQ( array ))
{
contour = (CvSeq*)array;
if( !CV_IS_SEQ_POLYLINE( contour ))
CV_Error( CV_StsBadArg, "Unsupported sequence type" );
}
else
{
contour = cvPointSeqFromMat( CV_SEQ_KIND_CURVE, array, &contour_header, &block );
}
if( cvSliceLength( slice, contour ) == contour->total )
{
cv::AutoBuffer<double> abuf;
cv::Mat points = cv::cvarrToMat(contour, false, false, 0, &abuf);
return cv::contourArea( points, oriented !=0 );
}
if( CV_SEQ_ELTYPE( contour ) != CV_32SC2 )
CV_Error( CV_StsUnsupportedFormat,
"Only curves with integer coordinates are supported in case of contour slice" );
area = icvContourSecArea( contour, slice );
return oriented ? area : fabs(area);
}
/* Calculates bounding rectagnle of a point set or retrieves already calculated */
CV_IMPL CvRect
cvBoundingRect( CvArr* array, int update )
{
CvRect rect = { 0, 0, 0, 0 };
CvContour contour_header;
CvSeq* ptseq = 0;
CvSeqBlock block;
CvMat stub, *mat = 0;
int calculate = update;
if( CV_IS_SEQ( array ))
{
ptseq = (CvSeq*)array;
if( !CV_IS_SEQ_POINT_SET( ptseq ))
CV_Error( CV_StsBadArg, "Unsupported sequence type" );
if( ptseq->header_size < (int)sizeof(CvContour))
{
update = 0;
calculate = 1;
}
}
else
{
mat = cvGetMat( array, &stub );
if( CV_MAT_TYPE(mat->type) == CV_32SC2 ||
CV_MAT_TYPE(mat->type) == CV_32FC2 )
{
ptseq = cvPointSeqFromMat(CV_SEQ_KIND_GENERIC, mat, &contour_header, &block);
mat = 0;
}
else if( CV_MAT_TYPE(mat->type) != CV_8UC1 &&
CV_MAT_TYPE(mat->type) != CV_8SC1 )
CV_Error( CV_StsUnsupportedFormat,
"The image/matrix format is not supported by the function" );
update = 0;
calculate = 1;
}
if( !calculate )
return ((CvContour*)ptseq)->rect;
if( mat )
{
rect = cv::maskBoundingRect(cv::cvarrToMat(mat));
}
else if( ptseq->total )
{
cv::AutoBuffer<double> abuf;
rect = cv::pointSetBoundingRect(cv::cvarrToMat(ptseq, false, false, 0, &abuf));
}
if( update )
((CvContour*)ptseq)->rect = rect;
return rect;
}
Moments ocl_moments(InputArray _contour) //for contour
{
CvMoments mom;
memset(&mom, 0, sizeof(mom));
Mat arr = _contour.getMat();
CvMat c_array = arr;
const void* array = &c_array;
CvSeq* contour = 0;
if( CV_IS_SEQ( array ))
{
contour = (CvSeq*)(array);
if( !CV_IS_SEQ_POINT_SET( contour ))
CV_Error( CV_StsBadArg, "The passed sequence is not a valid contour" );
}
int type, coi = 0;
CvMat stub, *mat = (CvMat*)(array);
CvContour contourHeader;
CvSeqBlock block;
if( !contour )
{
mat = cvGetMat( mat, &stub, &coi );
type = CV_MAT_TYPE( mat->type );
if( type == CV_32SC2 || type == CV_32FC2 )
{
contour = cvPointSeqFromMat(
CV_SEQ_KIND_CURVE | CV_SEQ_FLAG_CLOSED,
mat, &contourHeader, &block );
}
}
CV_Assert(contour);
icvContourMoments(contour, &mom);
return mom;
}
CV_IMPL CvBox2D
cvFitEllipse2( const CvArr* array )
{
CvBox2D box;
CV_FUNCNAME( "cvFitEllipse2" );
memset( &box, 0, sizeof(box));
__BEGIN__;
CvContour contour_header;
CvSeq* ptseq = 0;
CvSeqBlock block;
if( CV_IS_SEQ( array ))
{
ptseq = (CvSeq*)array;
if( !CV_IS_SEQ_POINT_SET( ptseq ))
CV_ERROR( CV_StsBadArg, "Unsupported sequence type" );
}
else
{
CV_CALL( ptseq = cvPointSeqFromMat(
CV_SEQ_KIND_GENERIC, array, &contour_header, &block ));
}
if( ptseq->total < 6 )
CV_ERROR( CV_StsBadSize, "Number of points should be >= 6" );
IPPI_CALL( icvFitEllipse_32f( ptseq, &box ));
__END__;
return box;
}
CV_IMPL double
cvPointPolygonTest( const CvArr* _contour, CvPoint2D32f pt, int measure_dist )
{
double result = 0;
CV_FUNCNAME( "cvCheckPointPolygon" );
__BEGIN__;
CvSeqBlock block;
CvContour header;
CvSeq* contour = (CvSeq*)_contour;
CvSeqReader reader;
int i, total, counter = 0;
int is_float;
double min_dist_num = FLT_MAX, min_dist_denom = 1;
CvPoint ip = {0,0};
if( !CV_IS_SEQ(contour) )
{
CV_CALL( contour = cvPointSeqFromMat( CV_SEQ_KIND_CURVE + CV_SEQ_FLAG_CLOSED,
_contour, &header, &block ));
}
else if( CV_IS_SEQ_POLYGON(contour) )
{
if( contour->header_size == sizeof(CvContour) && !measure_dist )
{
CvRect r = ((CvContour*)contour)->rect;
if( pt.x < r.x || pt.y < r.y ||
pt.x >= r.x + r.width || pt.y >= r.y + r.height )
return -100;
}
}
else if( CV_IS_SEQ_CHAIN(contour) )
{
CV_ERROR( CV_StsBadArg,
"Chains are not supported. Convert them to polygonal representation using cvApproxChains()" );
}
else
CV_ERROR( CV_StsBadArg, "Input contour is neither a valid sequence nor a matrix" );
total = contour->total;
is_float = CV_SEQ_ELTYPE(contour) == CV_32FC2;
cvStartReadSeq( contour, &reader, -1 );
if( !is_float && !measure_dist && (ip.x = cvRound(pt.x)) == pt.x && (ip.y = cvRound(pt.y)) == pt.y )
{
// the fastest "pure integer" branch
CvPoint v0, v;
CV_READ_SEQ_ELEM( v, reader );
for( i = 0; i < total; i++ )
{
int dist;
v0 = v;
CV_READ_SEQ_ELEM( v, reader );
if( (v0.y <= ip.y && v.y <= ip.y) ||
(v0.y > ip.y && v.y > ip.y) ||
(v0.x < ip.x && v.x < ip.x) )
{
if( ip.y == v.y && (ip.x == v.x || (ip.y == v0.y &&
((v0.x <= ip.x && ip.x <= v.x) || (v.x <= ip.x && ip.x <= v0.x)))) )
EXIT;
continue;
}
dist = (ip.y - v0.y)*(v.x - v0.x) - (ip.x - v0.x)*(v.y - v0.y);
if( dist == 0 )
EXIT;
if( v.y < v0.y )
dist = -dist;
counter += dist > 0;
}
result = counter % 2 == 0 ? -100 : 100;
}
else
{
CvPoint2D32f v0, v;
CvPoint iv;
if( is_float )
{
CV_READ_SEQ_ELEM( v, reader );
}
else
{
CV_READ_SEQ_ELEM( iv, reader );
v = cvPointTo32f( iv );
}
if( !measure_dist )
{
for( i = 0; i < total; i++ )
{
double dist;
v0 = v;
if( is_float )
{
CV_READ_SEQ_ELEM( v, reader );
}
else
{
CV_READ_SEQ_ELEM( iv, reader );
v = cvPointTo32f( iv );
}
if( (v0.y <= pt.y && v.y <= pt.y) ||
(v0.y > pt.y && v.y > pt.y) ||
(v0.x < pt.x && v.x < pt.x) )
{
if( pt.y == v.y && (pt.x == v.x || (pt.y == v0.y &&
((v0.x <= pt.x && pt.x <= v.x) || (v.x <= pt.x && pt.x <= v0.x)))) )
EXIT;
continue;
}
dist = (double)(pt.y - v0.y)*(v.x - v0.x) - (double)(pt.x - v0.x)*(v.y - v0.y);
if( dist == 0 )
EXIT;
if( v.y < v0.y )
dist = -dist;
counter += dist > 0;
}
result = counter % 2 == 0 ? -100 : 100;
}
else
{
for( i = 0; i < total; i++ )
{
double dx, dy, dx1, dy1, dx2, dy2, dist_num, dist_denom = 1;
v0 = v;
if( is_float )
{
CV_READ_SEQ_ELEM( v, reader );
}
else
{
CV_READ_SEQ_ELEM( iv, reader );
v = cvPointTo32f( iv );
}
dx = v.x - v0.x; dy = v.y - v0.y;
dx1 = pt.x - v0.x; dy1 = pt.y - v0.y;
dx2 = pt.x - v.x; dy2 = pt.y - v.y;
if( dx1*dx + dy1*dy <= 0 )
dist_num = dx1*dx1 + dy1*dy1;
else if( dx2*dx + dy2*dy >= 0 )
dist_num = dx2*dx2 + dy2*dy2;
else
{
dist_num = (dy1*dx - dx1*dy);
dist_num *= dist_num;
dist_denom = dx*dx + dy*dy;
}
if( dist_num*min_dist_denom < min_dist_num*dist_denom )
{
min_dist_num = dist_num;
min_dist_denom = dist_denom;
if( min_dist_num == 0 )
break;
}
if( (v0.y <= pt.y && v.y <= pt.y) ||
(v0.y > pt.y && v.y > pt.y) ||
(v0.x < pt.x && v.x < pt.x) )
continue;
dist_num = dy1*dx - dx1*dy;
if( dy < 0 )
dist_num = -dist_num;
counter += dist_num > 0;
}
result = sqrt(min_dist_num/min_dist_denom);
if( counter % 2 == 0 )
result = -result;
}
}
__END__;
return result;
}
CV_IMPL CvBox2D
cvFitEllipse2( const CvArr* array )
{
CvBox2D box;
double* Ad = 0, *bd = 0;
CV_FUNCNAME( "cvFitEllipse2" );
memset( &box, 0, sizeof(box));
__BEGIN__;
CvContour contour_header;
CvSeq* ptseq = 0;
CvSeqBlock block;
int n;
if( CV_IS_SEQ( array ))
{
ptseq = (CvSeq*)array;
if( !CV_IS_SEQ_POINT_SET( ptseq ))
CV_ERROR( CV_StsBadArg, "Unsupported sequence type" );
}
else
{
CV_CALL( ptseq = cvPointSeqFromMat(
CV_SEQ_KIND_GENERIC, array, &contour_header, &block ));
}
n = ptseq->total;
if( n < 5 )
CV_ERROR( CV_StsBadSize, "Number of points should be >= 6" );
#if 1
icvFitEllipse_F( ptseq, &box );
#else
/*
* New fitellipse algorithm, contributed by Dr. Daniel Weiss
*/
{
double gfp[5], rp[5], t;
CvMat A, b, x;
const double min_eps = 1e-6;
int i, is_float;
CvSeqReader reader;
CV_CALL( Ad = (double*)cvAlloc( n*5*sizeof(Ad[0]) ));
CV_CALL( bd = (double*)cvAlloc( n*sizeof(bd[0]) ));
// first fit for parameters A - E
A = cvMat( n, 5, CV_64F, Ad );
b = cvMat( n, 1, CV_64F, bd );
x = cvMat( 5, 1, CV_64F, gfp );
cvStartReadSeq( ptseq, &reader );
is_float = CV_SEQ_ELTYPE(ptseq) == CV_32FC2;
for( i = 0; i < n; i++ )
{
CvPoint2D32f p;
if( is_float )
p = *(CvPoint2D32f*)(reader.ptr);
else
{
p.x = (float)((int*)reader.ptr)[0];
p.y = (float)((int*)reader.ptr)[1];
}
CV_NEXT_SEQ_ELEM( sizeof(p), reader );
bd[i] = 10000.0; // 1.0?
Ad[i*5] = -(double)p.x * p.x; // A - C signs inverted as proposed by APP
Ad[i*5 + 1] = -(double)p.y * p.y;
Ad[i*5 + 2] = -(double)p.x * p.y;
Ad[i*5 + 3] = p.x;
Ad[i*5 + 4] = p.y;
}
cvSolve( &A, &b, &x, CV_SVD );
// now use general-form parameters A - E to find the ellipse center:
// differentiate general form wrt x/y to get two equations for cx and cy
A = cvMat( 2, 2, CV_64F, Ad );
b = cvMat( 2, 1, CV_64F, bd );
x = cvMat( 2, 1, CV_64F, rp );
Ad[0] = 2 * gfp[0];
Ad[1] = Ad[2] = gfp[2];
Ad[3] = 2 * gfp[1];
bd[0] = gfp[3];
bd[1] = gfp[4];
cvSolve( &A, &b, &x, CV_SVD );
// re-fit for parameters A - C with those center coordinates
A = cvMat( n, 3, CV_64F, Ad );
b = cvMat( n, 1, CV_64F, bd );
x = cvMat( 3, 1, CV_64F, gfp );
for( i = 0; i < n; i++ )
{
CvPoint2D32f p;
if( is_float )
p = *(CvPoint2D32f*)(reader.ptr);
else
{
p.x = (float)((int*)reader.ptr)[0];
p.y = (float)((int*)reader.ptr)[1];
}
CV_NEXT_SEQ_ELEM( sizeof(p), reader );
bd[i] = 1.0;
Ad[i * 3] = (p.x - rp[0]) * (p.x - rp[0]);
Ad[i * 3 + 1] = (p.y - rp[1]) * (p.y - rp[1]);
Ad[i * 3 + 2] = (p.x - rp[0]) * (p.y - rp[1]);
}
cvSolve(&A, &b, &x, CV_SVD);
// store angle and radii
rp[4] = -0.5 * atan2(gfp[2], gfp[1] - gfp[0]); // convert from APP angle usage
t = sin(-2.0 * rp[4]);
if( fabs(t) > fabs(gfp[2])*min_eps )
t = gfp[2]/t;
else
t = gfp[1] - gfp[0];
rp[2] = fabs(gfp[0] + gfp[1] - t);
if( rp[2] > min_eps )
rp[2] = sqrt(2.0 / rp[2]);
rp[3] = fabs(gfp[0] + gfp[1] + t);
if( rp[3] > min_eps )
rp[3] = sqrt(2.0 / rp[3]);
box.center.x = (float)rp[0];
box.center.y = (float)rp[1];
box.size.width = (float)(rp[2]*2);
box.size.height = (float)(rp[3]*2);
if( box.size.width > box.size.height )
{
float tmp;
CV_SWAP( box.size.width, box.size.height, tmp );
box.angle = (float)(90 + rp[4]*180/CV_PI);
}
if( box.angle < -180 )
box.angle += 360;
if( box.angle > 360 )
box.angle -= 360;
}
#endif
__END__;
cvFree( &Ad );
cvFree( &bd );
return box;
}
CV_IMPL int
cvMinEnclosingCircle( const void* array, CvPoint2D32f * _center, float *_radius )
{
const int max_iters = 100;
const float eps = FLT_EPSILON*2;
CvPoint2D32f center = { 0, 0 };
float radius = 0;
int result = 0;
if( _center )
_center->x = _center->y = 0.f;
if( _radius )
*_radius = 0;
CV_FUNCNAME( "cvMinEnclosingCircle" );
__BEGIN__;
CvSeqReader reader;
int i, k, count;
CvPoint2D32f pts[8];
CvContour contour_header;
CvSeqBlock block;
CvSeq* sequence = 0;
int is_float;
if( !_center || !_radius )
CV_ERROR( CV_StsNullPtr, "Null center or radius pointers" );
if( CV_IS_SEQ(array) )
{
sequence = (CvSeq*)array;
if( !CV_IS_SEQ_POINT_SET( sequence ))
CV_ERROR( CV_StsBadArg, "The passed sequence is not a valid contour" );
}
else
{
CV_CALL( sequence = cvPointSeqFromMat(
CV_SEQ_KIND_GENERIC, array, &contour_header, &block ));
}
if( sequence->total <= 0 )
CV_ERROR_FROM_STATUS( CV_BADSIZE_ERR );
CV_CALL( cvStartReadSeq( sequence, &reader, 0 ));
count = sequence->total;
is_float = CV_SEQ_ELTYPE(sequence) == CV_32FC2;
if( !is_float )
{
CvPoint *pt_left, *pt_right, *pt_top, *pt_bottom;
CvPoint pt;
pt_left = pt_right = pt_top = pt_bottom = (CvPoint *)(reader.ptr);
CV_READ_SEQ_ELEM( pt, reader );
for( i = 1; i < count; i++ )
{
CvPoint* pt_ptr = (CvPoint*)reader.ptr;
CV_READ_SEQ_ELEM( pt, reader );
if( pt.x < pt_left->x )
pt_left = pt_ptr;
if( pt.x > pt_right->x )
pt_right = pt_ptr;
if( pt.y < pt_top->y )
pt_top = pt_ptr;
if( pt.y > pt_bottom->y )
pt_bottom = pt_ptr;
}
pts[0] = cvPointTo32f( *pt_left );
pts[1] = cvPointTo32f( *pt_right );
pts[2] = cvPointTo32f( *pt_top );
pts[3] = cvPointTo32f( *pt_bottom );
}
else
{
CvPoint2D32f *pt_left, *pt_right, *pt_top, *pt_bottom;
CvPoint2D32f pt;
pt_left = pt_right = pt_top = pt_bottom = (CvPoint2D32f *) (reader.ptr);
CV_READ_SEQ_ELEM( pt, reader );
for( i = 1; i < count; i++ )
{
CvPoint2D32f* pt_ptr = (CvPoint2D32f*)reader.ptr;
CV_READ_SEQ_ELEM( pt, reader );
if( pt.x < pt_left->x )
pt_left = pt_ptr;
if( pt.x > pt_right->x )
pt_right = pt_ptr;
if( pt.y < pt_top->y )
pt_top = pt_ptr;
if( pt.y > pt_bottom->y )
pt_bottom = pt_ptr;
}
pts[0] = *pt_left;
pts[1] = *pt_right;
pts[2] = *pt_top;
pts[3] = *pt_bottom;
}
for( k = 0; k < max_iters; k++ )
{
double min_delta = 0, delta;
CvPoint2D32f ptfl;
icvFindEnslosingCicle4pts_32f( pts, ¢er, &radius );
cvStartReadSeq( sequence, &reader, 0 );
for( i = 0; i < count; i++ )
{
if( !is_float )
{
ptfl.x = (float)((CvPoint*)reader.ptr)->x;
ptfl.y = (float)((CvPoint*)reader.ptr)->y;
}
else
{
ptfl = *(CvPoint2D32f*)reader.ptr;
}
CV_NEXT_SEQ_ELEM( sequence->elem_size, reader );
delta = icvIsPtInCircle( ptfl, center, radius );
if( delta < min_delta )
{
min_delta = delta;
pts[3] = ptfl;
}
}
result = min_delta >= 0;
if( result )
break;
}
if( !result )
{
cvStartReadSeq( sequence, &reader, 0 );
radius = 0.f;
for( i = 0; i < count; i++ )
{
CvPoint2D32f ptfl;
float t, dx, dy;
if( !is_float )
{
ptfl.x = (float)((CvPoint*)reader.ptr)->x;
ptfl.y = (float)((CvPoint*)reader.ptr)->y;
}
else
{
ptfl = *(CvPoint2D32f*)reader.ptr;
}
CV_NEXT_SEQ_ELEM( sequence->elem_size, reader );
dx = center.x - ptfl.x;
dy = center.y - ptfl.y;
t = dx*dx + dy*dy;
radius = MAX(radius,t);
}
radius = (float)(sqrt(radius)*(1 + eps));
result = 1;
}
__END__;
*_center = center;
*_radius = radius;
return result;
}
/* Calculates bounding rectagnle of a point set or retrieves already calculated */
CV_IMPL CvRect
cvBoundingRect( CvArr* array, int update )
{
CvSeqReader reader;
CvRect rect = { 0, 0, 0, 0 };
CvContour contour_header;
CvSeq* ptseq = 0;
CvSeqBlock block;
CV_FUNCNAME( "cvBoundingRect" );
__BEGIN__;
CvMat stub, *mat = 0;
int xmin = 0, ymin = 0, xmax = -1, ymax = -1, i, j, k;
int calculate = update;
if( CV_IS_SEQ( array ))
{
ptseq = (CvSeq*)array;
if( !CV_IS_SEQ_POINT_SET( ptseq ))
CV_ERROR( CV_StsBadArg, "Unsupported sequence type" );
if( ptseq->header_size < (int)sizeof(CvContour))
{
/*if( update == 1 )
CV_ERROR( CV_StsBadArg, "The header is too small to fit the rectangle, "
"so it could not be updated" );*/
update = 0;
calculate = 1;
}
}
else
{
CV_CALL( mat = cvGetMat( array, &stub ));
if( CV_MAT_TYPE(mat->type) == CV_32SC2 ||
CV_MAT_TYPE(mat->type) == CV_32FC2 )
{
CV_CALL( ptseq = cvPointSeqFromMat(
CV_SEQ_KIND_GENERIC, mat, &contour_header, &block ));
mat = 0;
}
else if( CV_MAT_TYPE(mat->type) != CV_8UC1 &&
CV_MAT_TYPE(mat->type) != CV_8SC1 )
CV_ERROR( CV_StsUnsupportedFormat,
"The image/matrix format is not supported by the function" );
update = 0;
calculate = 1;
}
if( !calculate )
{
rect = ((CvContour*)ptseq)->rect;
EXIT;
}
if( mat )
{
CvSize size = cvGetMatSize(mat);
xmin = size.width;
ymin = -1;
for( i = 0; i < size.height; i++ )
{
uchar* _ptr = mat->data.ptr + i*mat->step;
uchar* ptr = (uchar*)cvAlignPtr(_ptr, 4);
int have_nz = 0, k_min, offset = (int)(ptr - _ptr);
j = 0;
offset = MIN(offset, size.width);
for( ; j < offset; j++ )
if( _ptr[j] )
{
have_nz = 1;
break;
}
if( j < offset )
{
if( j < xmin )
xmin = j;
if( j > xmax )
xmax = j;
}
if( offset < size.width )
{
xmin -= offset;
xmax -= offset;
size.width -= offset;
j = 0;
for( ; j <= xmin - 4; j += 4 )
if( *((int*)(ptr+j)) )
break;
for( ; j < xmin; j++ )
if( ptr[j] )
{
xmin = j;
if( j > xmax )
xmax = j;
have_nz = 1;
break;
}
k_min = MAX(j-1, xmax);
k = size.width - 1;
for( ; k > k_min && (k&3) != 3; k-- )
if( ptr[k] )
break;
if( k > k_min && (k&3) == 3 )
{
for( ; k > k_min+3; k -= 4 )
if( *((int*)(ptr+k-3)) )
break;
}
for( ; k > k_min; k-- )
if( ptr[k] )
{
xmax = k;
have_nz = 1;
break;
}
if( !have_nz )
{
j &= ~3;
for( ; j <= k - 3; j += 4 )
if( *((int*)(ptr+j)) )
break;
for( ; j <= k; j++ )
if( ptr[j] )
{
have_nz = 1;
break;
}
}
xmin += offset;
xmax += offset;
size.width += offset;
}
if( have_nz )
{
if( ymin < 0 )
ymin = i;
ymax = i;
}
}
if( xmin >= size.width )
xmin = ymin = 0;
}
else if( ptseq->total )
{
int is_float = CV_SEQ_ELTYPE(ptseq) == CV_32FC2;
cvStartReadSeq( ptseq, &reader, 0 );
if( !is_float )
{
CvPoint pt;
/* init values */
CV_READ_SEQ_ELEM( pt, reader );
xmin = xmax = pt.x;
ymin = ymax = pt.y;
for( i = 1; i < ptseq->total; i++ )
{
CV_READ_SEQ_ELEM( pt, reader );
if( xmin > pt.x )
xmin = pt.x;
if( xmax < pt.x )
xmax = pt.x;
if( ymin > pt.y )
ymin = pt.y;
if( ymax < pt.y )
ymax = pt.y;
}
}
else
{
CvPoint pt;
Cv32suf v;
/* init values */
CV_READ_SEQ_ELEM( pt, reader );
xmin = xmax = CV_TOGGLE_FLT(pt.x);
ymin = ymax = CV_TOGGLE_FLT(pt.y);
for( i = 1; i < ptseq->total; i++ )
{
CV_READ_SEQ_ELEM( pt, reader );
pt.x = CV_TOGGLE_FLT(pt.x);
pt.y = CV_TOGGLE_FLT(pt.y);
if( xmin > pt.x )
xmin = pt.x;
if( xmax < pt.x )
xmax = pt.x;
if( ymin > pt.y )
ymin = pt.y;
if( ymax < pt.y )
ymax = pt.y;
}
v.i = CV_TOGGLE_FLT(xmin); xmin = cvFloor(v.f);
v.i = CV_TOGGLE_FLT(ymin); ymin = cvFloor(v.f);
/* because right and bottom sides of
the bounding rectangle are not inclusive
(note +1 in width and height calculation below),
cvFloor is used here instead of cvCeil */
v.i = CV_TOGGLE_FLT(xmax); xmax = cvFloor(v.f);
v.i = CV_TOGGLE_FLT(ymax); ymax = cvFloor(v.f);
}
}
rect.x = xmin;
rect.y = ymin;
rect.width = xmax - xmin + 1;
rect.height = ymax - ymin + 1;
if( update )
((CvContour*)ptseq)->rect = rect;
__END__;
return rect;
}
CV_IMPL CvSeq*
cvConvexHull2( const CvArr* array, void* hull_storage,
int orientation, int return_points )
{
union { CvContour* c; CvSeq* s; } hull;
CvPoint** pointer = 0;
CvPoint2D32f** pointerf = 0;
int* stack = 0;
CV_FUNCNAME( "cvConvexHull2" );
hull.s = 0;
__BEGIN__;
CvMat* mat = 0;
CvSeqReader reader;
CvSeqWriter writer;
CvContour contour_header;
union { CvContour c; CvSeq s; } hull_header;
CvSeqBlock block, hullblock;
CvSeq* ptseq = 0;
CvSeq* hullseq = 0;
int is_float;
int* t_stack;
int t_count;
int i, miny_ind = 0, maxy_ind = 0, total;
int hulltype;
int stop_idx;
sklansky_func sklansky;
if( CV_IS_SEQ( array ))
{
ptseq = (CvSeq*)array;
if( !CV_IS_SEQ_POINT_SET( ptseq ))
CV_ERROR( CV_StsBadArg, "Unsupported sequence type" );
if( hull_storage == 0 )
hull_storage = ptseq->storage;
}
else
{
CV_CALL( ptseq = cvPointSeqFromMat(
CV_SEQ_KIND_GENERIC, array, &contour_header, &block ));
}
if( CV_IS_STORAGE( hull_storage ))
{
if( return_points )
{
CV_CALL( hullseq = cvCreateSeq(
CV_SEQ_KIND_CURVE|CV_SEQ_ELTYPE(ptseq)|
CV_SEQ_FLAG_CLOSED|CV_SEQ_FLAG_CONVEX,
sizeof(CvContour), sizeof(CvPoint),(CvMemStorage*)hull_storage ));
}
else
{
CV_CALL( hullseq = cvCreateSeq(
CV_SEQ_KIND_CURVE|CV_SEQ_ELTYPE_PPOINT|
CV_SEQ_FLAG_CLOSED|CV_SEQ_FLAG_CONVEX,
sizeof(CvContour), sizeof(CvPoint*), (CvMemStorage*)hull_storage ));
}
}
else
{
if( !CV_IS_MAT( hull_storage ))
CV_ERROR(CV_StsBadArg, "Destination must be valid memory storage or matrix");
mat = (CvMat*)hull_storage;
if( mat->cols != 1 && mat->rows != 1 || !CV_IS_MAT_CONT(mat->type))
CV_ERROR( CV_StsBadArg,
"The hull matrix should be continuous and have a single row or a single column" );
if( mat->cols + mat->rows - 1 < ptseq->total )
CV_ERROR( CV_StsBadSize, "The hull matrix size might be not enough to fit the hull" );
if( CV_MAT_TYPE(mat->type) != CV_SEQ_ELTYPE(ptseq) &&
CV_MAT_TYPE(mat->type) != CV_32SC1 )
CV_ERROR( CV_StsUnsupportedFormat,
"The hull matrix must have the same type as input or 32sC1 (integers)" );
CV_CALL( hullseq = cvMakeSeqHeaderForArray(
CV_SEQ_KIND_CURVE|CV_MAT_TYPE(mat->type)|CV_SEQ_FLAG_CLOSED,
sizeof(contour_header), CV_ELEM_SIZE(mat->type), mat->data.ptr,
mat->cols + mat->rows - 1, &hull_header.s, &hullblock ));
cvClearSeq( hullseq );
}
total = ptseq->total;
if( total == 0 )
{
if( mat )
CV_ERROR( CV_StsBadSize,
"Point sequence can not be empty if the output is matrix" );
EXIT;
}
cvStartAppendToSeq( hullseq, &writer );
is_float = CV_SEQ_ELTYPE(ptseq) == CV_32FC2;
hulltype = CV_SEQ_ELTYPE(hullseq);
sklansky = !is_float ? (sklansky_func)icvSklansky_32s :
(sklansky_func)icvSklansky_32f;
CV_CALL( pointer = (CvPoint**)cvAlloc( ptseq->total*sizeof(pointer[0]) ));
CV_CALL( stack = (int*)cvAlloc( (ptseq->total + 2)*sizeof(stack[0]) ));
pointerf = (CvPoint2D32f**)pointer;
cvStartReadSeq( ptseq, &reader );
for( i = 0; i < total; i++ )
{
pointer[i] = (CvPoint*)reader.ptr;
CV_NEXT_SEQ_ELEM( ptseq->elem_size, reader );
}
// sort the point set by x-coordinate, find min and max y
if( !is_float )
{
icvSortPointsByPointers_32s( pointer, total, 0 );
for( i = 1; i < total; i++ )
{
int y = pointer[i]->y;
if( pointer[miny_ind]->y > y )
miny_ind = i;
if( pointer[maxy_ind]->y < y )
maxy_ind = i;
}
}
else
{
icvSortPointsByPointers_32f( pointerf, total, 0 );
for( i = 1; i < total; i++ )
{
float y = pointerf[i]->y;
if( pointerf[miny_ind]->y > y )
miny_ind = i;
if( pointerf[maxy_ind]->y < y )
maxy_ind = i;
}
}
if( pointer[0]->x == pointer[total-1]->x &&
pointer[0]->y == pointer[total-1]->y )
{
if( hulltype == CV_SEQ_ELTYPE_PPOINT )
{
CV_WRITE_SEQ_ELEM( pointer[0], writer );
}
else if( hulltype == CV_SEQ_ELTYPE_INDEX )
{
int index = 0;
CV_WRITE_SEQ_ELEM( index, writer );
}
else
{
CvPoint pt = pointer[0][0];
CV_WRITE_SEQ_ELEM( pt, writer );
}
goto finish_hull;
}
/*upper half */
{
int *tl_stack = stack;
int tl_count = sklansky( pointer, 0, maxy_ind, tl_stack, -1, 1 );
int *tr_stack = tl_stack + tl_count;
int tr_count = sklansky( pointer, ptseq->total - 1, maxy_ind, tr_stack, -1, -1 );
/* gather upper part of convex hull to output */
if( orientation == CV_COUNTER_CLOCKWISE )
{
CV_SWAP( tl_stack, tr_stack, t_stack );
CV_SWAP( tl_count, tr_count, t_count );
}
if( hulltype == CV_SEQ_ELTYPE_PPOINT )
{
for( i = 0; i < tl_count - 1; i++ )
CV_WRITE_SEQ_ELEM( pointer[tl_stack[i]], writer );
for( i = tr_count - 1; i > 0; i-- )
CV_WRITE_SEQ_ELEM( pointer[tr_stack[i]], writer );
}
else if( hulltype == CV_SEQ_ELTYPE_INDEX )
{
CV_CALL( icvCalcAndWritePtIndices( pointer, tl_stack,
0, tl_count-1, ptseq, &writer ));
CV_CALL( icvCalcAndWritePtIndices( pointer, tr_stack,
tr_count-1, 0, ptseq, &writer ));
}
else
{
for( i = 0; i < tl_count - 1; i++ )
CV_WRITE_SEQ_ELEM( pointer[tl_stack[i]][0], writer );
for( i = tr_count - 1; i > 0; i-- )
CV_WRITE_SEQ_ELEM( pointer[tr_stack[i]][0], writer );
}
stop_idx = tr_count > 2 ? tr_stack[1] : tl_count > 2 ? tl_stack[tl_count - 2] : -1;
}
/* lower half */
{
int *bl_stack = stack;
int bl_count = sklansky( pointer, 0, miny_ind, bl_stack, 1, -1 );
int *br_stack = stack + bl_count;
int br_count = sklansky( pointer, ptseq->total - 1, miny_ind, br_stack, 1, 1 );
if( orientation != CV_COUNTER_CLOCKWISE )
{
CV_SWAP( bl_stack, br_stack, t_stack );
CV_SWAP( bl_count, br_count, t_count );
}
if( stop_idx >= 0 )
{
int check_idx = bl_count > 2 ? bl_stack[1] :
bl_count + br_count > 2 ? br_stack[2-bl_count] : -1;
if( check_idx == stop_idx || check_idx >= 0 &&
pointer[check_idx]->x == pointer[stop_idx]->x &&
pointer[check_idx]->y == pointer[stop_idx]->y )
{
/* if all the points lie on the same line, then
the bottom part of the convex hull is the mirrored top part
(except the exteme points).*/
bl_count = MIN( bl_count, 2 );
br_count = MIN( br_count, 2 );
}
}
if( hulltype == CV_SEQ_ELTYPE_PPOINT )
{
for( i = 0; i < bl_count - 1; i++ )
CV_WRITE_SEQ_ELEM( pointer[bl_stack[i]], writer );
for( i = br_count - 1; i > 0; i-- )
CV_WRITE_SEQ_ELEM( pointer[br_stack[i]], writer );
}
else if( hulltype == CV_SEQ_ELTYPE_INDEX )
{
CV_CALL( icvCalcAndWritePtIndices( pointer, bl_stack,
0, bl_count-1, ptseq, &writer ));
CV_CALL( icvCalcAndWritePtIndices( pointer, br_stack,
br_count-1, 0, ptseq, &writer ));
}
else
{
for( i = 0; i < bl_count - 1; i++ )
CV_WRITE_SEQ_ELEM( pointer[bl_stack[i]][0], writer );
for( i = br_count - 1; i > 0; i-- )
CV_WRITE_SEQ_ELEM( pointer[br_stack[i]][0], writer );
}
}
finish_hull:
CV_CALL( cvEndWriteSeq( &writer ));
if( mat )
{
if( mat->rows > mat->cols )
mat->rows = hullseq->total;
else
mat->cols = hullseq->total;
}
else
{
hull.s = hullseq;
hull.c->rect = cvBoundingRect( ptseq,
ptseq->header_size < (int)sizeof(CvContour) ||
&ptseq->flags == &contour_header.flags );
/*if( ptseq != (CvSeq*)&contour_header )
hullseq->v_prev = ptseq;*/
}
__END__;
cvFree( &pointer );
cvFree( &stack );
return hull.s;
}
CV_IMPL CvSeq*
cvConvexHull2( const CvArr* array, void* hull_storage,
int orientation, int return_points )
{
CvMat* mat = 0;
CvContour contour_header;
CvSeq hull_header;
CvSeqBlock block, hullblock;
CvSeq* ptseq = 0;
CvSeq* hullseq = 0;
if( CV_IS_SEQ( array ))
{
ptseq = (CvSeq*)array;
if( !CV_IS_SEQ_POINT_SET( ptseq ))
CV_Error( CV_StsBadArg, "Unsupported sequence type" );
if( hull_storage == 0 )
hull_storage = ptseq->storage;
}
else
{
ptseq = cvPointSeqFromMat( CV_SEQ_KIND_GENERIC, array, &contour_header, &block );
}
bool isStorage = isStorageOrMat(hull_storage);
if(isStorage)
{
if( return_points )
{
hullseq = cvCreateSeq(CV_SEQ_KIND_CURVE|CV_SEQ_ELTYPE(ptseq)|
CV_SEQ_FLAG_CLOSED|CV_SEQ_FLAG_CONVEX,
sizeof(CvContour), sizeof(CvPoint),(CvMemStorage*)hull_storage );
}
else
{
hullseq = cvCreateSeq(
CV_SEQ_KIND_CURVE|CV_SEQ_ELTYPE_PPOINT|
CV_SEQ_FLAG_CLOSED|CV_SEQ_FLAG_CONVEX,
sizeof(CvContour), sizeof(CvPoint*), (CvMemStorage*)hull_storage );
}
}
else
{
mat = (CvMat*)hull_storage;
if( (mat->cols != 1 && mat->rows != 1) || !CV_IS_MAT_CONT(mat->type))
CV_Error( CV_StsBadArg,
"The hull matrix should be continuous and have a single row or a single column" );
if( mat->cols + mat->rows - 1 < ptseq->total )
CV_Error( CV_StsBadSize, "The hull matrix size might be not enough to fit the hull" );
if( CV_MAT_TYPE(mat->type) != CV_SEQ_ELTYPE(ptseq) &&
CV_MAT_TYPE(mat->type) != CV_32SC1 )
CV_Error( CV_StsUnsupportedFormat,
"The hull matrix must have the same type as input or 32sC1 (integers)" );
hullseq = cvMakeSeqHeaderForArray(
CV_SEQ_KIND_CURVE|CV_MAT_TYPE(mat->type)|CV_SEQ_FLAG_CLOSED,
sizeof(hull_header), CV_ELEM_SIZE(mat->type), mat->data.ptr,
mat->cols + mat->rows - 1, &hull_header, &hullblock );
cvClearSeq( hullseq );
}
int hulltype = CV_SEQ_ELTYPE(hullseq);
int total = ptseq->total;
if( total == 0 )
{
if( !isStorage )
CV_Error( CV_StsBadSize,
"Point sequence can not be empty if the output is matrix" );
return 0;
}
cv::AutoBuffer<double> _ptbuf;
cv::Mat h0;
cv::convexHull(cv::cvarrToMat(ptseq, false, false, 0, &_ptbuf), h0,
orientation == CV_CLOCKWISE, CV_MAT_CN(hulltype) == 2);
if( hulltype == CV_SEQ_ELTYPE_PPOINT )
{
const int* idx = h0.ptr<int>();
int ctotal = (int)h0.total();
for( int i = 0; i < ctotal; i++ )
{
void* ptr = cvGetSeqElem(ptseq, idx[i]);
cvSeqPush( hullseq, &ptr );
}
}
else
cvSeqPushMulti(hullseq, h0.ptr(), (int)h0.total());
if (isStorage)
{
return hullseq;
}
else
{
if( mat->rows > mat->cols )
mat->rows = hullseq->total;
else
mat->cols = hullseq->total;
return 0;
}
}
/* Calculates bounding rectagnle of a point set or retrieves already calculated */
CV_IMPL CvRect
cvBoundingRect( CvArr* array, int update )
{
CvSeqReader reader;
CvRect rect = { 0, 0, 0, 0 };
CvContour contour_header;
CvSeq* ptseq = 0;
CvSeqBlock block;
CvMat stub, *mat = 0;
int xmin = 0, ymin = 0, xmax = -1, ymax = -1, i, j, k;
int calculate = update;
if( CV_IS_SEQ( array ))
{
ptseq = (CvSeq*)array;
if( !CV_IS_SEQ_POINT_SET( ptseq ))
CV_Error( CV_StsBadArg, "Unsupported sequence type" );
if( ptseq->header_size < (int)sizeof(CvContour))
{
update = 0;
calculate = 1;
}
}
else
{
mat = cvGetMat( array, &stub );
if( CV_MAT_TYPE(mat->type) == CV_32SC2 ||
CV_MAT_TYPE(mat->type) == CV_32FC2 )
{
ptseq = cvPointSeqFromMat(CV_SEQ_KIND_GENERIC, mat, &contour_header, &block);
mat = 0;
}
else if( CV_MAT_TYPE(mat->type) != CV_8UC1 &&
CV_MAT_TYPE(mat->type) != CV_8SC1 )
CV_Error( CV_StsUnsupportedFormat,
"The image/matrix format is not supported by the function" );
update = 0;
calculate = 1;
}
if( !calculate )
return ((CvContour*)ptseq)->rect;
if( mat )
{
CvSize size = cvGetMatSize(mat);
xmin = size.width;
ymin = -1;
for( i = 0; i < size.height; i++ )
{
uchar* _ptr = mat->data.ptr + i*mat->step;
uchar* ptr = (uchar*)cvAlignPtr(_ptr, 4);
int have_nz = 0, k_min, offset = (int)(ptr - _ptr);
j = 0;
offset = MIN(offset, size.width);
for( ; j < offset; j++ )
if( _ptr[j] )
{
have_nz = 1;
break;
}
if( j < offset )
{
if( j < xmin )
xmin = j;
if( j > xmax )
xmax = j;
}
if( offset < size.width )
{
xmin -= offset;
xmax -= offset;
size.width -= offset;
j = 0;
for( ; j <= xmin - 4; j += 4 )
if( *((int*)(ptr+j)) )
break;
for( ; j < xmin; j++ )
if( ptr[j] )
{
xmin = j;
if( j > xmax )
xmax = j;
have_nz = 1;
break;
}
k_min = MAX(j-1, xmax);
k = size.width - 1;
for( ; k > k_min && (k&3) != 3; k-- )
if( ptr[k] )
break;
if( k > k_min && (k&3) == 3 )
{
for( ; k > k_min+3; k -= 4 )
if( *((int*)(ptr+k-3)) )
break;
}
for( ; k > k_min; k-- )
if( ptr[k] )
{
xmax = k;
have_nz = 1;
break;
}
if( !have_nz )
{
j &= ~3;
for( ; j <= k - 3; j += 4 )
if( *((int*)(ptr+j)) )
break;
for( ; j <= k; j++ )
if( ptr[j] )
{
have_nz = 1;
break;
}
}
xmin += offset;
xmax += offset;
size.width += offset;
}
if( have_nz )
{
if( ymin < 0 )
ymin = i;
ymax = i;
}
}
if( xmin >= size.width )
xmin = ymin = 0;
}
else if( ptseq->total )
{
int is_float = CV_SEQ_ELTYPE(ptseq) == CV_32FC2;
cvStartReadSeq( ptseq, &reader, 0 );
CvPoint pt;
CV_READ_SEQ_ELEM( pt, reader );
#if CV_SSE4_2
if(cv::checkHardwareSupport(CV_CPU_SSE4_2))
{
if( !is_float )
{
__m128i minval, maxval;
minval = maxval = _mm_loadl_epi64((const __m128i*)(&pt)); //min[0]=pt.x, min[1]=pt.y
for( i = 1; i < ptseq->total; i++)
{
__m128i ptXY = _mm_loadl_epi64((const __m128i*)(reader.ptr));
CV_NEXT_SEQ_ELEM(sizeof(pt), reader);
minval = _mm_min_epi32(ptXY, minval);
maxval = _mm_max_epi32(ptXY, maxval);
}
xmin = _mm_cvtsi128_si32(minval);
ymin = _mm_cvtsi128_si32(_mm_srli_si128(minval, 4));
xmax = _mm_cvtsi128_si32(maxval);
ymax = _mm_cvtsi128_si32(_mm_srli_si128(maxval, 4));
}
else
{
__m128 minvalf, maxvalf, z = _mm_setzero_ps(), ptXY = _mm_setzero_ps();
minvalf = maxvalf = _mm_loadl_pi(z, (const __m64*)(&pt));
for( i = 1; i < ptseq->total; i++ )
{
ptXY = _mm_loadl_pi(ptXY, (const __m64*)reader.ptr);
CV_NEXT_SEQ_ELEM(sizeof(pt), reader);
minvalf = _mm_min_ps(minvalf, ptXY);
maxvalf = _mm_max_ps(maxvalf, ptXY);
}
float xyminf[2], xymaxf[2];
_mm_storel_pi((__m64*)xyminf, minvalf);
_mm_storel_pi((__m64*)xymaxf, maxvalf);
xmin = cvFloor(xyminf[0]);
ymin = cvFloor(xyminf[1]);
xmax = cvFloor(xymaxf[0]);
ymax = cvFloor(xymaxf[1]);
}
}
else
#endif
{
if( !is_float )
{
xmin = xmax = pt.x;
ymin = ymax = pt.y;
for( i = 1; i < ptseq->total; i++ )
{
CV_READ_SEQ_ELEM( pt, reader );
if( xmin > pt.x )
xmin = pt.x;
if( xmax < pt.x )
xmax = pt.x;
if( ymin > pt.y )
ymin = pt.y;
if( ymax < pt.y )
ymax = pt.y;
}
}
else
{
Cv32suf v;
// init values
xmin = xmax = CV_TOGGLE_FLT(pt.x);
ymin = ymax = CV_TOGGLE_FLT(pt.y);
for( i = 1; i < ptseq->total; i++ )
{
CV_READ_SEQ_ELEM( pt, reader );
pt.x = CV_TOGGLE_FLT(pt.x);
pt.y = CV_TOGGLE_FLT(pt.y);
if( xmin > pt.x )
xmin = pt.x;
if( xmax < pt.x )
xmax = pt.x;
if( ymin > pt.y )
ymin = pt.y;
if( ymax < pt.y )
ymax = pt.y;
}
v.i = CV_TOGGLE_FLT(xmin); xmin = cvFloor(v.f);
v.i = CV_TOGGLE_FLT(ymin); ymin = cvFloor(v.f);
// because right and bottom sides of the bounding rectangle are not inclusive
// (note +1 in width and height calculation below), cvFloor is used here instead of cvCeil
v.i = CV_TOGGLE_FLT(xmax); xmax = cvFloor(v.f);
v.i = CV_TOGGLE_FLT(ymax); ymax = cvFloor(v.f);
}
}
rect.x = xmin;
rect.y = ymin;
rect.width = xmax - xmin + 1;
rect.height = ymax - ymin + 1;
}
if( update )
((CvContour*)ptseq)->rect = rect;
return rect;
}
CV_IMPL int
cvMinEnclosingCircle( const void* array, CvPoint2D32f * _center, float *_radius )
{
const int max_iters = 100;
const float eps = FLT_EPSILON*2;
CvPoint2D32f center = { 0, 0 };
float radius = 0;
int result = 0;
if( _center )
_center->x = _center->y = 0.f;
if( _radius )
*_radius = 0;
CvSeqReader reader;
int k, count;
CvPoint2D32f pts[8];
CvContour contour_header;
CvSeqBlock block;
CvSeq* sequence = 0;
int is_float;
if( !_center || !_radius )
CV_Error( CV_StsNullPtr, "Null center or radius pointers" );
if( CV_IS_SEQ(array) )
{
sequence = (CvSeq*)array;
if( !CV_IS_SEQ_POINT_SET( sequence ))
CV_Error( CV_StsBadArg, "The passed sequence is not a valid contour" );
}
else
{
sequence = cvPointSeqFromMat(
CV_SEQ_KIND_GENERIC, array, &contour_header, &block );
}
if( sequence->total <= 0 )
CV_Error( CV_StsBadSize, "" );
cvStartReadSeq( sequence, &reader, 0 );
count = sequence->total;
is_float = CV_SEQ_ELTYPE(sequence) == CV_32FC2;
if( !is_float )
{
CvPoint *pt_left, *pt_right, *pt_top, *pt_bottom;
CvPoint pt;
pt_left = pt_right = pt_top = pt_bottom = (CvPoint *)(reader.ptr);
CV_READ_SEQ_ELEM( pt, reader );
for(int i = 1; i < count; i++ )
{
CvPoint* pt_ptr = (CvPoint*)reader.ptr;
CV_READ_SEQ_ELEM( pt, reader );
if( pt.x < pt_left->x )
pt_left = pt_ptr;
if( pt.x > pt_right->x )
pt_right = pt_ptr;
if( pt.y < pt_top->y )
pt_top = pt_ptr;
if( pt.y > pt_bottom->y )
pt_bottom = pt_ptr;
}
pts[0] = cvPointTo32f( *pt_left );
pts[1] = cvPointTo32f( *pt_right );
pts[2] = cvPointTo32f( *pt_top );
pts[3] = cvPointTo32f( *pt_bottom );
}
else
{
CvPoint2D32f *pt_left, *pt_right, *pt_top, *pt_bottom;
CvPoint2D32f pt;
pt_left = pt_right = pt_top = pt_bottom = (CvPoint2D32f *) (reader.ptr);
CV_READ_SEQ_ELEM( pt, reader );
for(int i = 1; i < count; i++ )
{
CvPoint2D32f* pt_ptr = (CvPoint2D32f*)reader.ptr;
CV_READ_SEQ_ELEM( pt, reader );
if( pt.x < pt_left->x )
pt_left = pt_ptr;
if( pt.x > pt_right->x )
pt_right = pt_ptr;
if( pt.y < pt_top->y )
pt_top = pt_ptr;
if( pt.y > pt_bottom->y )
pt_bottom = pt_ptr;
}
pts[0] = *pt_left;
pts[1] = *pt_right;
pts[2] = *pt_top;
pts[3] = *pt_bottom;
}
for( k = 0; k < max_iters; k++ )
{
double min_delta = 0, delta;
CvPoint2D32f ptfl, farAway = { 0, 0};
/*only for first iteration because the alg is repared at the loop's foot*/
if(k==0)
icvFindEnslosingCicle4pts_32f( pts, ¢er, &radius );
cvStartReadSeq( sequence, &reader, 0 );
for(int i = 0; i < count; i++ )
{
if( !is_float )
{
ptfl.x = (float)((CvPoint*)reader.ptr)->x;
ptfl.y = (float)((CvPoint*)reader.ptr)->y;
}
else
{
ptfl = *(CvPoint2D32f*)reader.ptr;
}
CV_NEXT_SEQ_ELEM( sequence->elem_size, reader );
delta = icvIsPtInCircle( ptfl, center, radius );
if( delta < min_delta )
{
min_delta = delta;
farAway = ptfl;
}
}
result = min_delta >= 0;
if( result )
break;
CvPoint2D32f ptsCopy[4];
/* find good replacement partner for the point which is at most far away,
starting with the one that lays in the actual circle (i=3) */
for(int i = 3; i >=0; i-- )
{
for(int j = 0; j < 4; j++ )
{
ptsCopy[j]=(i != j)? pts[j]: farAway;
}
icvFindEnslosingCicle4pts_32f(ptsCopy, ¢er, &radius );
if( icvIsPtInCircle( pts[i], center, radius )>=0){ // replaced one again in the new circle?
pts[i] = farAway;
break;
}
}
}
if( !result )
{
cvStartReadSeq( sequence, &reader, 0 );
radius = 0.f;
for(int i = 0; i < count; i++ )
{
CvPoint2D32f ptfl;
float t, dx, dy;
if( !is_float )
{
ptfl.x = (float)((CvPoint*)reader.ptr)->x;
ptfl.y = (float)((CvPoint*)reader.ptr)->y;
}
else
{
ptfl = *(CvPoint2D32f*)reader.ptr;
}
CV_NEXT_SEQ_ELEM( sequence->elem_size, reader );
dx = center.x - ptfl.x;
dy = center.y - ptfl.y;
t = dx*dx + dy*dy;
radius = MAX(radius,t);
}
radius = (float)(sqrt(radius)*(1 + eps));
result = 1;
}
*_center = center;
*_radius = radius;
return result;
}
/* Calculates bounding rectagnle of a point set or retrieves already calculated */
CV_IMPL CvRect
cvBoundingRect( CvArr* array, int update )
{
CvSeqReader reader;
CvRect rect = { 0, 0, 0, 0 };
CvContour contour_header;
CvSeq* ptseq = 0;
CvSeqBlock block;
CV_FUNCNAME( "cvBoundingRect" );
__BEGIN__;
int calculate = update;
if( CV_IS_SEQ( array ))
{
ptseq = (CvSeq*)array;
if( !CV_IS_SEQ_POINT_SET( ptseq ))
CV_ERROR( CV_StsBadArg, "Unsupported sequence type" );
if( ptseq->header_size < (int)sizeof(CvContour))
{
if( update == 1 )
CV_ERROR( CV_StsBadArg, "The header is too small to fit the rectangle, "
"so it could not be updated" );
calculate = 1;
}
}
else
{
CV_CALL( ptseq = cvPointSeqFromMat(
CV_SEQ_KIND_GENERIC, array, &contour_header, &block ));
calculate = 1;
}
if( calculate )
{
if( ptseq->total )
{
int is_float = CV_SEQ_ELTYPE(ptseq) == CV_32FC2;
int xmin, ymin, xmax, ymax, i;
cvStartReadSeq( ptseq, &reader, 0 );
if( !is_float )
{
CvPoint pt;
/* init values */
CV_READ_SEQ_ELEM( pt, reader );
xmin = xmax = pt.x;
ymin = ymax = pt.y;
for( i = 1; i < ptseq->total; i++ )
{
CV_READ_SEQ_ELEM( pt, reader );
if( xmin > pt.x )
xmin = pt.x;
if( xmax < pt.x )
xmax = pt.x;
if( ymin > pt.y )
ymin = pt.y;
if( ymax < pt.y )
ymax = pt.y;
}
}
else
{
CvPoint pt;
/* init values */
CV_READ_SEQ_ELEM( pt, reader );
xmin = xmax = CV_TOGGLE_FLT(pt.x);
ymin = ymax = CV_TOGGLE_FLT(pt.y);
for( i = 1; i < ptseq->total; i++ )
{
CV_READ_SEQ_ELEM( pt, reader );
pt.x = CV_TOGGLE_FLT(pt.x);
pt.y = CV_TOGGLE_FLT(pt.y);
if( xmin > pt.x )
xmin = pt.x;
if( xmax < pt.x )
xmax = pt.x;
if( ymin > pt.y )
ymin = pt.y;
if( ymax < pt.y )
ymax = pt.y;
}
xmin = CV_TOGGLE_FLT(xmin);
ymin = CV_TOGGLE_FLT(ymin);
xmax = CV_TOGGLE_FLT(xmax);
ymax = CV_TOGGLE_FLT(ymax);
xmin = cvFloor( (float&)xmin );
ymin = cvFloor( (float&)ymin );
/* because right and bottom sides of
the bounding rectangle are not inclusive,
cvFloor is used here (instead of cvCeil) */
xmax = cvFloor( (float&)xmax );
ymax = cvFloor( (float&)ymax );
}
rect.x = xmin;
rect.y = ymin;
rect.width = xmax - xmin + 1;
rect.height = ymax - ymin + 1;
}
if( update )
((CvContour*)ptseq)->rect = rect;
}
else
{
rect = ((CvContour*)ptseq)->rect;
}
__END__;
return rect;
}
CV_IMPL CvBox2D
cvMinAreaRect2( const CvArr* array, CvMemStorage* storage )
{
CvMemStorage* temp_storage = 0;
CvBox2D box;
CvPoint2D32f* points = 0;
CV_FUNCNAME( "cvMinAreaRect2" );
memset(&box, 0, sizeof(box));
__BEGIN__;
int i, n;
CvSeqReader reader;
CvContour contour_header;
CvSeqBlock block;
CvSeq* ptseq = (CvSeq*)array;
CvPoint2D32f out[3];
if( CV_IS_SEQ(ptseq) )
{
if( !CV_IS_SEQ_POINT_SET(ptseq) &&
(CV_SEQ_KIND(ptseq) != CV_SEQ_KIND_CURVE || !CV_IS_SEQ_CONVEX(ptseq) ||
CV_SEQ_ELTYPE(ptseq) != CV_SEQ_ELTYPE_PPOINT ))
CV_ERROR( CV_StsUnsupportedFormat,
"Input sequence must consist of 2d points or pointers to 2d points" );
if( !storage )
storage = ptseq->storage;
}
else
{
CV_CALL( ptseq = cvPointSeqFromMat(
CV_SEQ_KIND_GENERIC, array, &contour_header, &block ));
}
if( storage )
{
CV_CALL( temp_storage = cvCreateChildMemStorage( storage ));
}
else
{
CV_CALL( temp_storage = cvCreateMemStorage(1 << 10));
}
if( !CV_IS_SEQ_CONVEX( ptseq ))
{
CV_CALL( ptseq = cvConvexHull2( ptseq, temp_storage, CV_CLOCKWISE, 1 ));
}
else if( !CV_IS_SEQ_POINT_SET( ptseq ))
{
CvSeqWriter writer;
if( !CV_IS_SEQ(ptseq->v_prev) || !CV_IS_SEQ_POINT_SET(ptseq->v_prev))
CV_ERROR( CV_StsBadArg,
"Convex hull must have valid pointer to point sequence stored in v_prev" );
cvStartReadSeq( ptseq, &reader );
cvStartWriteSeq( CV_SEQ_KIND_CURVE|CV_SEQ_FLAG_CONVEX|CV_SEQ_ELTYPE(ptseq->v_prev),
sizeof(CvContour), CV_ELEM_SIZE(ptseq->v_prev->flags),
temp_storage, &writer );
for( i = 0; i < ptseq->total; i++ )
{
CvPoint pt = **(CvPoint**)(reader.ptr);
CV_WRITE_SEQ_ELEM( pt, writer );
}
ptseq = cvEndWriteSeq( &writer );
}
n = ptseq->total;
CV_CALL( points = (CvPoint2D32f*)cvAlloc( n*sizeof(points[0]) ));
cvStartReadSeq( ptseq, &reader );
if( CV_SEQ_ELTYPE( ptseq ) == CV_32SC2 )
{
for( i = 0; i < n; i++ )
{
CvPoint pt;
CV_READ_SEQ_ELEM( pt, reader );
points[i].x = (float)pt.x;
points[i].y = (float)pt.y;
}
}
else
{
for( i = 0; i < n; i++ )
{
CV_READ_SEQ_ELEM( points[i], reader );
}
}
if( n > 2 )
{
icvRotatingCalipers( points, n, CV_CALIPERS_MINAREARECT, (float*)out );
box.center.x = out[0].x + (out[1].x + out[2].x)*0.5f;
box.center.y = out[0].y + (out[1].y + out[2].y)*0.5f;
box.size.height = (float)sqrt((double)out[1].x*out[1].x + (double)out[1].y*out[1].y);
box.size.width = (float)sqrt((double)out[2].x*out[2].x + (double)out[2].y*out[2].y);
box.angle = (float)atan2( -(double)out[1].y, (double)out[1].x );
}
else if( n == 2 )
{
box.center.x = (points[0].x + points[1].x)*0.5f;
box.center.y = (points[0].y + points[1].y)*0.5f;
double dx = points[1].x - points[0].x;
double dy = points[1].y - points[0].y;
box.size.height = (float)sqrt(dx*dx + dy*dy);
box.size.width = 0;
box.angle = (float)atan2( -dy, dx );
}
else
{
if( n == 1 )
box.center = points[0];
}
box.angle = (float)(box.angle*180/CV_PI);
__END__;
cvReleaseMemStorage( &temp_storage );
cvFree( &points );
return box;
}
/* it must have more than 3 points */
CV_IMPL CvSeq*
cvConvexityDefects( const CvArr* array,
const CvArr* hullarray,
CvMemStorage* storage )
{
CvSeq* defects = 0;
CV_FUNCNAME( "cvConvexityDefects" );
__BEGIN__;
int i, index;
CvPoint* hull_cur;
/* is orientation of hull different from contour one */
int rev_orientation;
CvContour contour_header;
union { CvContour c; CvSeq s; } hull_header;
CvSeqBlock block, hullblock;
CvSeq *ptseq = (CvSeq*)array, *hull = (CvSeq*)hullarray;
CvSeqReader hull_reader;
CvSeqReader ptseq_reader;
CvSeqWriter writer;
int is_index;
if( CV_IS_SEQ( ptseq ))
{
if( !CV_IS_SEQ_POINT_SET( ptseq ))
CV_ERROR( CV_StsUnsupportedFormat,
"Input sequence is not a sequence of points" );
if( !storage )
storage = ptseq->storage;
}
else
{
CV_CALL( ptseq = cvPointSeqFromMat(
CV_SEQ_KIND_GENERIC, array, &contour_header, &block ));
}
if( CV_SEQ_ELTYPE( ptseq ) != CV_32SC2 )
CV_ERROR( CV_StsUnsupportedFormat,
"Floating-point coordinates are not supported here" );
if( CV_IS_SEQ( hull ))
{
int hulltype = CV_SEQ_ELTYPE( hull );
if( hulltype != CV_SEQ_ELTYPE_PPOINT && hulltype != CV_SEQ_ELTYPE_INDEX )
CV_ERROR( CV_StsUnsupportedFormat,
"Convex hull must represented as a sequence "
"of indices or sequence of pointers" );
if( !storage )
storage = hull->storage;
}
else
{
CvMat* mat = (CvMat*)hull;
if( !CV_IS_MAT( hull ))
CV_ERROR(CV_StsBadArg, "Convex hull is neither sequence nor matrix");
if( mat->cols != 1 && mat->rows != 1 ||
!CV_IS_MAT_CONT(mat->type) || CV_MAT_TYPE(mat->type) != CV_32SC1 )
CV_ERROR( CV_StsBadArg,
"The matrix should be 1-dimensional and continuous array of int's" );
if( mat->cols + mat->rows - 1 > ptseq->total )
CV_ERROR( CV_StsBadSize, "Convex hull is larger than the point sequence" );
CV_CALL( hull = cvMakeSeqHeaderForArray(
CV_SEQ_KIND_CURVE|CV_MAT_TYPE(mat->type)|CV_SEQ_FLAG_CLOSED,
sizeof(CvContour), CV_ELEM_SIZE(mat->type), mat->data.ptr,
mat->cols + mat->rows - 1, &hull_header.s, &hullblock ));
}
is_index = CV_SEQ_ELTYPE(hull) == CV_SEQ_ELTYPE_INDEX;
if( !storage )
CV_ERROR( CV_StsNullPtr, "NULL storage pointer" );
CV_CALL( defects = cvCreateSeq( CV_SEQ_KIND_GENERIC, sizeof(CvSeq),
sizeof(CvConvexityDefect), storage ));
if( ptseq->total < 4 || hull->total < 3)
{
//CV_ERROR( CV_StsBadSize,
// "point seq size must be >= 4, convex hull size must be >= 3" );
EXIT;
}
/* recognize co-orientation of ptseq and its hull */
{
int sign = 0;
int index1, index2, index3;
if( !is_index )
{
CvPoint* pos = *CV_SEQ_ELEM( hull, CvPoint*, 0 );
CV_CALL( index1 = cvSeqElemIdx( ptseq, pos ));
pos = *CV_SEQ_ELEM( hull, CvPoint*, 1 );
CV_CALL( index2 = cvSeqElemIdx( ptseq, pos ));
pos = *CV_SEQ_ELEM( hull, CvPoint*, 2 );
CV_CALL( index3 = cvSeqElemIdx( ptseq, pos ));
}
else
{
int main(int argc, char * argv[])
{
if(argc < 2) {
fprintf(stderr, "%s image1 image2\n", argv[0]);
return 1;
}
char * im1fname = argv[1];
char * im2fname = argv[2];
IplImage * image1 = cvLoadImage(im1fname, CV_LOAD_IMAGE_GRAYSCALE);
IplImage * eigenvalues = cvCreateImage(cvGetSize(image1), 32, 1);
IplImage * temp = cvCreateImage(cvGetSize(image1), 32, 1);
int count = MAX_COUNT;
double quality = 0.5;
// double min_distance = 2;
double min_distance = 50;
int block_size = 7;
int use_harris = 0;
int win_size = 10;
int flags = 0;
CvPoint2D32f * source_points = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(CvPoint2D32f));
CvPoint2D32f * dest_points = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(CvPoint2D32f));
CvPoint2D32f * delaunay_points = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(CvPoint2D32f));
cvGoodFeaturesToTrack( image1, eigenvalues, temp, source_points, &count,
quality, min_distance, 0, block_size, use_harris, 0.04 );
printf("%d features\n",count);
setbuf(stdout, NULL);
printf("Finding corner subpix...");
cvFindCornerSubPix( image1, source_points, count,
cvSize(win_size,win_size), cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03));
printf("done.\n");
cvReleaseImage(&eigenvalues);
cvReleaseImage(&temp);
IplImage * image2 = cvLoadImage(im2fname, CV_LOAD_IMAGE_GRAYSCALE);
char * status = (char*)cvAlloc(sizeof(char)*MAX_COUNT);
IplImage * pyramid = cvCreateImage( cvGetSize(image1), IPL_DEPTH_8U, 1 );
IplImage * second_pyramid = cvCreateImage( cvGetSize(image2), IPL_DEPTH_8U, 1 );
printf("Computing optical flow...");
cvCalcOpticalFlowPyrLK(image1, image2, pyramid, second_pyramid, source_points,
dest_points, count, cvSize(win_size,win_size), 4, status, 0,
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03),
flags);
printf("done.\n");
int num_matches = 0;
int num_out_matches = 0;
int max_dist = 30;
int offset = 200;
CvMemStorage * storage = cvCreateMemStorage(0);
CvSubdiv2D * delaunay = cvCreateSubdivDelaunay2D( cvRect(0,0,image1->width,image1->height), storage);
cvReleaseImage(&image1);
cvReleaseImage(&image2);
image1 = cvLoadImage(im1fname, CV_LOAD_IMAGE_COLOR);
image2 = cvLoadImage(im2fname, CV_LOAD_IMAGE_COLOR);
cvSet( image1, cvScalarAll(255) );
std::map<CvPoint, CvPoint> point_lookup_map;
std::vector<std::pair<CvPoint, CvPoint> > point_lookup;
// put corners in the point lookup as going to themselves
point_lookup_map[cvPoint(0,0)] = cvPoint(0,0);
point_lookup_map[cvPoint(0,image1->height-1)] = cvPoint(0,image1->height-1);
point_lookup_map[cvPoint(image1->width-1,0)] = cvPoint(image1->width-1,0);
point_lookup_map[cvPoint(image1->width-1,image1->height-1)] = cvPoint(image1->width-1,image1->height-1);
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(0,0), cvPoint(0,0)));
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(0,image1->height-1), cvPoint(0,image1->height-1)));
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(image1->width-1,0), cvPoint(image1->width-1,0)));
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(image1->width-1,image1->height-1), cvPoint(image1->width-1,image1->height-1)));
printf("Inserting corners...");
// put corners in the Delaunay subdivision
for(unsigned int i = 0; i < point_lookup.size(); i++) {
cvSubdivDelaunay2DInsert( delaunay, cvPointTo32f(point_lookup[i].first) );
}
printf("done.\n");
CvSubdiv2DEdge proxy_edge;
for(int i = 0; i < count; i++) {
if(status[i]) {
CvPoint source = cvPointFrom32f(source_points[i]);
CvPoint dest = cvPointFrom32f(dest_points[i]);
if((((int)fabs((double)(source.x - dest.x))) > max_dist) ||
(((int)fabs((double)(source.y - dest.y))) > max_dist)) {
num_out_matches++;
}
else if((dest.x >= 0) && (dest.y >= 0) && (dest.x < (image1->width)) && (dest.y < (image1->height))) {
if(point_lookup_map.find(source) == point_lookup_map.end()) {
num_matches++;
point_lookup_map[source] = dest;
point_lookup.push_back(std::pair<CvPoint,CvPoint>(source,dest));
delaunay_points[i] = (cvSubdivDelaunay2DInsert( delaunay, cvPointTo32f(source) ))->pt;
cvSetImageROI( image1, cvRect(source.x-8,source.y-8,8*2,8*2) );
cvResetImageROI( image2 );
cvGetRectSubPix( image2, image1, dest_points[i] );
}
/*
cvSet2D( image1, source.y, source.x, cvGet2D( image2, dest.y, dest.x ) );
cvSet2D( image1, source.y, source.x+1, cvGet2D( image2, dest.y, dest.x+1 ) );
cvSet2D( image1, source.y, source.x-1, cvGet2D( image2, dest.y, dest.x-1 ) );
cvSet2D( image1, source.y+1, source.x, cvGet2D( image2, dest.y+1, dest.x ) );
cvSet2D( image1, source.y-1, source.x, cvGet2D( image2, dest.y-1, dest.x ) );
cvSet2D( image1, source.y+1, source.x+1, cvGet2D( image2, dest.y+1, dest.x+1 ) );
cvSet2D( image1, source.y-1, source.x-1, cvGet2D( image2, dest.y-1, dest.x-1 ) );
cvSet2D( image1, source.y+1, source.x-1, cvGet2D( image2, dest.y+1, dest.x-1 ) );
cvSet2D( image1, source.y-1, source.x+1, cvGet2D( image2, dest.y-1, dest.x+1 ) );
*/
// cvCircle( image1, source, 4, CV_RGB(255,0,0), 2, CV_AA );
// cvCircle( image2, dest, 4, CV_RGB(255,0,0), 2, CV_AA );
}
/*
cvSetImageROI( image1, cvRect(source.x-offset,source.y-offset,offset*2,offset*2) );
cvSetImageROI( image2, cvRect(dest.x-offset,dest.y-offset,offset*2,offset*2) );
cvNamedWindow("image1",0);
cvNamedWindow("image2",0);
cvShowImage("image1",image1);
cvShowImage("image2",image2);
printf("%d,%d -> %d,%d\n",source.x,source.y,dest.x,dest.y);
cvWaitKey(0);
cvDestroyAllWindows();
*/
}
}
printf("%d %d\n",num_matches,num_out_matches);
printf("%d lookups\n",point_lookup_map.size());
cvResetImageROI( image1 );
cvSaveImage("sparse.jpg", image1);
cvReleaseImage(&image1);
image1 = cvLoadImage(im1fname, CV_LOAD_IMAGE_COLOR);
cvSet( image1, cvScalarAll(255) );
printf("Warping image...");
CvSeqReader reader;
int total = delaunay->edges->total;
int elem_size = delaunay->edges->elem_size;
cvStartReadSeq( (CvSeq*)(delaunay->edges), &reader, 0 );
std::vector<Triangle> trivec;
std::vector<CvMat *> baryinvvec;
for( int i = 0; i < total; i++ ) {
CvQuadEdge2D* edge = (CvQuadEdge2D*)(reader.ptr);
if( CV_IS_SET_ELEM( edge )) {
CvSubdiv2DEdge curedge = (CvSubdiv2DEdge)edge;
CvSubdiv2DEdge t = curedge;
Triangle temptri;
int count = 0;
// construct a triangle from this edge
do {
CvSubdiv2DPoint* pt = cvSubdiv2DEdgeOrg( t );
if(count < 3) {
pt->pt.x = pt->pt.x >= image1->width ? image1->width-1 : pt->pt.x;
pt->pt.y = pt->pt.y >= image1->height ? image1->height-1 : pt->pt.y;
pt->pt.x = pt->pt.x < 0 ? 0 : pt->pt.x;
pt->pt.y = pt->pt.y < 0 ? 0 : pt->pt.y;
temptri.points[count] = cvPointFrom32f( pt->pt );
}
else {
printf("More than 3 edges\n");
}
count++;
t = cvSubdiv2DGetEdge( t, CV_NEXT_AROUND_LEFT );
} while( t != curedge );
// check that triangle is not already in
if( std::find(trivec.begin(), trivec.end(), temptri) == trivec.end() ) {
// push triangle in and draw
trivec.push_back(temptri);
cvLine( image1, temptri.points[0], temptri.points[1], CV_RGB(255,0,0), 1, CV_AA, 0 );
cvLine( image1, temptri.points[1], temptri.points[2], CV_RGB(255,0,0), 1, CV_AA, 0 );
cvLine( image1, temptri.points[2], temptri.points[0], CV_RGB(255,0,0), 1, CV_AA, 0 );
// compute barycentric computation vector for this triangle
CvMat * barycen = cvCreateMat( 3, 3, CV_32FC1 );
CvMat * baryceninv = cvCreateMat( 3, 3, CV_32FC1 );
barycen->data.fl[3*0+0] = temptri.points[0].x;
barycen->data.fl[3*0+1] = temptri.points[1].x;
barycen->data.fl[3*0+2] = temptri.points[2].x;
barycen->data.fl[3*1+0] = temptri.points[0].y;
barycen->data.fl[3*1+1] = temptri.points[1].y;
barycen->data.fl[3*1+2] = temptri.points[2].y;
barycen->data.fl[3*2+0] = 1;
barycen->data.fl[3*2+1] = 1;
barycen->data.fl[3*2+2] = 1;
cvInvert( barycen, baryceninv, CV_LU );
baryinvvec.push_back(baryceninv);
cvReleaseMat( &barycen );
}
}
CV_NEXT_SEQ_ELEM( elem_size, reader );
}
printf("%d triangles...", trivec.size());
cvSaveImage("triangles.jpg", image1);
cvSet( image1, cvScalarAll(255) );
IplImage * clean_nonthresh = cvLoadImage( "conhull-clean.jpg", CV_LOAD_IMAGE_COLOR );
// for each triangle
for(unsigned int i = 0; i < trivec.size(); i++) {
Triangle curtri = trivec[i];
CvMat * curpoints = cvCreateMat( 1, 3, CV_32SC2 );
Triangle target;
std::map<CvPoint,CvPoint>::iterator piter[3];
printf("Triangle %d / %d\n",i,trivec.size());
bool is_corner = false;
for(int j = 0; j < 3; j++) {
/*
curpoints->data.i[2*j+0] = curtri.points[j].x;
curpoints->data.i[2*j+1] = curtri.points[j].y;
*/
CV_MAT_ELEM( *curpoints, CvPoint, 0, j ) = curtri.points[j];
printf("%d,%d\n",curtri.points[j].x,curtri.points[j].y);
/*
if((curtri.points[j] == cvPoint(0,0)) || (curtri.points[j] == cvPoint(0,image1->height)) ||(curtri.points[j] == cvPoint(image1->width,0)) ||(curtri.points[j] == cvPoint(image1->width,image1->height))) {
is_corner = true;
break;
}
*/
for(unsigned int k = 0; k < point_lookup.size(); k++) {
std::pair<CvPoint,CvPoint> thispair = point_lookup[k];
if(thispair.first == curtri.points[j]) {
target.points[j] = thispair.second;
break;
}
}
/*
piter[j] = point_lookup_map.find(curtri.points[j]);
if(piter[j] != point_lookup_map.end() ) {
target.points[j] = piter[j]->second;
}
*/
}
// if((piter[0] != point_lookup_map.end()) && (piter[1] != point_lookup_map.end()) && (piter[2] != point_lookup_map.end())) {
if(!is_corner) {
CvMat * newcorners = cvCreateMat( 3, 3, CV_32FC1 );
newcorners->data.fl[3*0+0] = target.points[0].x;
newcorners->data.fl[3*0+1] = target.points[1].x;
newcorners->data.fl[3*0+2] = target.points[2].x;
newcorners->data.fl[3*1+0] = target.points[0].y;
newcorners->data.fl[3*1+1] = target.points[1].y;
newcorners->data.fl[3*1+2] = target.points[2].y;
newcorners->data.fl[3*2+0] = 1;
newcorners->data.fl[3*2+1] = 1;
newcorners->data.fl[3*2+2] = 1;
CvContour hdr;
CvSeqBlock blk;
CvRect trianglebound = cvBoundingRect( cvPointSeqFromMat(CV_SEQ_KIND_CURVE+CV_SEQ_FLAG_CLOSED, curpoints, &hdr, &blk), 1 );
printf("Bounding box: %d,%d,%d,%d\n",trianglebound.x,trianglebound.y,trianglebound.width,trianglebound.height);
for(int y = trianglebound.y; (y < (trianglebound.y + trianglebound.height)) && ( y < image1->height); y++) {
for(int x = trianglebound.x; (x < (trianglebound.x + trianglebound.width)) && (x < image1->width); x++) {
// check to see if we're inside this triangle
/*
CvPoint v0 = cvPoint( curtri.points[2].x - curtri.points[0].x, curtri.points[2].y - curtri.points[0].y );
CvPoint v1 = cvPoint( curtri.points[1].x - curtri.points[0].x, curtri.points[1].y - curtri.points[0].y );
CvPoint v2 = cvPoint( x - curtri.points[0].x, y - curtri.points[0].y );
int dot00 = v0.x * v0.x + v0.y * v0. y;
int dot01 = v0.x * v1.x + v0.y * v1. y;
int dot02 = v0.x * v2.x + v0.y * v2. y;
int dot11 = v1.x * v1.x + v1.y * v1. y;
int dot12 = v1.x * v2.x + v1.y * v2. y;
double invDenom = 1.0 / (double)(dot00 * dot11 - dot01 * dot01);
double u = (double)(dot11 * dot02 - dot01 * dot12) * invDenom;
double v = (double)(dot00 * dot12 - dot01 * dot02) * invDenom;
*/
CvMat * curp = cvCreateMat(3, 1, CV_32FC1);
CvMat * result = cvCreateMat(3, 1, CV_32FC1);
curp->data.fl[0] = x;
curp->data.fl[1] = y;
curp->data.fl[2] = 1;
cvMatMul( baryinvvec[i], curp, result );
// double u = result->data.fl[0]/result->data.fl[2];
// double v = result->data.fl[1]/result->data.fl[2];
if( (result->data.fl[0] > 0) && (result->data.fl[1] > 0) && (fabs(1.0 - (result->data.fl[0]+result->data.fl[1]+result->data.fl[2])) <= 0.01) ) {
// if((u > 0) || (v > 0) /*&& ((u +v) < 1)*/ ) {
// printf("Barycentric: %f %f %f\n", result->data.fl[0], result->data.fl[1], result->data.fl[2]);
// this point is inside this triangle
// printf("Point %d,%d inside %d,%d %d,%d %d,%d\n",x,y,trivec[i].points[0].x,trivec[i].points[0].y,
// trivec[i].points[1].x,trivec[i].points[1].y,trivec[i].points[2].x,trivec[i].points[2].y);
CvMat * sourcepoint = cvCreateMat(3, 1, CV_32FC1);
cvMatMul( newcorners, result, sourcepoint );
double sourcex = sourcepoint->data.fl[0]/*/sourcepoint->data.fl[2]*/;
double sourcey = sourcepoint->data.fl[1]/*/sourcepoint->data.fl[2]*/;
if((sourcex >= 0) && (sourcey >= 0) && (sourcex < (image1->width)) && (sourcey < (image1->height))) {
// printf("%d,%d %d,%d\n",x,y,(int)sourcex,(int)sourcey);
cvSet2D( image1, y, x, cvGet2D( clean_nonthresh, (int)sourcey, (int)sourcex ) );
}
/*
if((i == 143) && (y == 3577) && (x > 2055) && (x < 2087)) {
printf("%d: %f, %f, %f\t%f, %f, %f\n",x,result->data.fl[0],result->data.fl[1],result->data.fl[2],
sourcepoint->data.fl[0],sourcepoint->data.fl[1],sourcepoint->data.fl[2]);
}
*/
cvReleaseMat( &sourcepoint );
// printf("Point %d,%d inside %d,%d %d,%d %d,%d\n",x,y,trivec[i].points[0].x,trivec[i].points[0].y,
// trivec[i].points[1].x,trivec[i].points[1].y,trivec[i].points[2].x,trivec[i].points[2].y);
}
cvReleaseMat( &result );
cvReleaseMat( &curp );
}
}
cvReleaseMat( &newcorners );
}
cvReleaseMat( &curpoints );
}
/*
for(int y = 0; y < image1->height; y++) {
for(int x = 0; x < image1->width; x++) {
CvMat * curp = cvCreateMat(3, 1, CV_32FC1);
CvMat * result = cvCreateMat(3, 1, CV_32FC1);
curp->data.fl[0] = x;
curp->data.fl[1] = y;
curp->data.fl[2] = 1;
for(unsigned int i = 0; i < baryinvvec.size(); i++) {
cvMatMul( baryinvvec[i], curp, result );
double u = result->data.fl[0]/result->data.fl[2];
double v = result->data.fl[1]/result->data.fl[2];
if((u > 0) && (v > 0) && (u + v < 1)) {
// printf("Point %d,%d inside %d,%d %d,%d %d,%d\n",x,y,trivec[i].points[0].x,trivec[i].points[0].y,
// trivec[i].points[1].x,trivec[i].points[1].y,trivec[i].points[2].x,trivec[i].points[2].y);
break;
}
}
cvReleaseMat( &result );
cvReleaseMat( &curp );
}
}
*/
cvReleaseImage( &clean_nonthresh );
#ifdef OLD_BUSTED
for(int y = 0; y < image1->height; y++) {
for(int x = 0; x < image1->width; x++) {
CvSubdiv2DPointLocation locate_result;
CvSubdiv2DEdge on_edge;
CvSubdiv2DPoint * on_vertex;
CvPoint curpoint = cvPoint( x, y );
locate_result = cvSubdiv2DLocate( delaunay, cvPointTo32f( curpoint ),
&on_edge, &on_vertex );
if( (locate_result != CV_PTLOC_OUTSIDE_RECT) && (locate_result != CV_PTLOC_ERROR) ) {
if( locate_result == CV_PTLOC_VERTEX ) { // this point is on a vertex
for(int i = 0; i < count; i++) {
if(((on_vertex->pt).x == delaunay_points[i].x) && ((on_vertex->pt).y == delaunay_points[i].y)) {
cvSet2D( image1, y, x, cvGet2D( image2, cvPointFrom32f(dest_points[i]).y, cvPointFrom32f(dest_points[i]).x ) );
break;
}
}
}
else if( locate_result == CV_PTLOC_ON_EDGE ) { // this point is on an edge
CvSubdiv2DPoint* org_pt;
CvSubdiv2DPoint* dst_pt;
CvPoint org_pt_warp;
CvPoint dst_pt_warp;
org_pt = cvSubdiv2DEdgeOrg(on_edge);
dst_pt = cvSubdiv2DEdgeDst(on_edge);
for(int i = 0; i < count; i++) {
if(((org_pt->pt).x == delaunay_points[i].x) && ((org_pt->pt).y == delaunay_points[i].y)) {
org_pt_warp = cvPointFrom32f(dest_points[i]);
}
if(((dst_pt->pt).x == delaunay_points[i].x) && ((dst_pt->pt).y == delaunay_points[i].y)) {
dst_pt_warp = cvPointFrom32f(dest_points[i]);
}
}
// compute vector length of original edge and current point
double original_length;
double cur_length;
if( (int)((org_pt->pt).x) == curpoint.x ) { // vertical line
original_length = fabs((org_pt->pt).y - (dst_pt->pt).y);
cur_length = fabs((org_pt->pt).y - curpoint.y);
}
else if( (int)((org_pt->pt).y) == curpoint.y ) { // horizontal line
original_length = fabs((org_pt->pt).x - (dst_pt->pt).x);
cur_length = fabs((org_pt->pt).x - curpoint.x);
}
else { // sloped line
original_length = sqrt(pow((org_pt->pt).x - (dst_pt->pt).x, 2.0) + pow((org_pt->pt).y - (dst_pt->pt).y, 2.0));
cur_length = sqrt(pow((org_pt->pt).x - curpoint.x, 2.0) + pow((org_pt->pt).y - curpoint.y, 2.0));
}
// compute ratio of this point on the edge
double ratio = cur_length / original_length;
// copy this point from the destination edge
CvPoint point_in_original;
int warped_x = (int)(org_pt_warp.x - dst_pt_warp.x);
int warped_y = (int)(org_pt_warp.y - dst_pt_warp.y);
if( org_pt_warp.x == curpoint.x ) { // vertical line
point_in_original.y = (int)(org_pt_warp.y + (ratio * (org_pt_warp.y - dst_pt_warp.y)));
point_in_original.x = org_pt_warp.x;
}
else if(org_pt_warp.y == curpoint.y) { // horizontal line
point_in_original.x = (int)(org_pt_warp.x + (ratio * (org_pt_warp.x - dst_pt_warp.x)));
point_in_original.y = org_pt_warp.y;
}
else { // sloped line
double destination_length = sqrt(pow((org_pt_warp).x - (dst_pt_warp).x, 2.0) + pow((org_pt_warp).y - (dst_pt_warp).y, 2.0));
double scaled_length = ratio * destination_length;
double dest_angle = atan(fabs( (double)warped_y / (double)warped_x ));
double xdist = scaled_length * cos(dest_angle);
double ydist = scaled_length * sin(dest_angle);
xdist = warped_x > 0 ? xdist : xdist * -1;
ydist = warped_y > 0 ? ydist : ydist * -1;
point_in_original.x = (int)( org_pt_warp.x + xdist);
point_in_original.y = (int)( org_pt_warp.y + ydist);
}
if((point_in_original.x >= 0) && (point_in_original.y >= 0) && (point_in_original.x < (image1->width)) && (point_in_original.y < (image1->height))) {
cvSet2D( image1, y, x, cvGet2D( image2, point_in_original.y, point_in_original.x ) );
}
else {
printf("Edge point outside image\n");
}
// cvSet2D( image1, y, x, cvGet2D( image2, (int)(org_pt_warp.x + (ratio * (org_pt_warp.x - dst_pt_warp.x))),
// (int)(org_pt_warp.y + (ratio * (org_pt_warp.y - dst_pt_warp.y))) ) );
}
else if( locate_result == CV_PTLOC_INSIDE ) { // this point is inside a facet (triangle)
/*
printf("Point inside facet: %d, %d\n",curpoint.x,curpoint.y);
int count = 0;
CvPoint * origins = (CvPoint*)malloc(sizeof(CvPoint)*3);
CvSubdiv2DEdge t = on_edge;
// count number of edges
do {
CvSubdiv2DPoint* pt = cvSubdiv2DEdgeOrg( t );
if(count < 3) {
origins[count] = cvPoint( cvRound(pt->pt.x), cvRound(pt->pt.y));
printf("%d,%d\t",origins[count].x,origins[count].y);
}
count++;
t = cvSubdiv2DGetEdge( t, CV_NEXT_AROUND_LEFT );
} while(t != on_edge);
printf("\n");
free(origins);
*/
}
}
}
}
#endif // OLD_BUSTED
printf("done.\n");
cvSaveImage("fullwarp.jpg", image1);
printf("Drawing subdivisions on warped image...");
draw_subdiv( image1, delaunay, NULL, NULL, 0, NULL );
// draw_subdiv( image1, delaunay, delaunay_points, source_points, count, status );
printf("done.\n");
cvSaveImage("edgeswarp.jpg", image1);
cvReleaseImage(&image2);
image2 = cvLoadImage(im2fname, CV_LOAD_IMAGE_COLOR);
// cvCreateImage( cvGetSize(image2), IPL_DEPTH_8U, 3 );
// cvCalcSubdivVoronoi2D( delaunay );
printf("Drawing subdivisions on unwarped image...");
draw_subdiv( image2, delaunay, delaunay_points, dest_points, count, status );
// draw_subdiv( image2, delaunay, NULL, NULL, 0, NULL );
printf("done.\n");
cvSaveImage("edges.jpg",image2);
cvReleaseImage(&image1);
cvFree(&source_points);
cvFree(&dest_points);
cvFree(&status);
cvReleaseMemStorage(&storage);
cvFree(&delaunay_points);
cvReleaseImage(&image2);
return 0;
}
/* calculates length of a curve (e.g. contour perimeter) */
CV_IMPL double
cvArcLength( const void *array, CvSlice slice, int is_closed )
{
double perimeter = 0;
int i, j = 0, count;
const int N = 16;
float buf[N];
CvMat buffer = cvMat( 1, N, CV_32F, buf );
CvSeqReader reader;
CvContour contour_header;
CvSeq* contour = 0;
CvSeqBlock block;
if( CV_IS_SEQ( array ))
{
contour = (CvSeq*)array;
if( !CV_IS_SEQ_POLYLINE( contour ))
CV_Error( CV_StsBadArg, "Unsupported sequence type" );
if( is_closed < 0 )
is_closed = CV_IS_SEQ_CLOSED( contour );
}
else
{
is_closed = is_closed > 0;
contour = cvPointSeqFromMat(
CV_SEQ_KIND_CURVE | (is_closed ? CV_SEQ_FLAG_CLOSED : 0),
array, &contour_header, &block );
}
if( contour->total > 1 )
{
int is_float = CV_SEQ_ELTYPE( contour ) == CV_32FC2;
cvStartReadSeq( contour, &reader, 0 );
cvSetSeqReaderPos( &reader, slice.start_index );
count = cvSliceLength( slice, contour );
count -= !is_closed && count == contour->total;
// scroll the reader by 1 point
reader.prev_elem = reader.ptr;
CV_NEXT_SEQ_ELEM( sizeof(CvPoint), reader );
for( i = 0; i < count; i++ )
{
float dx, dy;
if( !is_float )
{
CvPoint* pt = (CvPoint*)reader.ptr;
CvPoint* prev_pt = (CvPoint*)reader.prev_elem;
dx = (float)pt->x - (float)prev_pt->x;
dy = (float)pt->y - (float)prev_pt->y;
}
else
{
CvPoint2D32f* pt = (CvPoint2D32f*)reader.ptr;
CvPoint2D32f* prev_pt = (CvPoint2D32f*)reader.prev_elem;
dx = pt->x - prev_pt->x;
dy = pt->y - prev_pt->y;
}
reader.prev_elem = reader.ptr;
CV_NEXT_SEQ_ELEM( contour->elem_size, reader );
// Bugfix by Axel at rubico.com 2010-03-22, affects closed slices only
// wraparound not handled by CV_NEXT_SEQ_ELEM
if( is_closed && i == count - 2 )
cvSetSeqReaderPos( &reader, slice.start_index );
buffer.data.fl[j] = dx * dx + dy * dy;
if( ++j == N || i == count - 1 )
{
buffer.cols = j;
cvPow( &buffer, &buffer, 0.5 );
for( ; j > 0; j-- )
perimeter += buffer.data.fl[j-1];
}
}
}
return perimeter;
}
CV_IMPL CvBox2D
cvMinAreaRect2( const CvArr* array, CvMemStorage* storage )
{
cv::Ptr<CvMemStorage> temp_storage;
CvBox2D box;
cv::AutoBuffer<CvPoint2D32f> _points;
CvPoint2D32f* points;
memset(&box, 0, sizeof(box));
int i, n;
CvSeqReader reader;
CvContour contour_header;
CvSeqBlock block;
CvSeq* ptseq = (CvSeq*)array;
CvPoint2D32f out[3];
if( CV_IS_SEQ(ptseq) )
{
if( !CV_IS_SEQ_POINT_SET(ptseq) &&
(CV_SEQ_KIND(ptseq) != CV_SEQ_KIND_CURVE ||
CV_SEQ_ELTYPE(ptseq) != CV_SEQ_ELTYPE_PPOINT ))
CV_Error( CV_StsUnsupportedFormat,
"Input sequence must consist of 2d points or pointers to 2d points" );
if( !storage )
storage = ptseq->storage;
}
else
{
ptseq = cvPointSeqFromMat( CV_SEQ_KIND_GENERIC, array, &contour_header, &block );
}
if( storage )
{
temp_storage = cvCreateChildMemStorage( storage );
}
else
{
temp_storage = cvCreateMemStorage(1 << 10);
}
ptseq = cvConvexHull2( ptseq, temp_storage, CV_CLOCKWISE, 1 );
n = ptseq->total;
_points.allocate(n);
points = _points;
cvStartReadSeq( ptseq, &reader );
if( CV_SEQ_ELTYPE( ptseq ) == CV_32SC2 )
{
for( i = 0; i < n; i++ )
{
CvPoint pt;
CV_READ_SEQ_ELEM( pt, reader );
points[i].x = (float)pt.x;
points[i].y = (float)pt.y;
}
}
else
{
for( i = 0; i < n; i++ )
{
CV_READ_SEQ_ELEM( points[i], reader );
}
}
if( n > 2 )
{
icvRotatingCalipers( points, n, CV_CALIPERS_MINAREARECT, (float*)out );
box.center.x = out[0].x + (out[1].x + out[2].x)*0.5f;
box.center.y = out[0].y + (out[1].y + out[2].y)*0.5f;
box.size.width = (float)sqrt((double)out[1].x*out[1].x + (double)out[1].y*out[1].y);
box.size.height = (float)sqrt((double)out[2].x*out[2].x + (double)out[2].y*out[2].y);
box.angle = (float)atan2( (double)out[1].y, (double)out[1].x );
}
else if( n == 2 )
{
box.center.x = (points[0].x + points[1].x)*0.5f;
box.center.y = (points[0].y + points[1].y)*0.5f;
double dx = points[1].x - points[0].x;
double dy = points[1].y - points[0].y;
box.size.width = (float)sqrt(dx*dx + dy*dy);
box.size.height = 0;
box.angle = (float)atan2( dy, dx );
}
else
{
if( n == 1 )
box.center = points[0];
}
box.angle = (float)(box.angle*180/CV_PI);
return box;
}
int opticaltri( CvMat * &clean_texture, int verts )
{
char * im1fname = "conhull-dirty-thresh.jpg";
char * im2fname = "conhull-clean-thresh.jpg";
int count = MAX_COUNT;
char * status;
CvPoint2D32f * source_points;
CvPoint2D32f * dest_points;
CvPoint2D32f * delaunay_points = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(CvPoint2D32f));
// count = opticalflow( im1fname, im2fname, source_points, dest_points, status );
count = findsiftpoints( "conhull-dirty.jpg", "conhull-clean.jpg", source_points, dest_points, status );
IplImage * image1 = cvLoadImage(im1fname, CV_LOAD_IMAGE_COLOR);
CvMemStorage * storage = cvCreateMemStorage(0);
CvSubdiv2D * delaunay = cvCreateSubdivDelaunay2D( cvRect(0,0,image1->width,image1->height), storage);
IplImage * image2 = cvLoadImage(im2fname, CV_LOAD_IMAGE_COLOR);
cvSet( image1, cvScalarAll(255) );
std::map<CvPoint, CvPoint> point_lookup_map;
std::vector<std::pair<CvPoint, CvPoint> > point_lookup;
int num_matches = 0;
int num_out_matches = 0;
int max_dist = 50;
int offset = 200;
// put corners in the point lookup as going to themselves
point_lookup_map[cvPoint(0,0)] = cvPoint(0,0);
point_lookup_map[cvPoint(0,image1->height-1)] = cvPoint(0,image1->height-1);
point_lookup_map[cvPoint(image1->width-1,0)] = cvPoint(image1->width-1,0);
point_lookup_map[cvPoint(image1->width-1,image1->height-1)] = cvPoint(image1->width-1,image1->height-1);
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(0,0), cvPoint(0,0)));
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(0,image1->height-1), cvPoint(0,image1->height-1)));
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(image1->width-1,0), cvPoint(image1->width-1,0)));
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(image1->width-1,image1->height-1), cvPoint(image1->width-1,image1->height-1)));
printf("Inserting corners...");
// put corners in the Delaunay subdivision
for(unsigned int i = 0; i < point_lookup.size(); i++) {
cvSubdivDelaunay2DInsert( delaunay, cvPointTo32f(point_lookup[i].first) );
}
printf("done.\n");
CvSubdiv2DEdge proxy_edge;
for(int i = 0; i < count; i++) {
if(status[i]) {
CvPoint source = cvPointFrom32f(source_points[i]);
CvPoint dest = cvPointFrom32f(dest_points[i]);
if((((int)fabs((double)(source.x - dest.x))) > max_dist) ||
(((int)fabs((double)(source.y - dest.y))) > max_dist)) {
num_out_matches++;
}
else if((dest.x >= 0) && (dest.y >= 0) && (dest.x < (image1->width)) && (dest.y < (image1->height))) {
if(point_lookup_map.find(source) == point_lookup_map.end()) {
num_matches++;
point_lookup_map[source] = dest;
point_lookup.push_back(std::pair<CvPoint,CvPoint>(source,dest));
// delaunay_points[i] =
(cvSubdivDelaunay2DInsert( delaunay, cvPointTo32f(source) ))->pt;
cvSetImageROI( image1, cvRect(source.x-8,source.y-8,8*2,8*2) );
cvResetImageROI( image2 );
cvGetRectSubPix( image2, image1, dest_points[i] );
}
/*
cvSet2D( image1, source.y, source.x, cvGet2D( image2, dest.y, dest.x ) );
cvSet2D( image1, source.y, source.x+1, cvGet2D( image2, dest.y, dest.x+1 ) );
cvSet2D( image1, source.y, source.x-1, cvGet2D( image2, dest.y, dest.x-1 ) );
cvSet2D( image1, source.y+1, source.x, cvGet2D( image2, dest.y+1, dest.x ) );
cvSet2D( image1, source.y-1, source.x, cvGet2D( image2, dest.y-1, dest.x ) );
cvSet2D( image1, source.y+1, source.x+1, cvGet2D( image2, dest.y+1, dest.x+1 ) );
cvSet2D( image1, source.y-1, source.x-1, cvGet2D( image2, dest.y-1, dest.x-1 ) );
cvSet2D( image1, source.y+1, source.x-1, cvGet2D( image2, dest.y+1, dest.x-1 ) );
cvSet2D( image1, source.y-1, source.x+1, cvGet2D( image2, dest.y-1, dest.x+1 ) );
*/
// cvCircle( image1, source, 4, CV_RGB(255,0,0), 2, CV_AA );
// cvCircle( image2, dest, 4, CV_RGB(255,0,0), 2, CV_AA );
}
/*
cvSetImageROI( image1, cvRect(source.x-offset,source.y-offset,offset*2,offset*2) );
cvSetImageROI( image2, cvRect(dest.x-offset,dest.y-offset,offset*2,offset*2) );
cvNamedWindow("image1",0);
cvNamedWindow("image2",0);
cvShowImage("image1",image1);
cvShowImage("image2",image2);
printf("%d,%d -> %d,%d\n",source.x,source.y,dest.x,dest.y);
cvWaitKey(0);
cvDestroyAllWindows();
*/
}
}
printf("%d %d\n",num_matches,num_out_matches);
printf("%d lookups\n",point_lookup_map.size());
cvResetImageROI( image1 );
cvSaveImage("sparse.jpg", image1);
cvReleaseImage(&image1);
image1 = cvLoadImage(im1fname, CV_LOAD_IMAGE_COLOR);
cvSet( image1, cvScalarAll(255) );
printf("Warping image...");
CvSeqReader reader;
int total = delaunay->edges->total;
int elem_size = delaunay->edges->elem_size;
std::vector<Triangle> trivec;
std::vector<CvMat *> baryinvvec;
for( int i = 0; i < total*2; i++ ) {
if((i == 0) || (i == total)) {
cvStartReadSeq( (CvSeq*)(delaunay->edges), &reader, 0 );
}
CvQuadEdge2D* edge = (CvQuadEdge2D*)(reader.ptr);
if( CV_IS_SET_ELEM( edge )) {
CvSubdiv2DEdge curedge = (CvSubdiv2DEdge)edge;
CvSubdiv2DEdge t = curedge;
Triangle temptri;
int count = 0;
// construct a triangle from this edge
do {
CvSubdiv2DPoint* pt = cvSubdiv2DEdgeOrg( t );
if(count < 3) {
pt->pt.x = pt->pt.x >= image1->width ? image1->width-1 : pt->pt.x;
pt->pt.y = pt->pt.y >= image1->height ? image1->height-1 : pt->pt.y;
pt->pt.x = pt->pt.x < 0 ? 0 : pt->pt.x;
pt->pt.y = pt->pt.y < 0 ? 0 : pt->pt.y;
temptri.points[count] = cvPointFrom32f( pt->pt );
}
else {
printf("More than 3 edges\n");
}
count++;
if(i < total)
t = cvSubdiv2DGetEdge( t, CV_NEXT_AROUND_LEFT );
else
t = cvSubdiv2DGetEdge( t, CV_NEXT_AROUND_RIGHT );
} while( t != curedge );
// check that triangle is not already in
if( std::find(trivec.begin(), trivec.end(), temptri) == trivec.end() ) {
// push triangle in and draw
trivec.push_back(temptri);
cvLine( image1, temptri.points[0], temptri.points[1], CV_RGB(255,0,0), 1, CV_AA, 0 );
cvLine( image1, temptri.points[1], temptri.points[2], CV_RGB(255,0,0), 1, CV_AA, 0 );
cvLine( image1, temptri.points[2], temptri.points[0], CV_RGB(255,0,0), 1, CV_AA, 0 );
// compute barycentric computation vector for this triangle
CvMat * barycen = cvCreateMat( 3, 3, CV_32FC1 );
CvMat * baryceninv = cvCreateMat( 3, 3, CV_32FC1 );
barycen->data.fl[3*0+0] = temptri.points[0].x;
barycen->data.fl[3*0+1] = temptri.points[1].x;
barycen->data.fl[3*0+2] = temptri.points[2].x;
barycen->data.fl[3*1+0] = temptri.points[0].y;
barycen->data.fl[3*1+1] = temptri.points[1].y;
barycen->data.fl[3*1+2] = temptri.points[2].y;
barycen->data.fl[3*2+0] = 1;
barycen->data.fl[3*2+1] = 1;
barycen->data.fl[3*2+2] = 1;
cvInvert( barycen, baryceninv, CV_LU );
baryinvvec.push_back(baryceninv);
cvReleaseMat( &barycen );
}
}
CV_NEXT_SEQ_ELEM( elem_size, reader );
}
printf("%d triangles...", trivec.size());
cvSaveImage("triangles.jpg", image1);
cvSet( image1, cvScalarAll(255) );
IplImage * clean_nonthresh = cvLoadImage( "conhull-clean.jpg", CV_LOAD_IMAGE_COLOR );
// for each triangle
for(unsigned int i = 0; i < trivec.size(); i++) {
Triangle curtri = trivec[i];
CvMat * curpoints = cvCreateMat( 1, 3, CV_32SC2 );
Triangle target;
std::map<CvPoint,CvPoint>::iterator piter[3];
printf("Triangle %d / %d\n",i,trivec.size());
int is_corner = 0;
for(int j = 0; j < 3; j++) {
/*
curpoints->data.i[2*j+0] = curtri.points[j].x;
curpoints->data.i[2*j+1] = curtri.points[j].y;
*/
CV_MAT_ELEM( *curpoints, CvPoint, 0, j ) = curtri.points[j];
printf("%d,%d\n",curtri.points[j].x,curtri.points[j].y);
if((curtri.points[j] == cvPoint(0,0)) || (curtri.points[j] == cvPoint(0,image1->height - 1)) ||(curtri.points[j] == cvPoint(image1->width - 1,0)) ||(curtri.points[j] == cvPoint(image1->width - 1,image1->height - 1))) {
is_corner++;
}
for(unsigned int k = 0; k < point_lookup.size(); k++) {
std::pair<CvPoint,CvPoint> thispair = point_lookup[k];
if(thispair.first == curtri.points[j]) {
target.points[j] = thispair.second;
break;
}
}
/*
piter[j] = point_lookup_map.find(curtri.points[j]);
if(piter[j] != point_lookup_map.end() ) {
target.points[j] = piter[j]->second;
}
*/
}
// if((piter[0] != point_lookup_map.end()) && (piter[1] != point_lookup_map.end()) && (piter[2] != point_lookup_map.end())) {
if(is_corner < 3) {
CvMat * newcorners = cvCreateMat( 3, 3, CV_32FC1 );
newcorners->data.fl[3*0+0] = target.points[0].x;
newcorners->data.fl[3*0+1] = target.points[1].x;
newcorners->data.fl[3*0+2] = target.points[2].x;
newcorners->data.fl[3*1+0] = target.points[0].y;
newcorners->data.fl[3*1+1] = target.points[1].y;
newcorners->data.fl[3*1+2] = target.points[2].y;
newcorners->data.fl[3*2+0] = 1;
newcorners->data.fl[3*2+1] = 1;
newcorners->data.fl[3*2+2] = 1;
CvContour hdr;
CvSeqBlock blk;
CvRect trianglebound = cvBoundingRect( cvPointSeqFromMat(CV_SEQ_KIND_CURVE+CV_SEQ_FLAG_CLOSED, curpoints, &hdr, &blk), 1 );
printf("Bounding box: %d,%d,%d,%d\n",trianglebound.x,trianglebound.y,trianglebound.width,trianglebound.height);
for(int y = trianglebound.y; (y < (trianglebound.y + trianglebound.height)) && ( y < image1->height); y++) {
for(int x = trianglebound.x; (x < (trianglebound.x + trianglebound.width)) && (x < image1->width); x++) {
// check to see if we're inside this triangle
/*
CvPoint v0 = cvPoint( curtri.points[2].x - curtri.points[0].x, curtri.points[2].y - curtri.points[0].y );
CvPoint v1 = cvPoint( curtri.points[1].x - curtri.points[0].x, curtri.points[1].y - curtri.points[0].y );
CvPoint v2 = cvPoint( x - curtri.points[0].x, y - curtri.points[0].y );
int dot00 = v0.x * v0.x + v0.y * v0. y;
int dot01 = v0.x * v1.x + v0.y * v1. y;
int dot02 = v0.x * v2.x + v0.y * v2. y;
int dot11 = v1.x * v1.x + v1.y * v1. y;
int dot12 = v1.x * v2.x + v1.y * v2. y;
double invDenom = 1.0 / (double)(dot00 * dot11 - dot01 * dot01);
double u = (double)(dot11 * dot02 - dot01 * dot12) * invDenom;
double v = (double)(dot00 * dot12 - dot01 * dot02) * invDenom;
*/
CvMat * curp = cvCreateMat(3, 1, CV_32FC1);
CvMat * result = cvCreateMat(3, 1, CV_32FC1);
curp->data.fl[0] = x;
curp->data.fl[1] = y;
curp->data.fl[2] = 1;
cvMatMul( baryinvvec[i], curp, result );
// double u = result->data.fl[0]/result->data.fl[2];
// double v = result->data.fl[1]/result->data.fl[2];
/*
if((i == 3019) && (y == 1329) && (x > 2505) && (x < 2584)) {
printf("Range %d: %f, %f, %f\t%f, %f, %f\n",x,result->data.fl[0],result->data.fl[1],result->data.fl[2],
sourcepoint->data.fl[0],sourcepoint->data.fl[1],sourcepoint->data.fl[2]);
}
*/
if( (result->data.fl[0] > MIN_VAL) && (result->data.fl[1] > MIN_VAL) && (result->data.fl[2] > MIN_VAL) && (fabs(1.0 - (result->data.fl[0]+result->data.fl[1]+result->data.fl[2])) <= 0.01) ) {
// if((u > 0) || (v > 0) /*&& ((u +v) < 1)*/ )
// printf("Barycentric: %f %f %f\n", result->data.fl[0], result->data.fl[1], result->data.fl[2]);
// this point is inside this triangle
// printf("Point %d,%d inside %d,%d %d,%d %d,%d\n",x,y,trivec[i].points[0].x,trivec[i].points[0].y,
// trivec[i].points[1].x,trivec[i].points[1].y,trivec[i].points[2].x,trivec[i].points[2].y);
CvMat * sourcepoint = cvCreateMat(3, 1, CV_32FC1);
cvMatMul( newcorners, result, sourcepoint );
double sourcex = sourcepoint->data.fl[0]/*/sourcepoint->data.fl[2]*/;
double sourcey = sourcepoint->data.fl[1]/*/sourcepoint->data.fl[2]*/;
if((sourcex >= 0) && (sourcey >= 0) && (sourcex < (image1->width)) && (sourcey < (image1->height))) {
// printf("%d,%d %d,%d\n",x,y,(int)sourcex,(int)sourcey);
cvSet2D( image1, y, x, cvGet2D( clean_nonthresh, (int)sourcey, (int)sourcex ) );
}
// printf("Point %d,%d inside %d,%d %d,%d %d,%d\n",x,y,trivec[i].points[0].x,trivec[i].points[0].y,
// trivec[i].points[1].x,trivec[i].points[1].y,trivec[i].points[2].x,trivec[i].points[2].y);
cvReleaseMat( &sourcepoint );
}
cvReleaseMat( &result );
cvReleaseMat( &curp );
}
}
for(int k = 0; k < verts; k++) {
double x = clean_texture->data.fl[2*k+0];
double y = clean_texture->data.fl[2*k+1];
// check to see if we're inside this triangle
CvMat * curp = cvCreateMat(3, 1, CV_32FC1);
CvMat * result = cvCreateMat(3, 1, CV_32FC1);
curp->data.fl[0] = x;
curp->data.fl[1] = y;
curp->data.fl[2] = 1;
cvMatMul( baryinvvec[i], curp, result );
if( (result->data.fl[0] > MIN_VAL) && (result->data.fl[1] > MIN_VAL) && (result->data.fl[2] > MIN_VAL) && (fabs(1.0 - (result->data.fl[0]+result->data.fl[1]+result->data.fl[2])) <= 0.01) ) {
CvMat * sourcepoint = cvCreateMat(3, 1, CV_32FC1);
cvMatMul( newcorners, result, sourcepoint );
double sourcex = sourcepoint->data.fl[0]/*/sourcepoint->data.fl[2]*/;
double sourcey = sourcepoint->data.fl[1]/*/sourcepoint->data.fl[2]*/;
if((sourcex >= 0) && (sourcey >= 0) && (sourcex < (image1->width)) && (sourcey < (image1->height))) {
clean_texture->data.fl[2*k+0] = sourcex;
clean_texture->data.fl[2*k+1] = sourcey;
// cvSet2D( image1, y, x, cvGet2D( clean_nonthresh, (int)sourcey, (int)sourcex ) );
}
cvReleaseMat( &sourcepoint );
}
cvReleaseMat( &result );
cvReleaseMat( &curp );
}
cvReleaseMat( &newcorners );
}
cvReleaseMat( &curpoints );
}
cvReleaseImage( &clean_nonthresh );
printf("done.\n");
cvSaveImage("fullwarp.jpg", image1);
printf("Drawing subdivisions on warped image...");
draw_subdiv( image1, delaunay, NULL, NULL, 0, NULL );
// draw_subdiv( image1, delaunay, delaunay_points, source_points, count, status );
printf("done.\n");
cvSaveImage("edgeswarp.jpg", image1);
cvReleaseImage(&image2);
image2 = cvLoadImage(im2fname, CV_LOAD_IMAGE_COLOR);
// cvCreateImage( cvGetSize(image2), IPL_DEPTH_8U, 3 );
// cvCalcSubdivVoronoi2D( delaunay );
printf("Drawing subdivisions on unwarped image...");
// draw_subdiv( image2, delaunay, delaunay_points, dest_points, count, status );
// draw_subdiv( image2, delaunay, NULL, NULL, 0, NULL );
printf("done.\n");
cvSaveImage("edges.jpg",image2);
cvReleaseImage(&image1);
cvFree(&source_points);
cvFree(&dest_points);
cvFree(&status);
cvReleaseMemStorage(&storage);
cvFree(&delaunay_points);
cvReleaseImage(&image2);
return 0;
}
static void ocl_cvMoments( const void* array, CvMoments* mom, int binary )
{
const int TILE_SIZE = 256;
int type, depth, cn, coi = 0;
CvMat stub, *mat = (CvMat*)array;
CvContour contourHeader;
CvSeq* contour = 0;
CvSeqBlock block;
if( CV_IS_SEQ( array ))
{
contour = (CvSeq*)array;
if( !CV_IS_SEQ_POINT_SET( contour ))
CV_Error( CV_StsBadArg, "The passed sequence is not a valid contour" );
}
if( !moments )
CV_Error( CV_StsNullPtr, "" );
memset( mom, 0, sizeof(*mom));
if( !contour )
{
mat = cvGetMat( mat, &stub, &coi );
type = CV_MAT_TYPE( mat->type );
if( type == CV_32SC2 || type == CV_32FC2 )
{
contour = cvPointSeqFromMat(
CV_SEQ_KIND_CURVE | CV_SEQ_FLAG_CLOSED,
mat, &contourHeader, &block );
}
}
if( contour )
{
icvContourMoments( contour, mom );
return;
}
type = CV_MAT_TYPE( mat->type );
depth = CV_MAT_DEPTH( type );
cn = CV_MAT_CN( type );
cv::Size size = cvGetMatSize( mat );
if( cn > 1 && coi == 0 )
CV_Error( CV_StsBadArg, "Invalid image type" );
if( size.width <= 0 || size.height <= 0 )
return;
cv::Mat src0(mat);
cv::ocl::oclMat src(src0);
cv::Size tileSize;
int blockx,blocky;
if(size.width%TILE_SIZE == 0)
blockx = size.width/TILE_SIZE;
else
blockx = size.width/TILE_SIZE + 1;
if(size.height%TILE_SIZE == 0)
blocky = size.height/TILE_SIZE;
else
blocky = size.height/TILE_SIZE + 1;
cv::ocl::oclMat dst_m(blocky * 10, blockx, CV_64FC1);
cl_mem sum = openCLCreateBuffer(src.clCxt,CL_MEM_READ_WRITE,10*sizeof(double));
int tile_width = std::min(size.width,TILE_SIZE);
int tile_height = std::min(size.height,TILE_SIZE);
size_t localThreads[3] = { tile_height, 1, 1};
size_t globalThreads[3] = { size.height, blockx, 1};
std::vector<std::pair<size_t , const void *> > args,args_sum;
args.push_back( std::make_pair( sizeof(cl_mem) , (void *)&src.data ));
args.push_back( std::make_pair( sizeof(cl_int) , (void *)&src.rows ));
args.push_back( std::make_pair( sizeof(cl_int) , (void *)&src.cols ));
args.push_back( std::make_pair( sizeof(cl_int) , (void *)&src.step ));
args.push_back( std::make_pair( sizeof(cl_int) , (void *)&tileSize.width ));
args.push_back( std::make_pair( sizeof(cl_int) , (void *)&tileSize.height ));
args.push_back( std::make_pair( sizeof(cl_mem) , (void *)&dst_m.data ));
args.push_back( std::make_pair( sizeof(cl_int) , (void *)&dst_m.cols ));
args.push_back( std::make_pair( sizeof(cl_int) , (void *)&dst_m.step ));
args.push_back( std::make_pair( sizeof(cl_int) , (void *)&blocky ));
args.push_back( std::make_pair( sizeof(cl_int) , (void *)&type ));
args.push_back( std::make_pair( sizeof(cl_int) , (void *)&depth ));
args.push_back( std::make_pair( sizeof(cl_int) , (void *)&cn ));
args.push_back( std::make_pair( sizeof(cl_int) , (void *)&coi ));
args.push_back( std::make_pair( sizeof(cl_int) , (void *)&binary ));
args.push_back( std::make_pair( sizeof(cl_int) , (void *)&TILE_SIZE ));
openCLExecuteKernel(dst_m.clCxt, &moments, "CvMoments", globalThreads, localThreads, args, -1, depth);
size_t localThreadss[3] = { 128, 1, 1};
size_t globalThreadss[3] = { 128, 1, 1};
args_sum.push_back( std::make_pair( sizeof(cl_int) , (void *)&src.rows ));
args_sum.push_back( std::make_pair( sizeof(cl_int) , (void *)&src.cols ));
args_sum.push_back( std::make_pair( sizeof(cl_int) , (void *)&tile_height ));
args_sum.push_back( std::make_pair( sizeof(cl_int) , (void *)&tile_width ));
args_sum.push_back( std::make_pair( sizeof(cl_int) , (void *)&TILE_SIZE ));
args_sum.push_back( std::make_pair( sizeof(cl_mem) , (void *)&sum ));
args_sum.push_back( std::make_pair( sizeof(cl_mem) , (void *)&dst_m.data ));
args_sum.push_back( std::make_pair( sizeof(cl_int) , (void *)&dst_m.step ));
openCLExecuteKernel(dst_m.clCxt, &moments, "dst_sum", globalThreadss, localThreadss, args_sum, -1, -1);
double* dstsum = new double[10];
memset(dstsum,0,10*sizeof(double));
openCLReadBuffer(dst_m.clCxt,sum,(void *)dstsum,10*sizeof(double));
mom->m00 = dstsum[0];
mom->m10 = dstsum[1];
mom->m01 = dstsum[2];
mom->m20 = dstsum[3];
mom->m11 = dstsum[4];
mom->m02 = dstsum[5];
mom->m30 = dstsum[6];
mom->m21 = dstsum[7];
mom->m12 = dstsum[8];
mom->m03 = dstsum[9];
delete [] dstsum;
icvCompleteMomentState( mom );
}
