static float icvDistL2( const float *x, const float *y, void *user_param ) { int i, dims = (int)(size_t)user_param; double s = 0; for( i = 0; i < dims; i++ ) { double t = x[i] - y[i]; s += t * t; } return cvSqrt( (float)s ); }
std::pair< CvMat *, CvMat * > readkeys( const char * filename ) { int buffer_length = 2048; char buffer[buffer_length]; std::ifstream in_file( filename ); in_file.getline( buffer, buffer_length ); std::vector< std::string > tokens = tokenize_str( buffer, " " ); // parse header information int number_of_keypoints = lexical_cast< std::string, int >( tokens[0] ); int length = lexical_cast< std::string, int >( tokens[1] ); CvMat * key_vectors = cvCreateMat( number_of_keypoints, KEY_VECTORS_WIDTH, CV_32FC1 ); CvMat * key_descriptors = cvCreateMat( number_of_keypoints, KEY_DESCRIPTORS_WIDTH, CV_32FC1 ); // parse rest of file for( int j = 0; j < number_of_keypoints; j++ ) { // parse vectors in_file.getline( buffer, buffer_length ); tokens = tokenize_str( buffer, " " ); for( int i = 0; i < tokens.size(); i++ ) { cvmSet( key_vectors, j, i, lexical_cast< std::string, float >( tokens[i] ) ); } // parse descriptors float sum = 0; for( int k = 0; k < ceil( (float)length / DESCRIPTORS_PER_LINE ); k ++ ) { in_file.getline( buffer, buffer_length ); tokens = tokenize_str( buffer, " " ); for( int i = 0; i < tokens.size(); i++ ) sum += lexical_cast< std::string, float >( tokens[i] ) ; } float normalize = cvSqrt( pow( sum, 2 ) ); for( int i = 0; i < tokens.size(); i++ ) cvmSet( key_descriptors, j, i, lexical_cast< std::string, float >( tokens[i] ) / normalize ); } return std::make_pair( key_vectors, key_descriptors ); }
/****************************************************************************************\ triangle attributes calculations \****************************************************************************************/ static CvStatus icvCalcTriAttr( const CvSeq * contour, CvPoint t2, CvPoint t1, int n1, CvPoint t3, int n3, double *s, double *s_c, double *h, double *a, double *b ) { double x13, y13, x12, y12, l_base, nx, ny, qq; double eps = 1.e-5; x13 = t3.x - t1.x; y13 = t3.y - t1.y; x12 = t2.x - t1.x; y12 = t2.y - t1.y; qq = x13 * x13 + y13 * y13; l_base = cvSqrt( (float) (qq) ); if( l_base > eps ) { nx = y13 / l_base; ny = -x13 / l_base; *h = nx * x12 + ny * y12; *s = (*h) * l_base / 2.; *b = nx * y12 - ny * x12; *a = l_base; /* calculate interceptive area */ *s_c = cvContourArea( contour, cvSlice(n1, n3+1)); } else { *h = 0; *s = 0; *s_c = 0; *b = 0; *a = 0; } return CV_OK; }
void MyMat::RtfromH(const MyMat &invintrinsic, MyMat &R, MyMat &t) const { MyMat Rdt(3,3); Rdt.Mul(invintrinsic, (*this)); double scal = 0.0; for(int x=0;x<Rdt.m_data->rows;x++){ scal += Rdt(x,0)*Rdt(x,0); } scal = cvSqrt(scal); for(int y=0;y<Rdt.m_data->cols;y++){ for(int x=0;x<Rdt.m_data->rows;x++){ Rdt(x,y) /= scal; } } MyMat r1(3,1), r2(3,1), r1xr2(3,1); for(int x=0;x<3;x++){ r1(x,0) = Rdt(x,0); r2(x,0) = Rdt(x,1); } r1xr2.Cross(r1, r2); for(int y=0;y<2;y++){ for(int x=0;x<3;x++){ R(x,y) = Rdt(x,y); } } for(int x=0;x<3;x++){ R(x,2) = -r1xr2(x,0); t(x,0) = Rdt(x,2); } }
static CvStatus icvSnake8uC1R( unsigned char *src, int srcStep, CvSize roi, CvPoint * pt, int n, float *alpha, float *beta, float *gamma, int coeffUsage, CvSize win, CvTermCriteria criteria, int scheme ) { int i, j, k; int neighbors = win.height * win.width; int centerx = win.width >> 1; int centery = win.height >> 1; float invn; int iteration = 0; int converged = 0; float *Econt; float *Ecurv; float *Eimg; float *E; float _alpha, _beta, _gamma; /*#ifdef GRAD_SNAKE */ float *gradient = NULL; uchar *map = NULL; int map_width = ((roi.width - 1) >> 3) + 1; int map_height = ((roi.height - 1) >> 3) + 1; CvSepFilter pX, pY; #define WTILE_SIZE 8 #define TILE_SIZE (WTILE_SIZE + 2) short dx[TILE_SIZE*TILE_SIZE], dy[TILE_SIZE*TILE_SIZE]; CvMat _dx = cvMat( TILE_SIZE, TILE_SIZE, CV_16SC1, dx ); CvMat _dy = cvMat( TILE_SIZE, TILE_SIZE, CV_16SC1, dy ); CvMat _src = cvMat( roi.height, roi.width, CV_8UC1, src ); /* inner buffer of convolution process */ //char ConvBuffer[400]; /*#endif */ /* check bad arguments */ if( src == NULL ) return CV_NULLPTR_ERR; if( (roi.height <= 0) || (roi.width <= 0) ) return CV_BADSIZE_ERR; if( srcStep < roi.width ) return CV_BADSIZE_ERR; if( pt == NULL ) return CV_NULLPTR_ERR; if( n < 3 ) return CV_BADSIZE_ERR; if( alpha == NULL ) return CV_NULLPTR_ERR; if( beta == NULL ) return CV_NULLPTR_ERR; if( gamma == NULL ) return CV_NULLPTR_ERR; if( coeffUsage != CV_VALUE && coeffUsage != CV_ARRAY ) return CV_BADFLAG_ERR; if( (win.height <= 0) || (!(win.height & 1))) return CV_BADSIZE_ERR; if( (win.width <= 0) || (!(win.width & 1))) return CV_BADSIZE_ERR; invn = 1 / ((float) n); if( scheme == _CV_SNAKE_GRAD ) { pX.init_deriv( TILE_SIZE+2, CV_8UC1, CV_16SC1, 1, 0, 3 ); pY.init_deriv( TILE_SIZE+2, CV_8UC1, CV_16SC1, 0, 1, 3 ); gradient = (float *) cvAlloc( roi.height * roi.width * sizeof( float )); if( !gradient ) return CV_OUTOFMEM_ERR; map = (uchar *) cvAlloc( map_width * map_height ); if( !map ) { cvFree( &gradient ); return CV_OUTOFMEM_ERR; } /* clear map - no gradient computed */ memset( (void *) map, 0, map_width * map_height ); } Econt = (float *) cvAlloc( neighbors * sizeof( float )); Ecurv = (float *) cvAlloc( neighbors * sizeof( float )); Eimg = (float *) cvAlloc( neighbors * sizeof( float )); E = (float *) cvAlloc( neighbors * sizeof( float )); while( !converged ) { float ave_d = 0; int moved = 0; converged = 0; iteration++; /* compute average distance */ for( i = 1; i < n; i++ ) { int diffx = pt[i - 1].x - pt[i].x; int diffy = pt[i - 1].y - pt[i].y; ave_d += cvSqrt( (float) (diffx * diffx + diffy * diffy) ); } ave_d += cvSqrt( (float) ((pt[0].x - pt[n - 1].x) * (pt[0].x - pt[n - 1].x) + (pt[0].y - pt[n - 1].y) * (pt[0].y - pt[n - 1].y))); ave_d *= invn; /* average distance computed */ for( i = 0; i < n; i++ ) { /* Calculate Econt */ float maxEcont = 0; float maxEcurv = 0; float maxEimg = 0; float minEcont = _CV_SNAKE_BIG; float minEcurv = _CV_SNAKE_BIG; float minEimg = _CV_SNAKE_BIG; float Emin = _CV_SNAKE_BIG; int offsetx = 0; int offsety = 0; float tmp; /* compute bounds */ int left = MIN( pt[i].x, win.width >> 1 ); int right = MIN( roi.width - 1 - pt[i].x, win.width >> 1 ); int upper = MIN( pt[i].y, win.height >> 1 ); int bottom = MIN( roi.height - 1 - pt[i].y, win.height >> 1 ); maxEcont = 0; minEcont = _CV_SNAKE_BIG; for( j = -upper; j <= bottom; j++ ) { for( k = -left; k <= right; k++ ) { int diffx, diffy; float energy; if( i == 0 ) { diffx = pt[n - 1].x - (pt[i].x + k); diffy = pt[n - 1].y - (pt[i].y + j); } else { diffx = pt[i - 1].x - (pt[i].x + k); diffy = pt[i - 1].y - (pt[i].y + j); } Econt[(j + centery) * win.width + k + centerx] = energy = (float) fabs( ave_d - cvSqrt( (float) (diffx * diffx + diffy * diffy) )); maxEcont = MAX( maxEcont, energy ); minEcont = MIN( minEcont, energy ); } } tmp = maxEcont - minEcont; tmp = (tmp == 0) ? 0 : (1 / tmp); for( k = 0; k < neighbors; k++ ) { Econt[k] = (Econt[k] - minEcont) * tmp; } /* Calculate Ecurv */ maxEcurv = 0; minEcurv = _CV_SNAKE_BIG; for( j = -upper; j <= bottom; j++ ) { for( k = -left; k <= right; k++ ) { int tx, ty; float energy; if( i == 0 ) { tx = pt[n - 1].x - 2 * (pt[i].x + k) + pt[i + 1].x; ty = pt[n - 1].y - 2 * (pt[i].y + j) + pt[i + 1].y; } else if( i == n - 1 ) { tx = pt[i - 1].x - 2 * (pt[i].x + k) + pt[0].x; ty = pt[i - 1].y - 2 * (pt[i].y + j) + pt[0].y; } else { tx = pt[i - 1].x - 2 * (pt[i].x + k) + pt[i + 1].x; ty = pt[i - 1].y - 2 * (pt[i].y + j) + pt[i + 1].y; } Ecurv[(j + centery) * win.width + k + centerx] = energy = (float) (tx * tx + ty * ty); maxEcurv = MAX( maxEcurv, energy ); minEcurv = MIN( minEcurv, energy ); } } tmp = maxEcurv - minEcurv; tmp = (tmp == 0) ? 0 : (1 / tmp); for( k = 0; k < neighbors; k++ ) { Ecurv[k] = (Ecurv[k] - minEcurv) * tmp; } /* Calculate Eimg */ for( j = -upper; j <= bottom; j++ ) { for( k = -left; k <= right; k++ ) { float energy; if( scheme == _CV_SNAKE_GRAD ) { /* look at map and check status */ int x = (pt[i].x + k)/WTILE_SIZE; int y = (pt[i].y + j)/WTILE_SIZE; if( map[y * map_width + x] == 0 ) { int l, m; /* evaluate block location */ int upshift = y ? 1 : 0; int leftshift = x ? 1 : 0; int bottomshift = MIN( 1, roi.height - (y + 1)*WTILE_SIZE ); int rightshift = MIN( 1, roi.width - (x + 1)*WTILE_SIZE ); CvRect g_roi = { x*WTILE_SIZE - leftshift, y*WTILE_SIZE - upshift, leftshift + WTILE_SIZE + rightshift, upshift + WTILE_SIZE + bottomshift }; CvMat _src1; cvGetSubArr( &_src, &_src1, g_roi ); pX.process( &_src1, &_dx ); pY.process( &_src1, &_dy ); for( l = 0; l < WTILE_SIZE + bottomshift; l++ ) { for( m = 0; m < WTILE_SIZE + rightshift; m++ ) { gradient[(y*WTILE_SIZE + l) * roi.width + x*WTILE_SIZE + m] = (float) (dx[(l + upshift) * TILE_SIZE + m + leftshift] * dx[(l + upshift) * TILE_SIZE + m + leftshift] + dy[(l + upshift) * TILE_SIZE + m + leftshift] * dy[(l + upshift) * TILE_SIZE + m + leftshift]); } } map[y * map_width + x] = 1; } Eimg[(j + centery) * win.width + k + centerx] = energy = gradient[(pt[i].y + j) * roi.width + pt[i].x + k]; } else { Eimg[(j + centery) * win.width + k + centerx] = energy = src[(pt[i].y + j) * srcStep + pt[i].x + k]; } maxEimg = MAX( maxEimg, energy ); minEimg = MIN( minEimg, energy ); } } tmp = (maxEimg - minEimg); tmp = (tmp == 0) ? 0 : (1 / tmp); for( k = 0; k < neighbors; k++ ) { Eimg[k] = (minEimg - Eimg[k]) * tmp; } /* locate coefficients */ if( coeffUsage == CV_VALUE) { _alpha = *alpha; _beta = *beta; _gamma = *gamma; } else { _alpha = alpha[i]; _beta = beta[i]; _gamma = gamma[i]; } /* Find Minimize point in the neighbors */ for( k = 0; k < neighbors; k++ ) { E[k] = _alpha * Econt[k] + _beta * Ecurv[k] + _gamma * Eimg[k]; } Emin = _CV_SNAKE_BIG; for( j = -upper; j <= bottom; j++ ) { for( k = -left; k <= right; k++ ) { if( E[(j + centery) * win.width + k + centerx] < Emin ) { Emin = E[(j + centery) * win.width + k + centerx]; offsetx = k; offsety = j; } } } if( offsetx || offsety ) { pt[i].x += offsetx; pt[i].y += offsety; moved++; } } converged = (moved == 0); if( (criteria.type & CV_TERMCRIT_ITER) && (iteration >= criteria.max_iter) ) converged = 1; if( (criteria.type & CV_TERMCRIT_EPS) && (moved <= criteria.epsilon) ) converged = 1; } cvFree( &Econt ); cvFree( &Ecurv ); cvFree( &Eimg ); cvFree( &E ); if( scheme == _CV_SNAKE_GRAD ) { cvFree( &gradient ); cvFree( &map ); } return CV_OK; }
static CvStatus icvSnakeTrack8uC1R( unsigned char *src, int srcStep, CvSize roi, CvPoint * pt, int n, float *alpha, float *beta, float *gamma, int coeffUsage, CvSize win, CvTermCriteria criteria, int scheme , unsigned char *osrc, //original image where contours are generated. CvPoint *opt, //weights of the original fit float *oalpha, float *obeta, float *ogamma, float *oteta, float *ozeta, float *oomega, IplImage *dux, IplImage *odux, IplImage *duy, IplImage *oduy, float *oEarc_static = NULL, bool oEarc_static_ready = false, int *iterations = NULL) { int i, j, k; int neighbors = win.height * win.width; int centerx = win.width >> 1; int centery = win.height >> 1; float invn; int iteration = 0; int converged = 0; float *E; float _alpha=*alpha, _beta=*beta, _gamma=*gamma; /*#ifdef GRAD_SNAKE */ float *gradient = NULL; uchar *map = NULL; int map_width = ((roi.width - 1) >> 3) + 1; int map_height = ((roi.height - 1) >> 3) + 1; CvSepFilter pX, pY; #define WTILE_SIZE 8 #define TILE_SIZE (WTILE_SIZE + 2) short dx[TILE_SIZE*TILE_SIZE], dy[TILE_SIZE*TILE_SIZE]; CvMat _dx = cvMat( TILE_SIZE, TILE_SIZE, CV_16SC1, dx ); CvMat _dy = cvMat( TILE_SIZE, TILE_SIZE, CV_16SC1, dy ); CvMat _src = cvMat( roi.height, roi.width, CV_8UC1, src ); /* inner buffer of convolution process */ //char ConvBuffer[400]; /*#endif */ /* check bad arguments */ if( src == NULL ) return CV_NULLPTR_ERR; if( osrc == NULL ) return CV_NULLPTR_ERR; if( (roi.height <= 0) || (roi.width <= 0) ) return CV_BADSIZE_ERR; if( srcStep < roi.width ) return CV_BADSIZE_ERR; if( pt == NULL ) return CV_NULLPTR_ERR; if( n < 3 ) return CV_BADSIZE_ERR; if( alpha == NULL ) return CV_NULLPTR_ERR; if( beta == NULL ) return CV_NULLPTR_ERR; if( gamma == NULL ) return CV_NULLPTR_ERR; if( oalpha == NULL ) return CV_NULLPTR_ERR; if( obeta == NULL ) return CV_NULLPTR_ERR; if( ogamma == NULL ) return CV_NULLPTR_ERR; if( oteta == NULL ) return CV_NULLPTR_ERR; if( ozeta == NULL ) return CV_NULLPTR_ERR; if( coeffUsage != CV_VALUE && coeffUsage != CV_ARRAY ) return CV_BADFLAG_ERR; if( (win.height <= 0) || (!(win.height & 1))) return CV_BADSIZE_ERR; if( (win.width <= 0) || (!(win.width & 1))) return CV_BADSIZE_ERR; invn = 1 / ((float) n); if( scheme == _CV_SNAKE_GRAD ) { pX.init_deriv( TILE_SIZE+2, CV_8UC1, CV_16SC1, 1, 0, 3 ); pY.init_deriv( TILE_SIZE+2, CV_8UC1, CV_16SC1, 0, 1, 3 ); gradient = (float *) cvAlloc( roi.height * roi.width * sizeof( float )); memset((void*)gradient, 0, roi.height*roi.width*sizeof(float)); if( !gradient ) return CV_OUTOFMEM_ERR; map = (uchar *) cvAlloc( map_width * map_height ); if( !map ) { cvFree( &gradient ); return CV_OUTOFMEM_ERR; } /* clear map - no gradient computed */ memset( (void *) map, 0, map_width * map_height ); } E = (float *) cvAlloc( neighbors * sizeof( float )); //------------------------- Original Image Parameters ---------------- float _oalpha=*oalpha, _obeta=*obeta, _ogamma=*ogamma, _oteta=*oteta, _ozeta=*ozeta, _oomega=*oomega; CvMat _osrc = cvMat( roi.height, roi.width, CV_8UC1, osrc ); int roisize=roi.width*roi.width; #define INIT_ENERGY_TERM(X) \ float *X = (float *) cvAlloc( neighbors * sizeof( float )); \ float *o##X = (float *) cvAlloc( n * sizeof( float )); \ float *os##X = (float *) cvAlloc( n * sizeof( float )); \ memset(X, 0, neighbors * sizeof(float)); \ memset(o##X, 0, n * sizeof(float)); \ memset(os##X, 0, n * sizeof(float)); INIT_ENERGY_TERM(Econt) INIT_ENERGY_TERM(Ecurv) INIT_ENERGY_TERM(Eimg) INIT_ENERGY_TERM(Eint) INIT_ENERGY_TERM(Earc) INIT_ENERGY_TERM(Edux) INIT_ENERGY_TERM(Eduy) //------------------------ //End Original Image Parameters CvMat *_srcp; unsigned char *srcp; CvPoint *ppt; IplImage *pdux, *pduy; for (int isoriginit=0; isoriginit<2; isoriginit++){ if (!isoriginit){ ppt=opt; _srcp=&_osrc; srcp=osrc; pdux = odux; pduy = oduy; } else{ ppt=pt; _srcp=&_src; srcp=src; converged=0; iteration=0; if( scheme == _CV_SNAKE_GRAD ) memset( (void *) map, 0, map_width * map_height ); pdux = dux; pduy = duy; } while( !converged ) { float ave_d = 0; int moved = 0; converged = 0; iteration++; /* compute average distance */ for( i = 1; i < n; i++ ) { int diffx = ppt[i - 1].x - ppt[i].x; int diffy = ppt[i - 1].y - ppt[i].y; ave_d += cvSqrt( (float) (diffx * diffx + diffy * diffy) ); } ave_d += cvSqrt( (float) ((ppt[0].x - ppt[n - 1].x) * (ppt[0].x - ppt[n - 1].x) + (ppt[0].y - ppt[n - 1].y) * (ppt[0].y - ppt[n - 1].y))); ave_d *= invn; /* average distance computed */ for( i = 0; i < n; i++ ) { float Emin = _CV_SNAKE_BIG; float maxEcont = 0, minEcont = _CV_SNAKE_BIG; float maxEcurv = 0, minEcurv = _CV_SNAKE_BIG; float maxEimg = 0, minEimg = _CV_SNAKE_BIG; float maxEint = 0, minEint = _CV_SNAKE_BIG; float maxEarc = 0, minEarc = _CV_SNAKE_BIG; float maxEdux = 0, minEdux = _CV_SNAKE_BIG; float maxEduy = 0, minEduy = _CV_SNAKE_BIG; int offsetx = 0; int offsety = 0; float tmp, energy; int diffx, diffy; // compute bounds int left = MIN( ppt[i].x, win.width >> 1 ); int right = MIN( roi.width - 1 - ppt[i].x, win.width >> 1 ); int upper = MIN( ppt[i].y, win.height >> 1 ); int bottom = MIN( roi.height - 1 - ppt[i].y, win.height >> 1 ); // Calculate Econt if (!(alpha==0 || oalpha==0)){ for( j = -upper; j <= bottom; j++ ) { for( k = -left; k <= right; k++ ) { if( i == 0 ) { diffx = ppt[n - 1].x - (ppt[i].x + k); diffy = ppt[n - 1].y - (ppt[i].y + j); } else { diffx = ppt[i - 1].x - (ppt[i].x + k); diffy = ppt[i - 1].y - (ppt[i].y + j); } Econt[(j + centery) * win.width + k + centerx] = energy = (float) fabs( ave_d - cvSqrt( (float) (diffx * diffx + diffy * diffy) )); maxEcont = MAX( maxEcont, energy ); minEcont = MIN( minEcont, energy ); } } if(isoriginit){ tmp = maxEcont - minEcont; tmp = (tmp == 0) ? 0 : (1 / tmp); for( k = 0; k < neighbors; k++ ) Econt[k] = (Econt[k] - minEcont) * tmp; osEcont[i] = (oEcont[i] - minEcont)*tmp; } } // Calculate Ecurv if (!(beta==0 || obeta==0)) { for( j = -upper; j <= bottom; j++ ) { for( k = -left; k <= right; k++ ) { int tx, ty; if( i == 0 ) { tx = ppt[n - 1].x - 2 * (ppt[i].x + k) + ppt[i + 1].x; ty = ppt[n - 1].y - 2 * (ppt[i].y + j) + ppt[i + 1].y; } else if( i == n - 1 ) { tx = ppt[i - 1].x - 2 * (ppt[i].x + k) + ppt[0].x; ty = ppt[i - 1].y - 2 * (ppt[i].y + j) + ppt[0].y; } else { tx = ppt[i - 1].x - 2 * (ppt[i].x + k) + ppt[i + 1].x; ty = ppt[i - 1].y - 2 * (ppt[i].y + j) + ppt[i + 1].y; } Ecurv[(j + centery) * win.width + k + centerx] = energy = (float) (tx * tx + ty * ty); maxEcurv = MAX( maxEcurv, energy ); minEcurv = MIN( minEcurv, energy ); } } if(isoriginit){ tmp = maxEcurv - minEcurv; tmp = (tmp == 0) ? 0 : (1 / tmp); for( k = 0; k < neighbors; k++ ) Ecurv[k] = (Ecurv[k] - minEcurv) * tmp; osEcurv[i] = (oEcurv[i] - minEcurv)*tmp; } } // Calculate Eimg if (!(gamma==0 || ogamma==0)) { for( j = -upper; j <= bottom; j++ ) { for( k = -left; k <= right; k++ ) { if( scheme == _CV_SNAKE_GRAD ) { // look at map and check status int x = (ppt[i].x + k)/WTILE_SIZE; int y = (ppt[i].y + j)/WTILE_SIZE; if( map[y * map_width + x] == 0 ) { int l, m; // evaluate block location int upshift = y ? 1 : 0; int leftshift = x ? 1 : 0; int bottomshift = MIN( 1, roi.height - (y + 1)*WTILE_SIZE ); int rightshift = MIN( 1, roi.width - (x + 1)*WTILE_SIZE ); CvRect g_roi = { x*WTILE_SIZE - leftshift, y*WTILE_SIZE - upshift, leftshift + WTILE_SIZE + rightshift, upshift + WTILE_SIZE + bottomshift }; CvMat _src1; cvGetSubArr( _srcp, &_src1, g_roi ); pX.process( &_src1, &_dx ); pY.process( &_src1, &_dy ); for( l = 0; l < WTILE_SIZE + bottomshift; l++ ) { for( m = 0; m < WTILE_SIZE + rightshift; m++ ) { gradient[(y*WTILE_SIZE + l) * roi.width + x*WTILE_SIZE + m] = (float) (dx[(l + upshift) * TILE_SIZE + m + leftshift] * dx[(l + upshift) * TILE_SIZE + m + leftshift] + dy[(l + upshift) * TILE_SIZE + m + leftshift] * dy[(l + upshift) * TILE_SIZE + m + leftshift]); } } map[y * map_width + x] = 1; } Eimg[(j + centery) * win.width + k + centerx] = energy = gradient[(ppt[i].y + j) * roi.width + ppt[i].x + k]; } else Eimg[(j + centery) * win.width + k + centerx] = energy = srcp[(ppt[i].y + j) * srcStep + ppt[i].x + k]; maxEimg = MAX( maxEimg, energy ); minEimg = MIN( minEimg, energy ); } } if(isoriginit){ tmp = (maxEimg - minEimg); tmp = (tmp == 0) ? 0 : (1 / tmp); for( k = 0; k < neighbors; k++ ) Eimg[k] = (minEimg - Eimg[k]) * tmp; osEimg[i] = (minEimg - oEimg[i])*tmp; } } // Calculate Eint if (oteta!=0) { for( j = -upper; j <= bottom; j++ ) { for( k = -left; k <= right; k++ ) { Eint[(j + centery) * win.width + k + centerx] = energy = srcp[(ppt[i].y + j) * srcStep + ppt[i].x + k]; maxEint = MAX( maxEint, energy ); minEint = MIN( minEint, energy ); } } if (isoriginit){ tmp = maxEint - minEint; tmp = (tmp == 0) ? 0 : (1 / tmp); for( k = 0; k < neighbors; k++ ) Eint[k] = (Eint[k] - minEint) * tmp; osEint[i] = (oEint[i] - minEint)*tmp; } } // Calculate Earc if (ozeta!=0) { for( j = -upper; j <= bottom; j++ ) { for( k = -left; k <= right; k++ ) { int diff2x,diff2y; if( i == 0 ) { diffx = opt[n - 1].x - (ppt[i].x + k); diffy = opt[n - 1].y - (ppt[i].y + j); } else { diffx = opt[i - 1].x - (ppt[i].x + k); diffy = opt[i - 1].y - (ppt[i].y + j); } if( i == n-1 ) { diff2x = opt[0].x - (ppt[i].x + k); diff2y = opt[0].y - (ppt[i].y + j); } else { diff2x = opt[i + 1].x - (ppt[i].x + k); diff2y = opt[i + 1].y - (ppt[i].y + j); } Earc[(j + centery) * win.width + k + centerx] = energy = cvSqrt( (float) (diffx * diffx + diffy * diffy) ) + cvSqrt( (float) (diff2x * diff2x + diff2y * diff2y) ); maxEarc = MAX( maxEarc, energy ); minEarc = MIN( minEarc, energy ); } } if (isoriginit){ tmp = maxEarc - minEarc; tmp = (tmp == 0) ? 0 : (1 / tmp); for( k = 0; k < neighbors; k++ ) Earc[k] = (Earc[k] - minEarc) * tmp; osEarc[i] = (oEarc[i] - minEarc)*tmp; } } // Calculate Edu-x/y gradients if (oomega!=0) { for( j = -upper; j <= bottom; j++ ) { for( k = -left; k <= right; k++ ) { float energyy; Edux[(j + centery) * win.width + k + centerx] = energy = (float)pdux->imageData[(ppt[i].y + j) * pdux->widthStep + ppt[i].x + k]; Eduy[(j + centery) * win.width + k + centerx] = energyy = (float)pduy->imageData[(ppt[i].y + j) * pduy->widthStep + ppt[i].x + k]; maxEdux = MAX( maxEdux, energy ); minEdux = MIN( minEdux, energy ); maxEduy = MAX( maxEduy, energyy ); minEduy = MIN( minEduy, energyy ); } } if (isoriginit){ tmp = maxEdux - minEdux; tmp = (tmp == 0) ? 0 : (1 / tmp); for( k = 0; k < neighbors; k++ ) Edux[k] = (Edux[k] - minEdux) * tmp; osEdux[i] = (oEdux[i] - minEdux)*tmp; tmp = maxEduy - minEduy; tmp = (tmp == 0) ? 0 : (1 / tmp); for( k = 0; k < neighbors; k++ ) Eduy[k] = (Eduy[k] - minEduy) * tmp; osEduy[i] = (oEduy[i] - minEduy)*tmp; } } //---------------------- //end E calculations. int loci=centery*win.width+centerx; if (!isoriginit){ //Initialize Original image parameters oEcont[i] = Econt[loci]; oEcurv[i] = Ecurv[loci]; oEimg[i] = Eimg[loci]; oEint[i] = Eint[loci]; if (oEarc_static){ if (oEarc_static_ready) oEarc[i] = oEarc_static[i]; else oEarc[i] = oEarc_static[i] = Earc[loci]; } else oEarc[i] = Earc[loci]; oEdux[i] = Edux[loci]; oEduy[i] = Eduy[loci]; } else{ /* locate coefficients */ if( coeffUsage != CV_VALUE) { _alpha = alpha[i]; _beta = beta[i]; _gamma = gamma[i]; _oalpha = oalpha[i]; _obeta = obeta[i]; _ogamma = ogamma[i]; _oteta = oteta[i]; _ozeta = ozeta[i]; _oomega = oomega[i]; } /* Find Minimize point in the neighbors */ for( k = 0; k < neighbors; k++ ) { E[k] = _alpha * Econt[k] + _beta * Ecurv[k] + _gamma * Eimg[k] + _oalpha*fabs(Econt[k]-osEcont[i]) + _obeta*fabs(Ecurv[k]-osEcurv[i]) + _ogamma*fabs(Eimg[k]-osEimg[i]) + _oteta*fabs(Eint[k]-osEint[i]) + _ozeta*fabs(Earc[k]-osEarc[i]) + _oomega*(fabs(Edux[k]-osEdux[i])+fabs(Eduy[k]-osEduy[i])) ; } // Emin = _CV_SNAKE_BIG; Emin = E[loci]; for( j = -upper; j <= bottom; j++ ) { for( k = -left; k <= right; k++ ) { if( E[(j + centery) * win.width + k + centerx] < Emin )// || (E[(j + centery) * win.width + k + centerx] == Emin && rand()%2)) { Emin = E[(j + centery) * win.width + k + centerx]; offsetx = k; offsety = j; } } } if( offsetx || offsety ) { ppt[i].x += offsetx; ppt[i].y += offsety; moved++; } }//originit /* //Debugging: display gradient image float maxgrad=0; float mingrad=_CV_SNAKE_BIG; for(j=0; j<roi.width; j++){ for(k=0; k<roi.height; k++){ float grad=gradient[k*roi.width+j]; if(grad<mingrad) mingrad=grad; if(grad>maxgrad) maxgrad=grad; } } IplImage *gray = cvCreateImage( cvSize(roi.width, roi.height), IPL_DEPTH_8U, 1 ); tmp = (maxEimg - minEimg); tmp = (tmp == 0) ? 0 : (1 / tmp); for(j=0; j<roi.width; j++){ for(k=0; k<roi.height; k++){ float grad=gradient[k*roi.width+j]; cvSetReal2D(gray,k,j,((grad-mingrad)*tmp*255)); } } cvNamedWindow("tmp"); cvShowImage("tmp",gray); cvReleaseImage(&gray); cvWaitKey(100); */ }//for n converged = (moved == 0); if( (criteria.type & CV_TERMCRIT_ITER) && (iteration >= criteria.max_iter) ) converged = 1; if( (criteria.type & CV_TERMCRIT_EPS) && (moved <= criteria.epsilon) ) converged = 1; }//while !converged }//for isoriginit cvFree( &E ); cvFree( &Econt ); cvFree( &oEcont ); cvFree( &osEcont ); cvFree( &Ecurv ); cvFree( &oEcurv ); cvFree( &osEcurv ); cvFree( &Eimg ); cvFree( &oEimg ); cvFree( &osEimg ); cvFree( &Eint ); cvFree( &oEint ); cvFree( &osEint ); cvFree( &Earc ); cvFree( &oEarc ); cvFree( &osEarc ); cvFree( &Edux ); cvFree( &oEdux ); cvFree( &osEdux ); cvFree( &Eduy ); cvFree( &oEduy ); cvFree( &osEduy ); if( scheme == _CV_SNAKE_GRAD ) { cvFree( &gradient ); cvFree( &map ); } if(iterations) *iterations = iteration; return CV_OK; }
// // Transform // Transform the sample 'in place' // HRESULT CKalmTrack::Transform(IMediaSample *pSample) { BYTE* pData; CvImage image; pSample->GetPointer(&pData); AM_MEDIA_TYPE* pType = &m_pInput->CurrentMediaType(); VIDEOINFOHEADER *pvi = (VIDEOINFOHEADER *) pType->pbFormat; // Get the image properties from the BITMAPINFOHEADER CvSize size = cvSize( pvi->bmiHeader.biWidth, pvi->bmiHeader.biHeight ); int stride = (size.width * 3 + 3) & -4; cvInitImageHeader( &image, size, IPL_DEPTH_8U, 3, IPL_ORIGIN_TL, 4, 1 ); cvSetImageData( &image, pData,stride ); if(IsTracking == false) { if(IsInit == false) { CvPoint p1, p2; // Draw box p1.x = cvRound( size.width * m_params.x ); p1.y = cvRound( size.height * m_params.y ); p2.x = cvRound( size.width * (m_params.x + m_params.width)); p2.y = cvRound( size.height * (m_params.y + m_params.height)); CheckBackProject( &image ); cvRectangle( &image, p1, p2, -1, 1 ); } else { m_object.x = cvRound( size.width * m_params.x ); m_object.y = cvRound( size.height * m_params.y ); m_object.width = cvRound( size.width * m_params.width ); m_object.height = cvRound( size.height * m_params.height ); ApplyCamShift( &image, true ); CheckBackProject( &image ); IsTracking = true; } } else { cvKalmanUpdateByTime(Kalman); m_object.x = cvRound( Kalman->PriorState[0]-m_object.width*0.5); m_object.y = cvRound( Kalman->PriorState[2]-m_object.height*0.5 ); ApplyCamShift( &image, false ); CheckBackProject( &image ); cvRectangle( &image, cvPoint( m_object.x, m_object.y ), cvPoint( m_object.x + m_object.width, m_object.y + m_object.height ), -1, 1 ); Rectang(&image,m_Indicat1,-1); m_X.x = 10; m_X.y = 10; m_X.width=50*m_Old.x/size.width; m_X.height =10; Rectang(&image,m_X,CV_RGB(0,0,255)); m_Y.x = 10; m_Y.y = 10; m_Y.width=10; m_Y.height = 50*m_Old.y/size.height; Rectang(&image,m_Y,CV_RGB(255,0,0)); m_Indicat2.x = 0; m_Indicat2.y = size.height-50; m_Indicat2.width = 50; m_Indicat2.height = 50; Rectang(&image,m_Indicat2,-1); float Norm = cvSqrt(Measurement[1]*Measurement[1]+Measurement[3]*Measurement[3]); int VXNorm = (fabs(Measurement[1])>5)?(int)(12*Measurement[1]/Norm):0; int VYNorm = (fabs(Measurement[3])>5)?(int)(12*Measurement[3]/Norm):0; CvPoint pp1 = {25,size.height-25}; CvPoint pp2 = {25+VXNorm,size.height-25+VYNorm}; cvLine(&image,pp1,pp2,CV_RGB(0,0,0),3); /*CvPoint pp1 = {25,size.height-25}; double angle = atan2( Measurement[3], Measurement[1] ); CvPoint pp2 = {cvRound(25+12*cos(angle)),cvRound(size.height-25-12*sin(angle))}; cvLine(&image,pp1,pp2,0,3);*/ } cvSetImageData( &image, 0, 0 ); return NOERROR; } // Transform
CV_IMPL void cvCalcImageHomography( float* line, CvPoint3D32f* _center, float* _intrinsic, float* _homography ) { double norm_xy, norm_xz, xy_sina, xy_cosa, xz_sina, xz_cosa, nx1, plane_dist; float _ry[3], _rz[3], _r_trans[9]; CvMat rx = cvMat( 1, 3, CV_32F, line ); CvMat ry = cvMat( 1, 3, CV_32F, _ry ); CvMat rz = cvMat( 1, 3, CV_32F, _rz ); CvMat r_trans = cvMat( 3, 3, CV_32F, _r_trans ); CvMat center = cvMat( 3, 1, CV_32F, _center ); float _sub[9]; CvMat sub = cvMat( 3, 3, CV_32F, _sub ); float _t_trans[3]; CvMat t_trans = cvMat( 3, 1, CV_32F, _t_trans ); CvMat intrinsic = cvMat( 3, 3, CV_32F, _intrinsic ); CvMat homography = cvMat( 3, 3, CV_32F, _homography ); if( !line || !_center || !_intrinsic || !_homography ) CV_Error( CV_StsNullPtr, "" ); norm_xy = cvSqrt( line[0] * line[0] + line[1] * line[1] ); xy_cosa = line[0] / norm_xy; xy_sina = line[1] / norm_xy; norm_xz = cvSqrt( line[0] * line[0] + line[2] * line[2] ); xz_cosa = line[0] / norm_xz; xz_sina = line[2] / norm_xz; nx1 = -xz_sina; _rz[0] = (float)(xy_cosa * nx1); _rz[1] = (float)(xy_sina * nx1); _rz[2] = (float)xz_cosa; cvScale( &rz, &rz, 1./cvNorm(&rz,0,CV_L2) ); /* new axe y */ cvCrossProduct( &rz, &rx, &ry ); cvScale( &ry, &ry, 1./cvNorm( &ry, 0, CV_L2 ) ); /* transpone rotation matrix */ memcpy( &_r_trans[0], line, 3*sizeof(float)); memcpy( &_r_trans[3], _ry, 3*sizeof(float)); memcpy( &_r_trans[6], _rz, 3*sizeof(float)); /* calculate center distanse from arm plane */ plane_dist = cvDotProduct( ¢er, &rz ); /* calculate (I - r_trans)*center */ cvSetIdentity( &sub ); cvSub( &sub, &r_trans, &sub ); cvMatMul( &sub, ¢er, &t_trans ); cvMatMul( &t_trans, &rz, &sub ); cvScaleAdd( &sub, cvRealScalar(1./plane_dist), &r_trans, &sub ); /* ? */ cvMatMul( &intrinsic, &sub, &r_trans ); cvInvert( &intrinsic, &sub, CV_SVD ); cvMatMul( &r_trans, &sub, &homography ); }
static int aGestureRecognition(void) { IplImage *image, *imagew, *image_rez, *mask_rez, *image_hsv, *img_p[2],*img_v, *init_mask_ver = 0, *final_mask_ver = 0; CvPoint3D32f *pp, p; CvPoint pt; CvSize2D32f fsize; CvPoint3D32f center, cf; IplImage *image_mask, *image_maskw; CvSize size; CvHistogram *hist, *hist_mask; int width, height; int k_points, k_indexs; int warpFlag, interpolate; int hdim[2] = {20, 20}; double coeffs[3][3], rect[2][2], rez = 0, eps_rez = 2.5, rez_h; float *thresh[2]; float hv[3]; float reps, aeps, ww; float line[6], in[3][3], h[3][3]; float cx, cy, fx, fy; static char num[4]; char *name_image; char *name_range_image; char *name_verify_data; char *name_init_mask_very; char *name_final_mask_very; CvSeq *numbers; CvSeq *points; CvSeq *indexs; CvMemStorage *storage; CvRect hand_roi, hand_roi_trans; int i,j, lsize, block_size = 1000, flag; int code; FILE *filin, *fil_ver; /* read tests params */ code = TRS_OK; /* define input information */ strcpy (num, "001"); lsize = strlen(data_path)+12; name_verify_data = (char*)trsmAlloc(lsize); name_range_image = (char*)trsmAlloc(lsize); name_image = (char*)trsmAlloc(lsize); name_init_mask_very = (char*)trsmAlloc(lsize); name_final_mask_very = (char*)trsmAlloc(lsize); /* define input range_image file path */ strcpy(name_range_image, data_path); strcat(name_range_image, "rpts"); strcat(name_range_image, num); strcat(name_range_image, ".txt"); /* define input image file path */ strcpy(name_image, data_path); strcat(name_image, "real"); strcat(name_image, num); strcat(name_image, ".bmp"); /* define verify data file path */ strcpy(name_verify_data, data_path); strcat(name_verify_data, "very"); strcat(name_verify_data, num); strcat(name_verify_data, ".txt"); /* define verify init mask file path */ strcpy(name_init_mask_very, data_path); strcat(name_init_mask_very, "imas"); strcat(name_init_mask_very, num); strcat(name_init_mask_very, ".bmp"); /* define verify final mask file path */ strcpy(name_final_mask_very, data_path); strcat(name_final_mask_very, "fmas"); strcat(name_final_mask_very, num); strcat(name_final_mask_very, ".bmp"); filin = fopen(name_range_image,"r"); fil_ver = fopen(name_verify_data,"r"); fscanf( filin, "\n%d %d\n", &width, &height); printf("width=%d height=%d reading testing data...", width,height); OPENCV_CALL( storage = cvCreateMemStorage ( block_size ) ); OPENCV_CALL( points = cvCreateSeq( CV_SEQ_POINT3D_SET, sizeof(CvSeq), sizeof(CvPoint3D32f), storage ) ); OPENCV_CALL (indexs = cvCreateSeq( CV_SEQ_POINT_SET, sizeof(CvSeq), sizeof(CvPoint), storage ) ); pp = 0; /* read input image from file */ image = atsCreateImageFromFile( name_image ); if(image == NULL) {code = TRS_FAIL; goto m_exit;} /* read input 3D points from input file */ for (i = 0; i < height; i++) { for (j = 0; j < width; j++) { fscanf( filin, "%f %f %f\n", &p.x, &p.y, &p.z); if(/*p.x != 0 || p.y != 0 ||*/ p.z != 0) { OPENCV_CALL(cvSeqPush(points, &p)); pt.x = j; pt.y = i; OPENCV_CALL(cvSeqPush(indexs, &pt)); } } } k_points = points->total; k_indexs = indexs->total; /* convert sequence to array */ pp = (CvPoint3D32f*)trsmAlloc(k_points * sizeof(CvPoint3D32f)); OPENCV_CALL(cvCvtSeqToArray(points, pp )); /* find 3D-line */ reps = (float)0.1; aeps = (float)0.1; ww = (float)0.08; OPENCV_CALL( cvFitLine3D(pp, k_points, CV_DIST_WELSCH, &ww, reps, aeps, line )); /* find hand location */ flag = -1; fsize.width = fsize.height = (float)0.22; // (hand size in m) numbers = NULL; OPENCV_CALL( cvFindHandRegion (pp, k_points, indexs,line, fsize, flag,¢er,storage, &numbers)); /* read verify data */ fscanf( fil_ver, "%f %f %f\n", &cf.x, &cf.y, &cf.z); rez+= cvSqrt((center.x - cf.x)*(center.x - cf.x)+(center.y - cf.y)*(center.y - cf.y)+ (center.z - cf.z)*(center.z - cf.z))/3.; /* create hand mask */ size.height = height; size.width = width; OPENCV_CALL( image_mask = cvCreateImage(size, IPL_DEPTH_8U, 1) ); OPENCV_CALL( cvCreateHandMask(numbers, image_mask, &hand_roi) ); /* read verify initial image mask */ init_mask_ver = atsCreateImageFromFile( name_init_mask_very ); if(init_mask_ver == NULL) {code = TRS_FAIL; goto m_exit;} rez+= iplNorm(init_mask_ver, image_mask, IPL_L2) / (width*height+0.); /* calculate homographic transformation matrix */ cx = (float)(width / 2.); cy = (float)(height / 2.); fx = fy = (float)571.2048; /* define intrinsic camera parameters */ in[0][1] = in[1][0] = in[2][0] = in[2][1] = 0; in[0][0] = fx; in[0][2] = cx; in[1][1] = fy; in[1][2] = cy; in[2][2] = 1; OPENCV_CALL( cvCalcImageHomography(line, ¢er, in, h) ); rez_h = 0; for(i=0;i<3;i++) { fscanf( fil_ver, "%f %f %f\n", &hv[0], &hv[1], &hv[2]); for(j=0;j<3;j++) { rez_h+=(hv[j] - h[i][j])*(hv[j] - h[i][j]); } } rez+=sqrt(rez_h)/9.; /* image unwarping */ size.width = image->width; size.height = image->height; OPENCV_CALL( imagew = cvCreateImage(size, IPL_DEPTH_8U,3) ); OPENCV_CALL( image_maskw = cvCreateImage(size, IPL_DEPTH_8U,1) ); iplSet(image_maskw, 0); cvSetImageROI(image, hand_roi); cvSetImageROI(image_mask, hand_roi); /* convert homographic transformation matrix from float to double */ for(i=0;i<3;i++) for(j=0;j<3;j++) coeffs[i][j] = (double)h[i][j]; /* get bounding rectangle for image ROI */ iplGetPerspectiveBound(image, coeffs, rect); width = (int)(rect[1][0] - rect[0][0]); height = (int)(rect[1][1] - rect[0][1]); hand_roi_trans.x = (int)rect[0][0];hand_roi_trans.y = (int)rect[0][1]; hand_roi_trans.width = width; hand_roi_trans.height = height; cvMaxRect(&hand_roi, &hand_roi_trans, &hand_roi); iplSetROI((IplROI*)image->roi, 0, hand_roi.x, hand_roi.y, hand_roi.width,hand_roi.height); iplSetROI((IplROI*)image_mask->roi, 0, hand_roi.x, hand_roi.y, hand_roi.width,hand_roi.height); warpFlag = IPL_WARP_R_TO_Q; /* interpolate = IPL_INTER_CUBIC; */ /* interpolate = IPL_INTER_NN; */ interpolate = IPL_INTER_LINEAR; iplWarpPerspective(image, imagew, coeffs, warpFlag, interpolate); iplWarpPerspective(image_mask, image_maskw, coeffs, warpFlag, IPL_INTER_NN); /* set new image and mask ROI after transformation */ iplSetROI((IplROI*)imagew->roi,0, (int)rect[0][0], (int)rect[0][1],(int)width,(int)height); iplSetROI((IplROI*)image_maskw->roi,0, (int)rect[0][0], (int)rect[0][1],(int)width,(int)height); /* copy image ROI to new image and resize */ size.width = width; size.height = height; image_rez = cvCreateImage(size, IPL_DEPTH_8U,3); mask_rez = cvCreateImage(size, IPL_DEPTH_8U,1); iplCopy(imagew,image_rez); iplCopy(image_maskw,mask_rez); /* convert rezult image from RGB to HSV */ image_hsv = iplCreateImageHeader(3, 0, IPL_DEPTH_8U, "HSV", "HSV", IPL_DATA_ORDER_PIXEL, IPL_ORIGIN_TL,IPL_ALIGN_DWORD, image_rez->width, image_rez->height, NULL, NULL, NULL, NULL); iplAllocateImage(image_hsv, 0, 0 ); strcpy(image_rez->colorModel, "RGB"); strcpy(image_rez->channelSeq, "RGB"); image_rez->roi = NULL; iplRGB2HSV(image_rez, image_hsv); /* convert to three images planes */ img_p[0] = cvCreateImage(size, IPL_DEPTH_8U,1); img_p[1] = cvCreateImage(size, IPL_DEPTH_8U,1); img_v = cvCreateImage(size, IPL_DEPTH_8U,1); cvCvtPixToPlane(image_hsv, img_p[0], img_p[1], img_v, NULL); /* calculate histograms */ hist = cvCreateHist ( 2, hdim, CV_HIST_ARRAY); hist_mask = cvCreateHist ( 2, hdim, CV_HIST_ARRAY); /* install histogram threshold */ thresh[0] = (float*) trsmAlloc(2*sizeof(float)); thresh[1] = (float*) trsmAlloc(2*sizeof(float)); thresh[0][0] = thresh[1][0] = -0.5; thresh[0][1] = thresh[1][1] = 255.5; cvSetHistThresh( hist, thresh, 1); cvSetHistThresh( hist_mask, thresh, 1); cvCalcHist(img_p, hist, 0); cvCalcHistMask(img_p, mask_rez, hist_mask, 0); cvCalcProbDensity(hist, hist_mask, hist_mask); cvCalcBackProject( img_p, mask_rez, hist_mask ); /* read verify final image mask */ final_mask_ver = atsCreateImageFromFile( name_final_mask_very ); if(final_mask_ver == NULL) {code = TRS_FAIL; goto m_exit;} rez+= iplNorm(final_mask_ver, mask_rez, IPL_L2) / (width*height+0.); trsWrite( ATS_CON | ATS_SUM, "\n gesture recognition \n"); trsWrite( ATS_CON | ATS_SUM, "result testing error = %f \n",rez); if(rez > eps_rez) code = TRS_FAIL; else code = TRS_OK; m_exit: cvReleaseImage(&image_mask); cvReleaseImage(&mask_rez); cvReleaseImage(&image_rez); atsReleaseImage(final_mask_ver); atsReleaseImage(init_mask_ver); cvReleaseImage(&imagew); cvReleaseImage(&image_maskw); cvReleaseImage(&img_p[0]); cvReleaseImage(&img_p[1]); cvReleaseImage(&img_v); cvReleaseHist( &hist); cvReleaseHist( &hist_mask); cvReleaseMemStorage ( &storage ); trsFree(pp); trsFree(name_final_mask_very); trsFree(name_init_mask_very); trsFree(name_image); trsFree(name_range_image); trsFree(name_verify_data); fclose(filin); fclose(fil_ver); /* _getch(); */ return code; }
static CvStatus icvFindDominantPointsIPAN( CvSeq * contour, CvMemStorage * storage, CvSeq ** corners, int dmin2, int dmax2, int dneigh2, float amax ) { CvStatus status = CV_OK; /* variables */ int n = contour->total; float *sharpness; float *distance; icvPointInfo *ptInf; int i, j, k; CvSeqWriter writer; float mincos = (float) cos( 3.14159265359 * amax / 180 ); /* check bad arguments */ if( contour == NULL ) return CV_NULLPTR_ERR; if( storage == NULL ) return CV_NULLPTR_ERR; if( corners == NULL ) return CV_NULLPTR_ERR; if( dmin2 < 0 ) return CV_BADSIZE_ERR; if( dmax2 < dmin2 ) return CV_BADSIZE_ERR; if( (dneigh2 > dmax2) || (dneigh2 < 0) ) return CV_BADSIZE_ERR; if( (amax < 0) || (amax > 180) ) return CV_BADSIZE_ERR; sharpness = (float *) cvAlloc( n * sizeof( float )); distance = (float *) cvAlloc( n * sizeof( float )); ptInf = (icvPointInfo *) cvAlloc( n * sizeof( icvPointInfo )); /*****************************************************************************************/ /* First pass */ /*****************************************************************************************/ if( CV_IS_SEQ_CHAIN_CONTOUR( contour )) { CvChainPtReader reader; cvStartReadChainPoints( (CvChain *) contour, &reader ); for( i = 0; i < n; i++ ) { CV_READ_CHAIN_POINT( ptInf[i].pt, reader ); } } else if( CV_IS_SEQ_POLYGON( contour )) { CvSeqReader reader; cvStartReadSeq( contour, &reader, 0 ); for( i = 0; i < n; i++ ) { CV_READ_SEQ_ELEM( ptInf[i].pt, reader ); } } else { return CV_BADFLAG_ERR; } for( i = 0; i < n; i++ ) { /* find nearest suitable points which satisfy distance constraint >dmin */ int left_near = 0; int right_near = 0; int left_far, right_far; float dist_l = 0; float dist_r = 0; int i_plus = 0; int i_minus = 0; float max_cos_alpha; /* find right minimum */ while( dist_r < dmin2 ) { float dx, dy; int ind; if( i_plus >= n ) goto error; right_near = i_plus; if( dist_r < dneigh2 ) ptInf[i].right_neigh = i_plus; i_plus++; ind = (i + i_plus) % n; dx = (float) (ptInf[i].pt.x - ptInf[ind].pt.x); dy = (float) (ptInf[i].pt.y - ptInf[ind].pt.y); dist_r = dx * dx + dy * dy; } /* find right maximum */ while( dist_r <= dmax2 ) { float dx, dy; int ind; if( i_plus >= n ) goto error; distance[(i + i_plus) % n] = cvSqrt( dist_r ); if( dist_r < dneigh2 ) ptInf[i].right_neigh = i_plus; i_plus++; right_far = i_plus; ind = (i + i_plus) % n; dx = (float) (ptInf[i].pt.x - ptInf[ind].pt.x); dy = (float) (ptInf[i].pt.y - ptInf[ind].pt.y); dist_r = dx * dx + dy * dy; } right_far = i_plus; /* left minimum */ while( dist_l < dmin2 ) { float dx, dy; int ind; if( i_minus <= -n ) goto error; left_near = i_minus; if( dist_l < dneigh2 ) ptInf[i].left_neigh = i_minus; i_minus--; ind = i + i_minus; ind = (ind < 0) ? (n + ind) : ind; dx = (float) (ptInf[i].pt.x - ptInf[ind].pt.x); dy = (float) (ptInf[i].pt.y - ptInf[ind].pt.y); dist_l = dx * dx + dy * dy; } /* find left maximum */ while( dist_l <= dmax2 ) { float dx, dy; int ind; if( i_minus <= -n ) goto error; ind = i + i_minus; ind = (ind < 0) ? (n + ind) : ind; distance[ind] = cvSqrt( dist_l ); if( dist_l < dneigh2 ) ptInf[i].left_neigh = i_minus; i_minus--; left_far = i_minus; ind = i + i_minus; ind = (ind < 0) ? (n + ind) : ind; dx = (float) (ptInf[i].pt.x - ptInf[ind].pt.x); dy = (float) (ptInf[i].pt.y - ptInf[ind].pt.y); dist_l = dx * dx + dy * dy; } left_far = i_minus; if( (i_plus - i_minus) > n + 2 ) goto error; max_cos_alpha = -1; for( j = left_far + 1; j < left_near; j++ ) { float dx, dy; float a, a2; int leftind = i + j; leftind = (leftind < 0) ? (n + leftind) : leftind; a = distance[leftind]; a2 = a * a; for( k = right_near + 1; k < right_far; k++ ) { int ind = (i + k) % n; float c2, cosalpha; float b = distance[ind]; float b2 = b * b; /* compute cosinus */ dx = (float) (ptInf[leftind].pt.x - ptInf[ind].pt.x); dy = (float) (ptInf[leftind].pt.y - ptInf[ind].pt.y); c2 = dx * dx + dy * dy; cosalpha = (a2 + b2 - c2) / (2 * a * b); max_cos_alpha = MAX( max_cos_alpha, cosalpha ); if( max_cos_alpha < mincos ) max_cos_alpha = -1; sharpness[i] = max_cos_alpha; } } } /*****************************************************************************************/ /* Second pass */ /*****************************************************************************************/ cvStartWriteSeq( (contour->flags & ~CV_SEQ_ELTYPE_MASK) | CV_SEQ_ELTYPE_INDEX, sizeof( CvSeq ), sizeof( int ), storage, &writer ); /* second pass - nonmaxima suppression */ /* neighborhood of point < dneigh2 */ for( i = 0; i < n; i++ ) { int suppressed = 0; if( sharpness[i] == -1 ) continue; for( j = 1; (j <= ptInf[i].right_neigh) && (suppressed == 0); j++ ) { if( sharpness[i] < sharpness[(i + j) % n] ) suppressed = 1; } for( j = -1; (j >= ptInf[i].left_neigh) && (suppressed == 0); j-- ) { int ind = i + j; ind = (ind < 0) ? (n + ind) : ind; if( sharpness[i] < sharpness[ind] ) suppressed = 1; } if( !suppressed ) CV_WRITE_SEQ_ELEM( i, writer ); } *corners = cvEndWriteSeq( &writer ); cvFree( &sharpness ); cvFree( &distance ); cvFree( &ptInf ); return status; error: /* dmax is so big (more than contour diameter) that algorithm could become infinite cycle */ cvFree( &sharpness ); cvFree( &distance ); cvFree( &ptInf ); return CV_BADRANGE_ERR; }
/* 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; }
/*! \fn CvGabor::get_image(int Type) Get the speific type of image of Gabor Parameters: Type The Type of gabor kernel, e.g. REAL, IMAG, MAG, PHASE Returns: Pointer to image structure, or NULL on failure Return an Image (gandalf image class) with a specific Type "REAL" "IMAG" "MAG" "PHASE" */ IplImage* CvGabor::get_image(int Type) { if(IsKernelCreate() == false) { perror("Error: the Gabor kernel has not been created in get_image()!\n"); return NULL; } else { IplImage* pImage; IplImage *newimage; newimage = cvCreateImage(cvSize(Width,Width), IPL_DEPTH_8U, 1 ); //printf("Width is %d.\n",(int)Width); //printf("Sigma is %f.\n", Sigma); //printf("F is %f.\n", F); //printf("Phi is %f.\n", Phi); //pImage = gan_image_alloc_gl_d(Width, Width); pImage = cvCreateImage( cvSize(Width,Width), IPL_DEPTH_32F, 1 ); CvMat* kernel = cvCreateMat(Width, Width, CV_32FC1); CvMat* re = cvCreateMat(Width, Width, CV_32FC1); CvMat* im = cvCreateMat(Width, Width, CV_32FC1); double ve, ve1,ve2; CvScalar S; CvSize size = cvGetSize( kernel ); int rows = size.height; int cols = size.width; switch(Type) { case 1: //Real cvCopy( (CvMat*)Real, (CvMat*)kernel, NULL ); //pImage = cvGetImage( (CvMat*)kernel, pImageGL ); for (int i = 0; i < rows; i++) { for (int j = 0; j < cols; j++) { ve = cvGetReal2D((CvMat*)kernel, i, j); cvSetReal2D( (IplImage*)pImage, j, i, ve ); } } break; case 2: //Imag cvCopy( (CvMat*)Imag, (CvMat*)kernel, NULL ); //pImage = cvGetImage( (CvMat*)kernel, pImageGL ); for (int i = 0; i < rows; i++) { for (int j = 0; j < cols; j++) { ve = cvGetReal2D((CvMat*)kernel, i, j); cvSetReal2D( (IplImage*)pImage, j, i, ve ); } } break; case 3: //Magnitude //add by yao cvCopy( (CvMat*)Real, (CvMat*)re, NULL ); cvCopy( (CvMat*)Imag, (CvMat*)im, NULL ); for (int i = 0; i < rows; i++) { for (int j = 0; j < cols; j++) { ve1 = cvGetReal2D((CvMat*)re, i, j); ve2 = cvGetReal2D((CvMat*)im, i, j); ve = cvSqrt(ve1*ve1+ve2*ve2); cvSetReal2D( (IplImage*)pImage, j, i, ve ); } } break; case 4: //Phase ///@todo break; } cvNormalize((IplImage*)pImage, (IplImage*)pImage, 0, 255, CV_MINMAX, NULL ); cvConvertScaleAbs( (IplImage*)pImage, (IplImage*)newimage, 1, 0 ); cvReleaseMat(&kernel); cvReleaseImage(&pImage); return newimage; } }
ImageRAII match( IplImage * image1, IplImage * image2, std::pair< CvMat *, CvMat * > image1_keys, std::pair< CvMat *, CvMat * > image2_keys ) { ImageRAII appended_images = appendimages( image1, image2 ); ImageRAII rgb_appended_images( cvCreateImage( cvGetSize( appended_images.image ), appended_images.image->depth, 3 ) ); cvCvtColor( appended_images.image, rgb_appended_images.image, CV_GRAY2RGB ); CvScalar red; red.val[2] = 255; std::vector< std::pair< int, int > > points; // check for matches with the vectors in image1 and image2 for( int i = 0; i < image1_keys.first->height; i++ ) { double magnitude1 = 0; MatrixRAII current_vector( cvCreateMat( 1, image1_keys.first->cols, CV_32FC1 ) ); // keeps track of minimum row found b/t image1 and image2 vectors MatrixRAII min( cvCreateMat( 1, image2_keys.first->cols, CV_32FC1 ) ); cvGetRow( image1_keys.first, current_vector.matrix, i ); CvPoint point1 = cvPoint( ( int )cvmGet( current_vector.matrix, 0, 1 ), ( int )cvmGet( current_vector.matrix, 0, 0 ) ); std::map< float, int > angles; for( int k = 0; k < image1_keys.second->width; k++ ) magnitude1 += pow( cvmGet( image1_keys.second, i, k ), 2 ); magnitude1 = cvSqrt( magnitude1 ); // compare a vector in image1 to every vector in image2 by calculating the cosine simularity for( int j = 0; j < image2_keys.first->height; j++ ) { MatrixRAII descriptor1( cvCreateMat( 1, image1_keys.second->cols, CV_32FC1 ) ); MatrixRAII descriptor2( cvCreateMat( 1, image2_keys.second->cols, CV_32FC1 ) ); cvGetRow( image1_keys.second, descriptor1.matrix, i ); cvGetRow( image2_keys.second, descriptor2.matrix, j ); double dot_product = cvDotProduct( descriptor1.matrix, descriptor2.matrix ); double magnitude2 = 0; for( int k = 0; k < image2_keys.second->width; k++ ) magnitude2 += pow( cvmGet( descriptor1.matrix, 0, k ), 2 ); magnitude2 = cvSqrt( magnitude2 ); angles.insert( std::pair< float, int >( acos( dot_product / ( magnitude1 * magnitude2 ) ), j ) ); } std::map< float, int >::iterator it = angles.begin(); int index = it->second; float angle = it->first; it++; if( angle < THRESHOLD * it->first ) { points.push_back( std::make_pair( i, index ) ); } } std::vector< std::pair< int, int > >::iterator it; for( it = points.begin(); it < points.end(); it++ ) { CvPoint point1 = cvPoint( ( int )cvmGet( image1_keys.first, it->first, 1 ), ( int )cvmGet( image1_keys.first, it->first, 0 ) ); CvPoint point2 = cvPoint( ( int )cvmGet( image2_keys.first, it->second, 1 ) + image1->width, ( int )cvmGet( image2_keys.first, it->second, 0 ) ); cvLine( rgb_appended_images.image, point1, point2, red ); } return rgb_appended_images; }
ImageRAII canny( IplImage * image, std::pair< int, int > thresh, double sigma ) { const char * WINDOW_NAME = "Basic Canny Edge Detector"; ImageRAII grayscale( cvCreateImage( cvGetSize( image ), image->depth, 1 ) ); ImageRAII destination( cvCreateImage( cvGetSize( image ), image->depth, grayscale.image->nChannels ) ); ImageRAII gaussian( cvCreateImage( cvGetSize( image ), image->depth, grayscale.image->nChannels ) ); ImageRAII gradient_x( cvCreateImage( cvGetSize( image ), image->depth, grayscale.image->nChannels ) ); ImageRAII gradient_y( cvCreateImage( cvGetSize( image ), image->depth, grayscale.image->nChannels ) ); ImageRAII gradient( cvCreateImage( cvGetSize( image ), image->depth, grayscale.image->nChannels ) ); ImageRAII orientation( cvCreateImage( cvGetSize( image ), image->depth, grayscale.image->nChannels ) ); // convert image to grayscale cvCvtColor( image, grayscale.image, CV_BGR2GRAY ); // gaussian smoothing cvSmooth( grayscale.image, gaussian.image, CV_GAUSSIAN, GAUSSIAN_X, GAUSSIAN_Y, sigma ); // find edge strength cvSobel( gaussian.image, gradient_x.image, 1, 0, 3 ); cvSobel( gaussian.image, gradient_y.image, 0, 1, 3 ); // find edge orientation CvSize image_size = cvGetSize( gaussian.image ); for( int i = 0; i < image_size.width; i++ ) { for( int j = 0; j < image_size.height; j++ ) { double x = cvGet2D( gradient_x.image, j, i ).val[0]; double y = cvGet2D( gradient_y.image, j, i ).val[0]; float angle; if( x == 0 ) { if( y == 0 ) angle = 0; else angle = 90; } else angle = cvFastArctan( y, x ); CvScalar g; CvScalar a; g.val[0] = cvSqrt( pow( x, 2 ) + pow( y, 2 ) ); a.val[0] = find_angle( angle ); cvSet2D( destination.image, j, i, g ); cvSet2D( orientation.image, j, i, a ); } } ImageRAII suppressed_image = nonMaxSup( destination.image, orientation.image ); ImageRAII hysteresis_image = hysteresis( suppressed_image.image, orientation.image, thresh ); cvNamedWindow( WINDOW_NAME ); cvShowImage( WINDOW_NAME, destination.image ); cvMoveWindow( WINDOW_NAME, image_size.width, 0 ); return hysteresis_image; }