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( CV_StsBadSize, "" );
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 length of a curve (e.g. contour perimeter) */
CV_IMPL double
cvArcLength( const void *array, CvSlice slice, int is_closed )
{
double perimeter = 0;
CV_FUNCNAME( "cvArcLength" );
__BEGIN__;
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;
CV_CALL( 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 );
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];
}
}
}
__END__;
return perimeter;
}
// loads the mushroom database, which is a text file, containing
// one training sample per row, all the input variables and the output variable are categorical,
// the values are encoded by characters.
int mushroom_read_database(const char* filename, CvMat** data, CvMat** missing, CvMat** responses)
{
const int M = 1024;
FILE* f = fopen( filename, "rt" );
CvMemStorage* storage;
CvSeq* seq;
char buf[M+2], *ptr;
float* el_ptr;
CvSeqReader reader;
int i, j, var_count = 0;
if( !f )
return 0;
// read the first line and determine the number of variables
if( !fgets( buf, M, f ))
{
fclose(f);
return 0;
}
for( ptr = buf; *ptr != '\0'; ptr++ )
var_count += *ptr == ',';
assert( ptr - buf == (var_count+1)*2 );
// create temporary memory storage to store the whole database
el_ptr = new float[var_count+1];
storage = cvCreateMemStorage();
seq = cvCreateSeq( 0, sizeof(*seq), (var_count+1)*sizeof(float), storage );
for(;;)
{
for( i = 0; i <= var_count; i++ )
{
int c = buf[i*2];
el_ptr[i] = c == '?' ? -1.f : (float)c;
}
if( i != var_count+1 )
break;
cvSeqPush( seq, el_ptr );
if( !fgets( buf, M, f ) || !strchr( buf, ',' ) )
break;
}
fclose(f);
// allocate the output matrices and copy the base there
*data = cvCreateMat( seq->total, var_count, CV_32F );
*missing = cvCreateMat( seq->total, var_count, CV_8U );
*responses = cvCreateMat( seq->total, 1, CV_32F );
cvStartReadSeq( seq, &reader );
int missingValue = 0;
for (i = 0; i < seq->total; i++) {
const float* sdata = (float*)reader.ptr + 1;
float* ddata = data[0]->data.fl + var_count*i;
float* dr = responses[0]->data.fl + i;
uchar* dm = missing[0]->data.ptr + var_count*i;
char temp =0.0, temp2 =0.0, temp3 =0.0;
for (j = 0; j < var_count; j++ ) {
temp = sdata[j];
ddata[j] = sdata[j];
temp2 = sdata[j] < 0;
if (temp2>0)
++missingValue;
dm[j] = sdata[j] < 0;
}
// temp3 = sdata[-1];
*dr = sdata[-1];
CV_NEXT_SEQ_ELEM( seq->elem_size, reader );
}
cvReleaseMemStorage( &storage );
delete el_ptr;
return 1;
}
CV_IMPL CvSeq*
cvConvexHull2( const CvArr* array, void* hull_storage,
int orientation, int return_points )
{
CvSeq* hull = 0;
CvPoint** pointer = 0;
CvPoint2D32f** pointerf = 0;
int* stack = 0;
CV_FUNCNAME( "cvConvexHull2" );
__BEGIN__;
CvMat* mat = 0;
CvSeqReader reader;
CvSeqWriter writer;
CvContour contour_header, 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, (CvSeq*)&hull_header, &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 = hullseq;
((CvContour*)hull)->rect = cvBoundingRect( ptseq, ptseq == (CvSeq*)&contour_header );
/*if( ptseq != (CvSeq*)&contour_header )
hullseq->v_prev = ptseq;*/
}
__END__;
cvFree( (void**)&pointer );
cvFree( (void**)&stack );
return hull;
}
int run_calibration( CvSeq* image_points_seq, CvSize img_size, CvSize board_size,
float square_size, float aspect_ratio, int flags,
CvMat* camera_matrix, CvMat* dist_coeffs, CvMat** extr_params,
CvMat** reproj_errs, double* avg_reproj_err )
{
int code;
int image_count = image_points_seq->total;
int point_count = board_size.width*board_size.height;
CvMat* image_points = cvCreateMat( 1, image_count*point_count, CV_32FC2 );
CvMat* object_points = cvCreateMat( 1, image_count*point_count, CV_32FC3 );
CvMat* point_counts = cvCreateMat( 1, image_count, CV_32SC1 );
CvMat rot_vects, trans_vects;
int i, j, k;
CvSeqReader reader;
cvStartReadSeq( image_points_seq, &reader );
// initialize arrays of points
for( i = 0; i < image_count; i++ )
{
CvPoint2D32f* src_img_pt = (CvPoint2D32f*)reader.ptr;
CvPoint2D32f* dst_img_pt = ((CvPoint2D32f*)image_points->data.fl) + i*point_count;
CvPoint3D32f* obj_pt = ((CvPoint3D32f*)object_points->data.fl) + i*point_count;
for( j = 0; j < board_size.height; j++ )
for( k = 0; k < board_size.width; k++ )
{
*obj_pt++ = cvPoint3D32f(j*square_size, k*square_size, 0);
*dst_img_pt++ = *src_img_pt++;
}
CV_NEXT_SEQ_ELEM( image_points_seq->elem_size, reader );
}
cvSet( point_counts, cvScalar(point_count) );
*extr_params = cvCreateMat( image_count, 6, CV_32FC1 );
cvGetCols( *extr_params, &rot_vects, 0, 3 );
cvGetCols( *extr_params, &trans_vects, 3, 6 );
cvZero( camera_matrix );
cvZero( dist_coeffs );
if( flags & CV_CALIB_FIX_ASPECT_RATIO )
{
camera_matrix->data.db[0] = aspect_ratio;
camera_matrix->data.db[4] = 1.;
}
cvCalibrateCamera2( object_points, image_points, point_counts,
img_size, camera_matrix, dist_coeffs,
&rot_vects, &trans_vects, flags );
code = cvCheckArr( camera_matrix, CV_CHECK_QUIET ) &&
cvCheckArr( dist_coeffs, CV_CHECK_QUIET ) &&
cvCheckArr( *extr_params, CV_CHECK_QUIET );
*reproj_errs = cvCreateMat( 1, image_count, CV_64FC1 );
*avg_reproj_err =
compute_reprojection_error( object_points, &rot_vects, &trans_vects,
camera_matrix, dist_coeffs, image_points, point_counts, *reproj_errs );
fprintf( stderr, " Rot : %f\n",rot_vects.data.fl[0]); fprintf( stderr, "%f\n",rot_vects.data.fl[1]); fprintf( stderr, "%f\n",rot_vects.data.fl[2]);
fprintf( stderr, " Tra : %f\n",trans_vects.data.fl[0]); fprintf( stderr, "%f\n",trans_vects.data.fl[1]); fprintf( stderr, "%f\n",trans_vects.data.fl[2]);
cvReleaseMat( &object_points );
cvReleaseMat( &image_points );
cvReleaseMat( &point_counts );
return code;
}
/* for now this function works bad with singular cases
You can see in the code, that when some troubles with
matrices or some variables occur -
box filled with zero values is returned.
However in general function works fine.
*/
static CvStatus icvFitEllipse_32f( CvSeq* points, CvBox2D* box )
{
CvStatus status = CV_OK;
float u[6];
CvMatr32f D = 0;
float S[36]; /* S = D' * D */
float C[36];
float INVQ[36];
/* transposed eigenvectors */
float INVEIGV[36];
/* auxulary matrices */
float TMP1[36];
float TMP2[36];
int i, index = -1;
float eigenvalues[6];
float a, b, c, d, e, f;
float offx, offy;
float *matr;
int n = points->total;
CvSeqReader reader;
int is_float = CV_SEQ_ELTYPE(points) == CV_32FC2;
CvMat _S, _EIGVECS, _EIGVALS;
/* create matrix D of input points */
D = icvCreateMatrix_32f( 6, n );
offx = offy = 0;
cvStartReadSeq( points, &reader );
/* shift all points to zero */
for( i = 0; i < n; i++ )
{
if( !is_float )
{
offx += (float)((CvPoint*)reader.ptr)->x;
offy += (float)((CvPoint*)reader.ptr)->y;
}
else
{
offx += ((CvPoint2D32f*)reader.ptr)->x;
offy += ((CvPoint2D32f*)reader.ptr)->y;
}
CV_NEXT_SEQ_ELEM( points->elem_size, reader );
}
c = 1.f / n;
offx *= c;
offy *= c;
/* fill matrix rows as (x*x, x*y, y*y, x, y, 1 ) */
matr = D;
for( i = 0; i < n; i++ )
{
float x, y;
if( !is_float )
{
x = (float)((CvPoint*)reader.ptr)->x - offx;
y = (float)((CvPoint*)reader.ptr)->y - offy;
}
else
{
x = ((CvPoint2D32f*)reader.ptr)->x - offx;
y = ((CvPoint2D32f*)reader.ptr)->y - offy;
}
CV_NEXT_SEQ_ELEM( points->elem_size, reader );
matr[0] = x * x;
matr[1] = x * y;
matr[2] = y * y;
matr[3] = x;
matr[4] = y;
matr[5] = 1.f;
matr += 6;
}
/* compute S */
icvMulTransMatrixR_32f( D, 6, n, S );
/* fill matrix C */
icvSetZero_32f( C, 6, 6 );
C[2] = 2.f; //icvSetElement_32f( C, 6, 6, 0, 2, 2.f );
C[7] = -1.f; //icvSetElement_32f( C, 6, 6, 1, 1, -1.f );
C[12] = 2.f; //icvSetElement_32f( C, 6, 6, 2, 0, 2.f );
/* find eigenvalues */
//status1 = icvJacobiEigens_32f( S, INVEIGV, eigenvalues, 6, 0.f );
//assert( status1 == CV_OK );
_S = cvMat( 6, 6, CV_32F, S );
_EIGVECS = cvMat( 6, 6, CV_32F, INVEIGV );
_EIGVALS = cvMat( 6, 1, CV_32F, eigenvalues );
cvEigenVV( &_S, &_EIGVECS, &_EIGVALS, 0 );
//avoid troubles with small negative values
for( i = 0; i < 6; i++ )
eigenvalues[i] = (float)fabs(eigenvalues[i]);
cvbSqrt( eigenvalues, eigenvalues, 6 );
cvbInvSqrt( eigenvalues, eigenvalues, 6 );
for( i = 0; i < 6; i++ )
icvScaleVector_32f( &INVEIGV[i * 6], &INVEIGV[i * 6], 6, eigenvalues[i] );
// INVQ = transp(INVEIGV) * INVEIGV
icvMulTransMatrixR_32f( INVEIGV, 6, 6, INVQ );
/* create matrix INVQ*C*INVQ */
icvMulMatrix_32f( INVQ, 6, 6, C, 6, 6, TMP1 );
icvMulMatrix_32f( TMP1, 6, 6, INVQ, 6, 6, TMP2 );
/* find its eigenvalues and vectors */
//status1 = icvJacobiEigens_32f( TMP2, INVEIGV, eigenvalues, 6, 0.f );
//assert( status1 == CV_OK );
_S = cvMat( 6, 6, CV_32F, TMP2 );
cvEigenVV( &_S, &_EIGVECS, &_EIGVALS, 0 );
/* search for positive eigenvalue */
for( i = 0; i < 3; i++ )
{
if( eigenvalues[i] > 0 )
{
index = i;
break;
}
}
/* only 3 eigenvalues must be not zero
and only one of them must be positive
if it is not true - return zero result
*/
if( index == -1 )
{
box->center.x = box->center.y =
box->size.width = box->size.height =
box->angle = 0.f;
goto error;
}
/* now find truthful eigenvector */
icvTransformVector_32f( INVQ, &INVEIGV[index * 6], u, 6, 6 );
/* extract vector components */
a = u[0];
b = u[1];
c = u[2];
d = u[3];
e = u[4];
f = u[5];
{
/* extract ellipse axes from above values */
/*
1) find center of ellipse
it satisfy equation
| a b/2 | * | x0 | + | d/2 | = |0 |
| b/2 c | | y0 | | e/2 | |0 |
*/
float x0, y0;
float idet = 1.f / (a * c - b * b * 0.25f);
/* we must normalize (a b c d e f ) to fit (4ac-b^2=1) */
float scale = cvSqrt( 0.25f * idet );
if (!scale)
{
box->center.x = box->center.y =
box->size.width = box->size.height =
box->angle = 0.f;
goto error;
}
a *= scale;
b *= scale;
c *= scale;
d *= scale;
e *= scale;
f *= scale;
//x0 = box->center.x = (-d * c * 0.5f + e * b * 0.25f) * 4.f;
//y0 = box->center.y = (-a * e * 0.5f + d * b * 0.25f) * 4.f;
x0 = box->center.x = (-d * c + e * b * 0.5f) * 2.f;
y0 = box->center.y = (-a * e + d * b * 0.5f) * 2.f;
/* offset ellipse to (x0,y0) */
/* new f == F(x0,y0) */
f += a * x0 * x0 + b * x0 * y0 + c * y0 * y0 + d * x0 + e * y0;
if (!f)
{
box->center.x = box->center.y =
box->size.width = box->size.height =
box->angle = 0.f;
goto error;
}
scale = -1.f / f;
/* normalize to f = 1 */
a *= scale;
b *= scale;
c *= scale;
}
/* recover center */
box->center.x += offx;
box->center.y += offy;
/* extract axis of ellipse */
/* one more eigenvalue operation */
TMP1[0] = a;
TMP1[1] = TMP1[2] = b * 0.5f;
TMP1[3] = c;
//status1 = icvJacobiEigens_32f( TMP1, INVEIGV, eigenvalues, 2, 0.f );
//assert( status1 == CV_OK );
_S = cvMat( 2, 2, CV_32F, TMP1 );
_EIGVECS = cvMat( 2, 2, CV_32F, INVEIGV );
_EIGVALS = cvMat( 2, 1, CV_32F, eigenvalues );
cvEigenVV( &_S, &_EIGVECS, &_EIGVALS, 0 );
/* exteract axis length from eigenvectors */
box->size.height = 2 * cvInvSqrt( eigenvalues[0] );
box->size.width = 2 * cvInvSqrt( eigenvalues[1] );
if ( !(box->size.height && box->size.width) )
{
assert(0);
}
/* calc angle */
box->angle = cvFastArctan( INVEIGV[3], INVEIGV[2] );
error:
if( D )
icvDeleteMatrix( D );
return status;
}
int CvMLData::read_csv(const char* filename)
{
const int M = 1000000;
const char str_delimiter[3] = { ' ', delimiter, '\0' };
FILE* file = 0;
CvMemStorage* storage;
CvSeq* seq;
char *ptr;
float* el_ptr;
CvSeqReader reader;
int cols_count = 0;
uchar *var_types_ptr = 0;
clear();
file = fopen( filename, "rt" );
if( !file )
return -1;
// read the first line and determine the number of variables
std::vector<char> _buf(M);
char* buf = &_buf[0];
if( !fgets_chomp( buf, M, file ))
{
fclose(file);
return -1;
}
for( ptr = buf; *ptr != '\0'; ptr++ )
cols_count += (*ptr == delimiter);
if ( cols_count == 0)
{
fclose(file);
return -1;
}
cols_count++;
// create temporary memory storage to store the whole database
el_ptr = new float[cols_count];
storage = cvCreateMemStorage();
seq = cvCreateSeq( 0, sizeof(*seq), cols_count*sizeof(float), storage );
var_types = cvCreateMat( 1, cols_count, CV_8U );
cvZero( var_types );
var_types_ptr = var_types->data.ptr;
for(;;)
{
char *token = NULL;
int type;
token = strtok(buf, str_delimiter);
if (!token)
{
fclose(file);
return -1;
}
for (int i = 0; i < cols_count-1; i++)
{
str_to_flt_elem( token, el_ptr[i], type);
var_types_ptr[i] |= type;
token = strtok(NULL, str_delimiter);
if (!token)
{
fclose(file);
return -1;
}
}
str_to_flt_elem( token, el_ptr[cols_count-1], type);
var_types_ptr[cols_count-1] |= type;
cvSeqPush( seq, el_ptr );
if( !fgets_chomp( buf, M, file ) || !strchr( buf, delimiter ) )
break;
}
fclose(file);
values = cvCreateMat( seq->total, cols_count, CV_32FC1 );
missing = cvCreateMat( seq->total, cols_count, CV_8U );
var_idx_mask = cvCreateMat( 1, values->cols, CV_8UC1 );
cvSet( var_idx_mask, cvRealScalar(1) );
train_sample_count = seq->total;
cvStartReadSeq( seq, &reader );
for(int i = 0; i < seq->total; i++ )
{
const float* sdata = (float*)reader.ptr;
float* ddata = values->data.fl + cols_count*i;
uchar* dm = missing->data.ptr + cols_count*i;
for( int j = 0; j < cols_count; j++ )
{
ddata[j] = sdata[j];
dm[j] = ( fabs( MISS_VAL - sdata[j] ) <= FLT_EPSILON );
}
CV_NEXT_SEQ_ELEM( seq->elem_size, reader );
}
if ( cvNorm( missing, 0, CV_L1 ) <= FLT_EPSILON )
cvReleaseMat( &missing );
cvReleaseMemStorage( &storage );
delete []el_ptr;
return 0;
}
static void icvContourMoments( CvSeq* contour, CvMoments* mom )
{
if( contour->total )
{
CvSeqReader reader;
int lpt = contour->total;
double a00, a10, a01, a20, a11, a02, a30, a21, a12, a03;
cvStartReadSeq( contour, &reader, 0 );
size_t reader_size = lpt << 1;
cv::Mat reader_mat(1,reader_size,CV_32FC1);
bool is_float = CV_SEQ_ELTYPE(contour) == CV_32FC2;
if (!cv::ocl::Context::getContext()->supportsFeature(Context::CL_DOUBLE) && is_float)
{
CV_Error(CV_StsUnsupportedFormat, "Moments - double is not supported by your GPU!");
}
if( is_float )
{
for(size_t i = 0; i < reader_size; ++i)
{
reader_mat.at<float>(0, i++) = ((CvPoint2D32f*)(reader.ptr))->x;
reader_mat.at<float>(0, i) = ((CvPoint2D32f*)(reader.ptr))->y;
CV_NEXT_SEQ_ELEM( contour->elem_size, reader );
}
}
else
{
for(size_t i = 0; i < reader_size; ++i)
{
reader_mat.at<float>(0, i++) = ((CvPoint*)(reader.ptr))->x;
reader_mat.at<float>(0, i) = ((CvPoint*)(reader.ptr))->y;
CV_NEXT_SEQ_ELEM( contour->elem_size, reader );
}
}
cv::ocl::oclMat dst_a(10, lpt, CV_64FC1);
cv::ocl::oclMat reader_oclmat(reader_mat);
int llength = std::min(lpt,128);
size_t localThreads[3] = { llength, 1, 1};
size_t globalThreads[3] = { lpt, 1, 1};
std::vector<std::pair<size_t , const void *> > args;
args.push_back( std::make_pair( sizeof(cl_int) , (void *)&contour->total ));
args.push_back( std::make_pair( sizeof(cl_mem) , (void *)&reader_oclmat.data ));
args.push_back( std::make_pair( sizeof(cl_mem) , (void *)&dst_a.data ));
cl_int dst_step = (cl_int)dst_a.step;
args.push_back( std::make_pair( sizeof(cl_int) , (void *)&dst_step ));
openCLExecuteKernel(dst_a.clCxt, &moments, "icvContourMoments", globalThreads, localThreads, args, -1, -1);
cv::Mat dst(dst_a);
a00 = a10 = a01 = a20 = a11 = a02 = a30 = a21 = a12 = a03 = 0.0;
if (!cv::ocl::Context::getContext()->supportsFeature(Context::CL_DOUBLE))
{
for (int i = 0; i < contour->total; ++i)
{
a00 += dst.at<cl_long>(0, i);
a10 += dst.at<cl_long>(1, i);
a01 += dst.at<cl_long>(2, i);
a20 += dst.at<cl_long>(3, i);
a11 += dst.at<cl_long>(4, i);
a02 += dst.at<cl_long>(5, i);
a30 += dst.at<cl_long>(6, i);
a21 += dst.at<cl_long>(7, i);
a12 += dst.at<cl_long>(8, i);
a03 += dst.at<cl_long>(9, i);
}
}
else
{
a00 = cv::sum(dst.row(0))[0];
a10 = cv::sum(dst.row(1))[0];
a01 = cv::sum(dst.row(2))[0];
a20 = cv::sum(dst.row(3))[0];
a11 = cv::sum(dst.row(4))[0];
a02 = cv::sum(dst.row(5))[0];
a30 = cv::sum(dst.row(6))[0];
a21 = cv::sum(dst.row(7))[0];
a12 = cv::sum(dst.row(8))[0];
a03 = cv::sum(dst.row(9))[0];
}
double db1_2, db1_6, db1_12, db1_24, db1_20, db1_60;
if( fabs(a00) > FLT_EPSILON )
{
if( a00 > 0 )
{
db1_2 = 0.5;
db1_6 = 0.16666666666666666666666666666667;
db1_12 = 0.083333333333333333333333333333333;
db1_24 = 0.041666666666666666666666666666667;
db1_20 = 0.05;
db1_60 = 0.016666666666666666666666666666667;
}
else
{
db1_2 = -0.5;
db1_6 = -0.16666666666666666666666666666667;
db1_12 = -0.083333333333333333333333333333333;
db1_24 = -0.041666666666666666666666666666667;
db1_20 = -0.05;
db1_60 = -0.016666666666666666666666666666667;
}
// spatial moments
mom->m00 = a00 * db1_2;
mom->m10 = a10 * db1_6;
mom->m01 = a01 * db1_6;
mom->m20 = a20 * db1_12;
mom->m11 = a11 * db1_24;
mom->m02 = a02 * db1_12;
mom->m30 = a30 * db1_20;
mom->m21 = a21 * db1_60;
mom->m12 = a12 * db1_60;
mom->m03 = a03 * db1_20;
icvCompleteMomentState( mom );
}
}
}
void CBIR::buildDescriptorsMatrix(QString path){
QString rootG = "../img/keepcon2-g/";
//QString root = "../img/keepcon2/";
QString root = path;
QStringList images;
QStringList fileDirs;
//Carga todas las imágenes.
QDir dir(root);
dir.setFilter(QDir::Files | QDir::NoDotAndDotDot);
dir.setSorting(QDir::Name);
images << dir.entryList();
//Carga las imágenes de los grupos indicados.
/*
fileDirs << "auto" << "converse" << "doki";
for(int i=0; i<fileDirs.count(); i++){
QDir dir(rootG + fileDirs[i] + "/");
dir.setFilter(QDir::Files | QDir::NoDotAndDotDot);
dir.setSorting(QDir::Name | QDir::Reversed);
images << dir.entryList();
}
*/
//Carga de imágenes específicas.
//images << "auto.jpg" << "auto-f.jpg" << "converse.jpg";
cout << "Cantidad de imagenes: " << images.count() << endl;
//int length = (int)(imgDescriptors->elem_size/sizeof(float));
//descriptorsMat = cv::Mat(1106, DESCRIPTOR_DIMS, CV_32F);
//float* img_ptr = descriptorsMat.ptr<float>(0);
//CvSeqReader img_reader;
CvSeq* totalDescriptors = 0;
CvSeq* nextSeq = 0;
IplImage* img;
CvSeq *imgKeypoints, *imgDescriptors;
CvSURFParams params = cvSURFParams(500, 1);
//Calcula features para cada imagen
for(int i=0; i<images.count(); i++){
img = NULL;
imgKeypoints = 0;
imgDescriptors = 0;
qDebug() << "Intento de carga de la imagen" << images[i] << ".";
img = Utils::loadImage((root + images[i]).toAscii().data(), true);
if(img != NULL){
CvMemStorage* storage = cvCreateMemStorage(0);
cvExtractSURF(img, 0, &imgKeypoints, &imgDescriptors, storage, params);
qDebug() << "Imagen" << images[i] << "cargada con exito. Features:" << imgKeypoints->total;
featuresCount.append(QPair<QString, int>(images[i], imgDescriptors->total));
//Linkeo la nueva secuencia de descriptores a la lista.
if(totalDescriptors == 0){
totalDescriptors = imgDescriptors;
nextSeq = totalDescriptors;
}else{
nextSeq->h_next = imgDescriptors;
nextSeq = nextSeq->h_next;
}
//Copiar los descriptores a la matriz de features
/*
cvStartReadSeq(imgDescriptors, &img_reader);
for(int j=0; j<imgDescriptors->total; j++){ // j<1 para cargar un solo descriptor por cada imagen (prueba).
const float* descriptor = (const float*)img_reader.ptr;
CV_NEXT_SEQ_ELEM(img_reader.seq->elem_size, img_reader);
memcpy(img_ptr, descriptor, DESCRIPTOR_DIMS*sizeof(float));
img_ptr += DESCRIPTOR_DIMS;
}
*/
//cvReleaseMemStorage(&storage);
cvReleaseImage(&img);
}
}
int descriptorsCount = 0;
int sequencesCount = 0;
CvSeq* iterateSeq = totalDescriptors;
while(iterateSeq != nextSeq){
descriptorsCount += iterateSeq->total;
iterateSeq = iterateSeq->h_next;
sequencesCount++;
}
if(iterateSeq != 0){
descriptorsCount += iterateSeq->total;
sequencesCount++;
}
qDebug() << "Total de secuencias:" << sequencesCount;
qDebug() << "Total de descriptores:" << descriptorsCount;
//Creo la matriz de descriptores ahora que se conoce la cantidad.
descriptorsMat = cv::Mat(descriptorsCount, DESCRIPTOR_DIMS, CV_32F);
float* img_ptr = descriptorsMat.ptr<float>(0);
CvSeqReader img_reader;
//Copia los descriptores de la lista de secuencias a la matriz.
iterateSeq = totalDescriptors;
CvSeq* deallocateSeq = 0;
for(int i=0; i<sequencesCount; i++){
deallocateSeq = iterateSeq;
cvStartReadSeq(iterateSeq, &img_reader);
int j;
for(j=0; j<iterateSeq->total; j++){
const float* descriptor = (const float*)img_reader.ptr;
CV_NEXT_SEQ_ELEM(img_reader.seq->elem_size, img_reader);
memcpy(img_ptr, descriptor, DESCRIPTOR_DIMS*sizeof(float));
img_ptr += DESCRIPTOR_DIMS;
}
iterateSeq = iterateSeq->h_next;
//Probar que efectivamente se libera la memoria.
cvReleaseMemStorage(&deallocateSeq->storage);
}
}
cv::Mat CBIR::getClustersIndices(QString path){
QString imgName = path.section('/', -1);
IplImage* img = Utils::loadImage(path.toAscii().data(), true);
if(img == NULL){
qDebug() << "La imagen no fue cargada.";
exit(1);
}
//Computo los features.
CvSeq *imgKeypoints, *imgDescriptors;
CvSURFParams params = cvSURFParams(500, 1);
imgKeypoints = 0;
imgDescriptors = 0;
CvMemStorage* storage = cvCreateMemStorage(0);
cvExtractSURF(img, 0, &imgKeypoints, &imgDescriptors, storage, params);
qDebug() << "Imagen" << path << "cargada con exito. Features:" << imgKeypoints->total;
//Copio los descriptores a una Mat.
cv::Mat queryDescriptorsMat(imgDescriptors->total, DESCRIPTOR_DIMS, CV_32F);
float* img_ptr = queryDescriptorsMat.ptr<float>(0);
CvSeqReader img_reader;
cvStartReadSeq(imgDescriptors, &img_reader);
for(int j=0; j<imgDescriptors->total; j++){
const float* descriptor = (const float*)img_reader.ptr;
CV_NEXT_SEQ_ELEM(img_reader.seq->elem_size, img_reader);
memcpy(img_ptr, descriptor, DESCRIPTOR_DIMS*sizeof(float));
img_ptr += DESCRIPTOR_DIMS;
}
cvReleaseMemStorage(&storage);
cvReleaseImage(&img);
if(clustersMat.data == NULL)
exit(1);
//Creo el índice para los cluster centers.
cv::flann::KDTreeIndexParams kdtiParams = cv::flann::KDTreeIndexParams(8);
cv::flann::Index clustersIndex(clustersMat, kdtiParams);
//Clusterizo cada feature de la query según Knn-Search.
cv::Mat indices(queryDescriptorsMat.rows, 1, CV_32S);
cv::Mat dists(queryDescriptorsMat.rows, 1, CV_32F);
clustersIndex.knnSearch(queryDescriptorsMat, indices, dists, 1, cv::flann::SearchParams(1024));
/***************************************************************************************************************/
//Guardo el archivo para realizar la query al índice.
QFile query("cbir/lemur/query/" + imgName + ".query");
QTextStream stream(&query);
if (!query.open(QIODevice::WriteOnly | QIODevice::Text))
qDebug() << "Ocurrio un error al intentar abrir el archivo" + imgName + ".query";
stream << "<DOC 1>" << endl;
// Itero sobre todos los features de la imagen.
for(int i=0; i<queryDescriptorsMat.rows ;i++){
stream << indices.at<int>(i, 0) << endl;
}
stream << "</DOC>";
query.close();
//Guardo el archivo con los parámetros de la query.
QFile qP("cbir/lemur/query/query_params");
QTextStream qPStream(&qP);
if (!qP.open(QIODevice::WriteOnly | QIODevice::Text))
qDebug() << "Ocurrio un error al intentar abrir el archivo query_params";
qPStream << "<parameters>" << endl <<
"<index>e:\\Proyectos\\Git\\keepc\\release\\cbir\\lemur\\index\\index.key</index>" << endl <<
"<retModel>tfidf</retModel>" << endl <<
"<textQuery>e:\\Proyectos\\Git\\keepc\\release\\cbir\\lemur\\query\\" << imgName << ".query</textQuery>" << endl <<
"<resultFile>e:\\Proyectos\\Git\\keepc\\release\\cbir\\lemur\\query\\" << imgName << ".results</resultFile>" << endl <<
"<TRECResultFormat>1</TRECResultFormat>" << endl <<
"<resultCount>10</resultCount>" << endl <<
"</parameters>";
qP.close();
return indices;
}
void moCalibrationModule::guiBuild(void) {
std::ostringstream oss;
moPointList screenPoints = this->property("screenPoints").asPointList();
moPointList::iterator it;
unsigned int index = 0;
this->gui.clear();
this->gui.push_back("viewport 1000 1000");
this->gui.push_back("color 0 121 184");
for ( it = screenPoints.begin(); it != screenPoints.end(); it++ ) {
if ( index == this->active_point )
this->gui.push_back("color 255 255 255");
oss.str("");
oss << "circle " << (int)(it->x * 1000.) << " " << (int)(it->y * 1000.) << " 50";
this->gui.push_back(oss.str());
if ( index == this->active_point )
this->gui.push_back("color 120 120 120");
index++;
}
// draw delaunay triangles ?
if ( this->subdiv == NULL )
return;
this->gui.push_back("color 255 255 255");
CvSeqReader reader;
int i, total = this->subdiv->edges->total;
int elem_size = this->subdiv->edges->elem_size;
cvStartReadSeq( (CvSeq*)(this->subdiv->edges), &reader, 0 );
for( i = 0; i < total; i++ )
{
CvQuadEdge2D* edge = (CvQuadEdge2D*)(reader.ptr);
if( CV_IS_SET_ELEM( edge ))
{
CvSubdiv2DPoint* org_pt;
CvSubdiv2DPoint* dst_pt;
moPoint org;
moPoint dst;
org_pt = cvSubdiv2DEdgeOrg((CvSubdiv2DEdge)edge);
dst_pt = cvSubdiv2DEdgeDst((CvSubdiv2DEdge)edge);
if( org_pt && dst_pt &&
this->delaunayToScreen.find(org_pt) != this->delaunayToScreen.end() &&
this->delaunayToScreen.find(dst_pt) != this->delaunayToScreen.end() )
{
org = this->delaunayToScreen[org_pt];
dst = this->delaunayToScreen[dst_pt];
oss.str("");
oss << "line " << int(org.x * 1000.);
oss << " " << int(org.y * 1000.);
oss << " " << int(dst.x * 1000.);
oss << " " << int(dst.y * 1000.);
this->gui.push_back(oss.str());
LOG(MO_DEBUG, "drawing line: " << i << " surface pos:" << org_pt->pt.x << ","<< org_pt->pt.y<<"|"<<dst_pt->pt.x << ","<< dst_pt->pt.y);
LOG(MO_DEBUG, " creen points: " << oss.str());
}
}
CV_NEXT_SEQ_ELEM( elem_size, reader );
}
// draw touches (since touch can be changed, if we lock input
// we'll not hve trouble)
if ( this->input != NULL ) {
moDataGenericList::iterator it;
int x, y;
this->input->lock();
for ( it = this->blobs.begin(); it != this->blobs.end(); it++ ) {
x = (int)((*it)->properties["x"]->asDouble() * 1000.);
y = (int)((*it)->properties["y"]->asDouble() * 1000.);
this->gui.push_back("color 121 0 184");
oss.str("");
oss << "circle " << x << " " << y << " 80";
this->gui.push_back(oss.str());
this->gui.push_back("color 255 255 255");
oss.str("");
oss << "line " << x - 160 << " " << y << " " << x + 160 << " " << y;
this->gui.push_back(oss.str());
oss.str("");
oss << "line " << x << " " << y - 160 << " " << x << " " << y + 160;
this->gui.push_back(oss.str());
}
this->input->unlock();
}
}
CV_IMPL void*
cvLoad( const char* filename, CvMemStorage* memstorage,
const char* name, const char** _real_name )
{
void* ptr = 0;
const char* real_name = 0;
cv::FileStorage fs(cvOpenFileStorage(filename, memstorage, CV_STORAGE_READ));
CvFileNode* node = 0;
if( !fs.isOpened() )
return 0;
if( name )
{
node = cvGetFileNodeByName( *fs, 0, name );
}
else
{
int i, k;
for( k = 0; k < (*fs)->roots->total; k++ )
{
CvSeq* seq;
CvSeqReader reader;
node = (CvFileNode*)cvGetSeqElem( (*fs)->roots, k );
CV_Assert(node != NULL);
if( !CV_NODE_IS_MAP( node->tag ))
return 0;
seq = node->data.seq;
node = 0;
cvStartReadSeq( seq, &reader, 0 );
// find the first element in the map
for( i = 0; i < seq->total; i++ )
{
if( CV_IS_SET_ELEM( reader.ptr ))
{
node = (CvFileNode*)reader.ptr;
goto stop_search;
}
CV_NEXT_SEQ_ELEM( seq->elem_size, reader );
}
}
stop_search:
;
}
if( !node )
CV_Error( CV_StsObjectNotFound, "Could not find the/an object in file storage" );
real_name = cvGetFileNodeName( node );
ptr = cvRead( *fs, node, 0 );
// sanity check
if( !memstorage && (CV_IS_SEQ( ptr ) || CV_IS_SET( ptr )) )
CV_Error( CV_StsNullPtr,
"NULL memory storage is passed - the loaded dynamic structure can not be stored" );
if( cvGetErrStatus() < 0 )
{
cvRelease( (void**)&ptr );
real_name = 0;
}
if( _real_name)
{
if (real_name)
{
*_real_name = (const char*)cvAlloc(strlen(real_name));
memcpy((void*)*_real_name, real_name, strlen(real_name));
} else {
*_real_name = 0;
}
}
return ptr;
}
CV_IMPL void
cvReadRawDataSlice( const CvFileStorage* fs, CvSeqReader* reader,
int len, void* _data, const char* dt )
{
char* data0 = (char*)_data;
int fmt_pairs[CV_FS_MAX_FMT_PAIRS*2], k = 0, fmt_pair_count;
int i = 0, count = 0;
CV_CHECK_FILE_STORAGE( fs );
if( !reader || !data0 )
CV_Error( CV_StsNullPtr, "Null pointer to reader or destination array" );
if( !reader->seq && len != 1 )
CV_Error( CV_StsBadSize, "The readed sequence is a scalar, thus len must be 1" );
fmt_pair_count = icvDecodeFormat( dt, fmt_pairs, CV_FS_MAX_FMT_PAIRS );
size_t step = ::icvCalcStructSize(dt, 0);
for(;;)
{
int offset = 0;
for( k = 0; k < fmt_pair_count; k++ )
{
int elem_type = fmt_pairs[k*2+1];
int elem_size = CV_ELEM_SIZE(elem_type);
char* data;
count = fmt_pairs[k*2];
offset = cvAlign( offset, elem_size );
data = data0 + offset;
for( i = 0; i < count; i++ )
{
CvFileNode* node = (CvFileNode*)reader->ptr;
if( CV_NODE_IS_INT(node->tag) )
{
int ival = node->data.i;
switch( elem_type )
{
case CV_8U:
*(uchar*)data = cv::saturate_cast<uchar>(ival);
data++;
break;
case CV_8S:
*(char*)data = cv::saturate_cast<schar>(ival);
data++;
break;
case CV_16U:
*(ushort*)data = cv::saturate_cast<ushort>(ival);
data += sizeof(ushort);
break;
case CV_16S:
*(short*)data = cv::saturate_cast<short>(ival);
data += sizeof(short);
break;
case CV_32S:
*(int*)data = ival;
data += sizeof(int);
break;
case CV_32F:
*(float*)data = (float)ival;
data += sizeof(float);
break;
case CV_64F:
*(double*)data = (double)ival;
data += sizeof(double);
break;
case CV_USRTYPE1: /* reference */
*(size_t*)data = ival;
data += sizeof(size_t);
break;
default:
CV_Error( CV_StsUnsupportedFormat, "Unsupported type" );
return;
}
}
else if( CV_NODE_IS_REAL(node->tag) )
{
double fval = node->data.f;
int ival;
switch( elem_type )
{
case CV_8U:
ival = cvRound(fval);
*(uchar*)data = cv::saturate_cast<uchar>(ival);
data++;
break;
case CV_8S:
ival = cvRound(fval);
*(char*)data = cv::saturate_cast<schar>(ival);
data++;
break;
case CV_16U:
ival = cvRound(fval);
*(ushort*)data = cv::saturate_cast<ushort>(ival);
data += sizeof(ushort);
break;
case CV_16S:
ival = cvRound(fval);
*(short*)data = cv::saturate_cast<short>(ival);
data += sizeof(short);
break;
case CV_32S:
ival = cvRound(fval);
*(int*)data = ival;
data += sizeof(int);
break;
case CV_32F:
*(float*)data = (float)fval;
data += sizeof(float);
break;
case CV_64F:
*(double*)data = fval;
data += sizeof(double);
break;
case CV_USRTYPE1: /* reference */
ival = cvRound(fval);
*(size_t*)data = ival;
data += sizeof(size_t);
break;
default:
CV_Error( CV_StsUnsupportedFormat, "Unsupported type" );
return;
}
}
else
CV_Error( CV_StsError,
"The sequence element is not a numerical scalar" );
CV_NEXT_SEQ_ELEM( sizeof(CvFileNode), *reader );
if( !--len )
goto end_loop;
}
offset = (int)(data - data0);
}
data0 += step;
}
end_loop:
if( i != count - 1 || k != fmt_pair_count - 1 )
CV_Error( CV_StsBadSize,
"The sequence slice does not fit an integer number of records" );
if( !reader->seq )
reader->ptr -= sizeof(CvFileNode);
}
/* for now this function works bad with singular cases
You can see in the code, that when some troubles with
matrices or some variables occur -
box filled with zero values is returned.
However in general function works fine.
*/
static void
icvFitEllipse_F( CvSeq* points, CvBox2D* box )
{
CvMat* D = 0;
CV_FUNCNAME( "icvFitEllipse_F" );
__BEGIN__;
double S[36], C[36], T[36];
int i, j;
double eigenvalues[6], eigenvectors[36];
double a, b, c, d, e, f;
double x0, y0, idet, scale, offx = 0, offy = 0;
int n = points->total;
CvSeqReader reader;
int is_float = CV_SEQ_ELTYPE(points) == CV_32FC2;
CvMat _S = cvMat(6,6,CV_64F,S), _C = cvMat(6,6,CV_64F,C), _T = cvMat(6,6,CV_64F,T);
CvMat _EIGVECS = cvMat(6,6,CV_64F,eigenvectors), _EIGVALS = cvMat(6,1,CV_64F,eigenvalues);
/* create matrix D of input points */
CV_CALL( D = cvCreateMat( n, 6, CV_64F ));
cvStartReadSeq( points, &reader );
/* shift all points to zero */
for( i = 0; i < n; i++ )
{
if( !is_float )
{
offx += ((CvPoint*)reader.ptr)->x;
offy += ((CvPoint*)reader.ptr)->y;
}
else
{
offx += ((CvPoint2D32f*)reader.ptr)->x;
offy += ((CvPoint2D32f*)reader.ptr)->y;
}
CV_NEXT_SEQ_ELEM( points->elem_size, reader );
}
offx /= n;
offy /= n;
// fill matrix rows as (x*x, x*y, y*y, x, y, 1 )
for( i = 0; i < n; i++ )
{
double x, y;
double* Dptr = D->data.db + i*6;
if( !is_float )
{
x = ((CvPoint*)reader.ptr)->x - offx;
y = ((CvPoint*)reader.ptr)->y - offy;
}
else
{
x = ((CvPoint2D32f*)reader.ptr)->x - offx;
y = ((CvPoint2D32f*)reader.ptr)->y - offy;
}
CV_NEXT_SEQ_ELEM( points->elem_size, reader );
Dptr[0] = x * x;
Dptr[1] = x * y;
Dptr[2] = y * y;
Dptr[3] = x;
Dptr[4] = y;
Dptr[5] = 1.;
}
// S = D^t*D
cvMulTransposed( D, &_S, 1 );
cvSVD( &_S, &_EIGVALS, &_EIGVECS, 0, CV_SVD_MODIFY_A + CV_SVD_U_T );
for( i = 0; i < 6; i++ )
{
double a = eigenvalues[i];
a = a < DBL_EPSILON ? 0 : 1./sqrt(sqrt(a));
for( j = 0; j < 6; j++ )
eigenvectors[i*6 + j] *= a;
}
// C = Q^-1 = transp(INVEIGV) * INVEIGV
cvMulTransposed( &_EIGVECS, &_C, 1 );
cvZero( &_S );
S[2] = 2.;
S[7] = -1.;
S[12] = 2.;
// S = Q^-1*S*Q^-1
cvMatMul( &_C, &_S, &_T );
cvMatMul( &_T, &_C, &_S );
// and find its eigenvalues and vectors too
//cvSVD( &_S, &_EIGVALS, &_EIGVECS, 0, CV_SVD_MODIFY_A + CV_SVD_U_T );
cvEigenVV( &_S, &_EIGVECS, &_EIGVALS, 0 );
for( i = 0; i < 3; i++ )
if( eigenvalues[i] > 0 )
break;
if( i >= 3 /*eigenvalues[0] < DBL_EPSILON*/ )
{
box->center.x = box->center.y =
box->size.width = box->size.height =
box->angle = 0.f;
EXIT;
}
// now find truthful eigenvector
_EIGVECS = cvMat( 6, 1, CV_64F, eigenvectors + 6*i );
_T = cvMat( 6, 1, CV_64F, T );
// Q^-1*eigenvecs[0]
cvMatMul( &_C, &_EIGVECS, &_T );
// extract vector components
a = T[0]; b = T[1]; c = T[2]; d = T[3]; e = T[4]; f = T[5];
///////////////// extract ellipse axes from above values ////////////////
/*
1) find center of ellipse
it satisfy equation
| a b/2 | * | x0 | + | d/2 | = |0 |
| b/2 c | | y0 | | e/2 | |0 |
*/
idet = a * c - b * b * 0.25;
idet = idet > DBL_EPSILON ? 1./idet : 0;
// we must normalize (a b c d e f ) to fit (4ac-b^2=1)
scale = sqrt( 0.25 * idet );
if( scale < DBL_EPSILON )
{
box->center.x = (float)offx;
box->center.y = (float)offy;
box->size.width = box->size.height = box->angle = 0.f;
EXIT;
}
a *= scale;
b *= scale;
c *= scale;
d *= scale;
e *= scale;
f *= scale;
x0 = (-d * c + e * b * 0.5) * 2.;
y0 = (-a * e + d * b * 0.5) * 2.;
// recover center
box->center.x = (float)(x0 + offx);
box->center.y = (float)(y0 + offy);
// offset ellipse to (x0,y0)
// new f == F(x0,y0)
f += a * x0 * x0 + b * x0 * y0 + c * y0 * y0 + d * x0 + e * y0;
if( fabs(f) < DBL_EPSILON )
{
box->size.width = box->size.height = box->angle = 0.f;
EXIT;
}
scale = -1. / f;
// normalize to f = 1
a *= scale;
b *= scale;
c *= scale;
// extract axis of ellipse
// one more eigenvalue operation
S[0] = a;
S[1] = S[2] = b * 0.5;
S[3] = c;
_S = cvMat( 2, 2, CV_64F, S );
_EIGVECS = cvMat( 2, 2, CV_64F, eigenvectors );
_EIGVALS = cvMat( 1, 2, CV_64F, eigenvalues );
cvSVD( &_S, &_EIGVALS, &_EIGVECS, 0, CV_SVD_MODIFY_A + CV_SVD_U_T );
// exteract axis length from eigenvectors
box->size.width = (float)(2./sqrt(eigenvalues[0]));
box->size.height = (float)(2./sqrt(eigenvalues[1]));
// calc angle
box->angle = (float)(180 - atan2(eigenvectors[2], eigenvectors[3])*180/CV_PI);
__END__;
cvReleaseMat( &D );
}
CV_IMPL void
cvCalcSubdivVoronoi2D( CvSubdiv2D * subdiv )
{
CvSeqReader reader;
int i, total, elem_size;
CV_FUNCNAME( "cvCalcSubdivVoronoi2D" );
__BEGIN__;
if( !subdiv )
CV_ERROR( CV_StsNullPtr, "" );
/* check if it is already calculated */
if( subdiv->is_geometry_valid )
EXIT;
total = subdiv->edges->total;
elem_size = subdiv->edges->elem_size;
cvClearSubdivVoronoi2D( subdiv );
cvStartReadSeq( (CvSeq *) (subdiv->edges), &reader, 0 );
if( total <= 3 )
EXIT;
/* skip first three edges (bounding triangle) */
for( i = 0; i < 3; i++ )
CV_NEXT_SEQ_ELEM( elem_size, reader );
/* loop through all quad-edges */
for( ; i < total; i++ )
{
CvQuadEdge2D *quadedge = (CvQuadEdge2D *) (reader.ptr);
if( CV_IS_SET_ELEM( quadedge ))
{
CvSubdiv2DEdge edge0 = (CvSubdiv2DEdge) quadedge, edge1, edge2;
double a0, b0, c0, a1, b1, c1;
CvPoint2D32f virt_point;
CvSubdiv2DPoint *voronoi_point;
if( !quadedge->pt[3] )
{
edge1 = cvSubdiv2DGetEdge( edge0, CV_NEXT_AROUND_LEFT );
edge2 = cvSubdiv2DGetEdge( edge1, CV_NEXT_AROUND_LEFT );
icvCreateCenterNormalLine( edge0, &a0, &b0, &c0 );
icvCreateCenterNormalLine( edge1, &a1, &b1, &c1 );
icvIntersectLines3( &a0, &b0, &c0, &a1, &b1, &c1, &virt_point );
if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
fabs( virt_point.y ) < FLT_MAX * 0.5 )
{
voronoi_point = cvSubdiv2DAddPoint( subdiv, virt_point, 1 );
quadedge->pt[3] =
((CvQuadEdge2D *) (edge1 & ~3))->pt[3 - (edge1 & 2)] =
((CvQuadEdge2D *) (edge2 & ~3))->pt[3 - (edge2 & 2)] = voronoi_point;
}
}
if( !quadedge->pt[1] )
{
edge1 = cvSubdiv2DGetEdge( edge0, CV_NEXT_AROUND_RIGHT );
edge2 = cvSubdiv2DGetEdge( edge1, CV_NEXT_AROUND_RIGHT );
icvCreateCenterNormalLine( edge0, &a0, &b0, &c0 );
icvCreateCenterNormalLine( edge1, &a1, &b1, &c1 );
icvIntersectLines3( &a0, &b0, &c0, &a1, &b1, &c1, &virt_point );
if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
fabs( virt_point.y ) < FLT_MAX * 0.5 )
{
voronoi_point = cvSubdiv2DAddPoint( subdiv, virt_point, 1 );
quadedge->pt[1] =
((CvQuadEdge2D *) (edge1 & ~3))->pt[1 + (edge1 & 2)] =
((CvQuadEdge2D *) (edge2 & ~3))->pt[1 + (edge2 & 2)] = voronoi_point;
}
}
}
CV_NEXT_SEQ_ELEM( elem_size, reader );
}
subdiv->is_geometry_valid = 1;
__END__;
}
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;
}
/*!
Computes the SURF points in the current image I and try to matched
them with the points in the reference list. Only the matched points
are stored.
\param I : The gray scaled image where the points are computed.
\return the number of point which have been matched.
*/
unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I)
{
IplImage* currentImage = NULL;
if((I.getWidth() % 8) == 0){
int height = (int)I.getHeight();
int width = (int)I.getWidth();
CvSize size = cvSize(width, height);
currentImage = cvCreateImageHeader(size, IPL_DEPTH_8U, 1);
currentImage->imageData = (char*)I.bitmap;
}else{
vpImageConvert::convert(I,currentImage);
}
/* we release the memory storage for the current points (it has to be kept
allocated for the get descriptor points, ...) */
if(storage_cur != NULL){
cvReleaseMemStorage(&storage_cur);
storage_cur = NULL;
}
storage_cur = cvCreateMemStorage(0);
cvExtractSURF( currentImage, 0, &image_keypoints, &image_descriptors,
storage_cur, params );
CvSeqReader reader, kreader;
cvStartReadSeq( ref_keypoints, &kreader );
cvStartReadSeq( ref_descriptors, &reader );
std::list<int> indexImagePair;
std::list<int> indexReferencePair;
unsigned int nbrPair = 0;
for(int i = 0; i < ref_descriptors->total; i++ )
{
const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
const float* descriptor = (const float*)reader.ptr;
CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
int nearest_neighbor = naiveNearestNeighbor( descriptor,
kp->laplacian,
image_keypoints,
image_descriptors );
if( nearest_neighbor >= 0 )
{
indexReferencePair.push_back(i);
indexImagePair.push_back(nearest_neighbor);
nbrPair++;
}
}
std::list<int>::const_iterator indexImagePairIter = indexImagePair.begin();
std::list<int>::const_iterator indexReferencePairIter = indexReferencePair.begin();
matchedReferencePoints.resize(0);
if (nbrPair == 0)
return (0);
currentImagePointsList.resize(nbrPair);
matchedReferencePoints.resize(nbrPair);
for (unsigned int i = 0; i < nbrPair; i++)
{
int index = *indexImagePairIter;
CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem(image_keypoints, index);
currentImagePointsList[i].set_i(r1->pt.y);
currentImagePointsList[i].set_j(r1->pt.x);
matchedReferencePoints[i] = (unsigned int)*indexReferencePairIter;
++indexImagePairIter;
++indexReferencePairIter;
}
if((I.getWidth() % 8) == 0){
currentImage->imageData = NULL;
cvReleaseImageHeader(¤tImage);
}else{
cvReleaseImage(¤tImage);
}
return nbrPair;
}
