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; }