/*F///////////////////////////////////////////////////////////////////////////////////////
// Name: icvCalcPGH
// Purpose:
// Calculates PGH(pairwise geometric histogram) for contour given.
// Context:
// Parameters:
// contour - pointer to input contour object.
// pgh - output histogram
// ang_dim - number of angle bins (vertical size of histogram)
// dist_dim - number of distance bins (horizontal size of histogram)
// Returns:
// CV_OK or error code
// Notes:
//F*/
static CvStatus
icvCalcPGH( const CvSeq * contour, float *pgh, int angle_dim, int dist_dim )
{
char local_buffer[(1 << 14) + 32];
float *local_buffer_ptr = (float *)cvAlignPtr(local_buffer,32);
float *buffer = local_buffer_ptr;
double angle_scale = (angle_dim - 0.51) / icv_acos_table[0];
double dist_scale = DBL_EPSILON;
int buffer_size;
int i, count, pass;
int *pghi = (int *) pgh;
int hist_size = angle_dim * dist_dim;
CvSeqReader reader1, reader2; /* external and internal readers */
if( !contour || !pgh )
return CV_NULLPTR_ERR;
if( angle_dim <= 0 || angle_dim > 180 || dist_dim <= 0 )
return CV_BADRANGE_ERR;
if( !CV_IS_SEQ_POINT_SET( contour ))
return CV_BADFLAG_ERR;
memset( pgh, 0, hist_size * sizeof( pgh[0] ));
count = contour->total;
/* allocate buffer for distances */
buffer_size = count * sizeof( float );
if( buffer_size > (int)sizeof(local_buffer) - 32 )
{
buffer = (float *) cvAlloc( buffer_size );
if( !buffer )
return CV_OUTOFMEM_ERR;
}
cvStartReadSeq( contour, &reader1, 0 );
cvStartReadSeq( contour, &reader2, 0 );
/* calc & store squared edge lengths, calculate maximal distance between edges */
for( i = 0; i < count; i++ )
{
CvPoint pt1, pt2;
double dx, dy;
CV_READ_EDGE( pt1, pt2, reader1 );
dx = pt2.x - pt1.x;
dy = pt2.y - pt1.y;
buffer[i] = (float)(1./sqrt(dx * dx + dy * dy));
}
/*
do 2 passes.
First calculates maximal distance.
Second calculates histogram itself.
*/
for( pass = 1; pass <= 2; pass++ )
{
double dist_coeff = 0, angle_coeff = 0;
/* run external loop */
for( i = 0; i < count; i++ )
{
CvPoint pt1, pt2;
int dx, dy;
int dist = 0;
CV_READ_EDGE( pt1, pt2, reader1 );
dx = pt2.x - pt1.x;
dy = pt2.y - pt1.y;
if( (dx | dy) != 0 )
{
int j;
if( pass == 2 )
{
dist_coeff = buffer[i] * dist_scale;
angle_coeff = buffer[i] * (_CV_ACOS_TABLE_SIZE / 2);
}
/* run internal loop (for current edge) */
for( j = 0; j < count; j++ )
{
CvPoint pt3, pt4;
CV_READ_EDGE( pt3, pt4, reader2 );
if( i != j ) /* process edge pair */
{
int d1 = (pt3.y - pt1.y) * dx - (pt3.x - pt1.x) * dy;
int d2 = (pt4.y - pt1.y) * dx - (pt2.x - pt1.x) * dy;
int cross_flag;
int *hist_row = 0;
if( pass == 2 )
{
int dp = (pt4.x - pt3.x) * dx + (pt4.y - pt3.y) * dy;
dp = cvRound( dp * angle_coeff * buffer[j] ) +
(_CV_ACOS_TABLE_SIZE / 2);
dp = MAX( dp, 0 );
dp = MIN( dp, _CV_ACOS_TABLE_SIZE - 1 );
hist_row = pghi + dist_dim *
cvRound( icv_acos_table[dp] * angle_scale );
d1 = cvRound( d1 * dist_coeff );
d2 = cvRound( d2 * dist_coeff );
}
cross_flag = (d1 ^ d2) < 0;
d1 = CV_IABS( d1 );
d2 = CV_IABS( d2 );
if( pass == 2 )
{
if( d1 >= dist_dim )
d1 = dist_dim - 1;
if( d2 >= dist_dim )
d2 = dist_dim - 1;
if( !cross_flag )
{
if( d1 > d2 ) /* make d1 <= d2 */
{
d1 ^= d2;
d2 ^= d1;
d1 ^= d2;
}
for( ; d1 <= d2; d1++ )
hist_row[d1]++;
}
else
{
for( ; d1 >= 0; d1-- )
hist_row[d1]++;
for( ; d2 >= 0; d2-- )
hist_row[d2]++;
}
}
else /* 1st pass */
{
d1 = CV_IMAX( d1, d2 );
dist = CV_IMAX( dist, d1 );
}
} /* end of processing of edge pair */
} /* end of internal loop */
if( pass == 1 )
{
double scale = dist * buffer[i];
dist_scale = MAX( dist_scale, scale );
}
}
} /* end of external loop */
if( pass == 1 )
{
dist_scale = (dist_dim - 0.51) / dist_scale;
}
} /* end of pass on loops */
/* convert hist to floats */
for( i = 0; i < hist_size; i++ )
{
((float *) pghi)[i] = (float) pghi[i];
}
if( buffer != local_buffer_ptr )
cvFree( &buffer );
return CV_OK;
}
static void
HoughLinesSDiv( const Mat& img,
float rho, float theta, int threshold,
int srn, int stn,
std::vector<Vec2f>& lines, int linesMax,
double min_theta, double max_theta )
{
#define _POINT(row, column)\
(image_src[(row)*step+(column)])
int index, i;
int ri, ti, ti1, ti0;
int row, col;
float r, t; /* Current rho and theta */
float rv; /* Some temporary rho value */
int fn = 0;
float xc, yc;
const float d2r = (float)(CV_PI / 180);
int sfn = srn * stn;
int fi;
int count;
int cmax = 0;
std::vector<hough_index> lst;
CV_Assert( img.type() == CV_8UC1 );
CV_Assert( linesMax > 0 );
threshold = MIN( threshold, 255 );
const uchar* image_src = img.ptr();
int step = (int)img.step;
int w = img.cols;
int h = img.rows;
float irho = 1 / rho;
float itheta = 1 / theta;
float srho = rho / srn;
float stheta = theta / stn;
float isrho = 1 / srho;
float istheta = 1 / stheta;
int rn = cvFloor( std::sqrt( (double)w * w + (double)h * h ) * irho );
int tn = cvFloor( 2 * CV_PI * itheta );
lst.push_back(hough_index(threshold, -1.f, 0.f));
// Precalculate sin table
std::vector<float> _sinTable( 5 * tn * stn );
float* sinTable = &_sinTable[0];
for( index = 0; index < 5 * tn * stn; index++ )
sinTable[index] = (float)cos( stheta * index * 0.2f );
std::vector<uchar> _caccum(rn * tn, (uchar)0);
uchar* caccum = &_caccum[0];
// Counting all feature pixels
for( row = 0; row < h; row++ )
for( col = 0; col < w; col++ )
fn += _POINT( row, col ) != 0;
std::vector<int> _x(fn), _y(fn);
int* x = &_x[0], *y = &_y[0];
// Full Hough Transform (it's accumulator update part)
fi = 0;
for( row = 0; row < h; row++ )
{
for( col = 0; col < w; col++ )
{
if( _POINT( row, col ))
{
int halftn;
float r0;
float scale_factor;
int iprev = -1;
float phi, phi1;
float theta_it; // Value of theta for iterating
// Remember the feature point
x[fi] = col;
y[fi] = row;
fi++;
yc = (float) row + 0.5f;
xc = (float) col + 0.5f;
/* Update the accumulator */
t = (float) fabs( cvFastArctan( yc, xc ) * d2r );
r = (float) std::sqrt( (double)xc * xc + (double)yc * yc );
r0 = r * irho;
ti0 = cvFloor( (t + CV_PI*0.5) * itheta );
caccum[ti0]++;
theta_it = rho / r;
theta_it = theta_it < theta ? theta_it : theta;
scale_factor = theta_it * itheta;
halftn = cvFloor( CV_PI / theta_it );
for( ti1 = 1, phi = theta_it - (float)(CV_PI*0.5), phi1 = (theta_it + t) * itheta;
ti1 < halftn; ti1++, phi += theta_it, phi1 += scale_factor )
{
rv = r0 * std::cos( phi );
i = (int)rv * tn;
i += cvFloor( phi1 );
assert( i >= 0 );
assert( i < rn * tn );
caccum[i] = (uchar) (caccum[i] + ((i ^ iprev) != 0));
iprev = i;
if( cmax < caccum[i] )
cmax = caccum[i];
}
}
}
}
// Starting additional analysis
count = 0;
for( ri = 0; ri < rn; ri++ )
{
for( ti = 0; ti < tn; ti++ )
{
if( caccum[ri * tn + ti] > threshold )
count++;
}
}
if( count * 100 > rn * tn )
{
HoughLinesStandard( img, rho, theta, threshold, lines, linesMax, min_theta, max_theta );
return;
}
std::vector<uchar> _buffer(srn * stn + 2);
uchar* buffer = &_buffer[0];
uchar* mcaccum = buffer + 1;
count = 0;
for( ri = 0; ri < rn; ri++ )
{
for( ti = 0; ti < tn; ti++ )
{
if( caccum[ri * tn + ti] > threshold )
{
count++;
memset( mcaccum, 0, sfn * sizeof( uchar ));
for( index = 0; index < fn; index++ )
{
int ti2;
float r0;
yc = (float) y[index] + 0.5f;
xc = (float) x[index] + 0.5f;
// Update the accumulator
t = (float) fabs( cvFastArctan( yc, xc ) * d2r );
r = (float) std::sqrt( (double)xc * xc + (double)yc * yc ) * isrho;
ti0 = cvFloor( (t + CV_PI * 0.5) * istheta );
ti2 = (ti * stn - ti0) * 5;
r0 = (float) ri *srn;
for( ti1 = 0; ti1 < stn; ti1++, ti2 += 5 )
{
rv = r * sinTable[(int) (std::abs( ti2 ))] - r0;
i = cvFloor( rv ) * stn + ti1;
i = CV_IMAX( i, -1 );
i = CV_IMIN( i, sfn );
mcaccum[i]++;
assert( i >= -1 );
assert( i <= sfn );
}
}
// Find peaks in maccum...
for( index = 0; index < sfn; index++ )
{
i = 0;
int pos = (int)(lst.size() - 1);
if( pos < 0 || lst[pos].value < mcaccum[index] )
{
hough_index vi(mcaccum[index],
index / stn * srho + ri * rho,
index % stn * stheta + ti * theta - (float)(CV_PI*0.5));
lst.push_back(vi);
for( ; pos >= 0; pos-- )
{
if( lst[pos].value > vi.value )
break;
lst[pos+1] = lst[pos];
}
lst[pos+1] = vi;
if( (int)lst.size() > linesMax )
lst.pop_back();
}
}
}
}
}
for( size_t idx = 0; idx < lst.size(); idx++ )
{
if( lst[idx].rho < 0 )
continue;
lines.push_back(Vec2f(lst[idx].rho, lst[idx].theta));
}
}
