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