// get column of input array
CV_IMPL CvMat*
cvGetDiag( const CvArr* arr, CvMat* submat, int diag )
{
CvMat* res = 0;
CV_FUNCNAME( "cvGetDiag" );
__BEGIN__;
CvMat stub, *mat = (CvMat*)arr;
int pix_size;
if( !CV_IS_ARR( mat ))
CV_CALL( mat = cvGetMat( mat, &stub ));
if( !submat )
CV_ERROR( CV_StsNullPtr, "" );
pix_size = icvPixSize[CV_ARR_TYPE(mat->type)];
if( diag >= 0 )
{
int len = mat->width - diag;
if( len <= 0 )
CV_ERROR( CV_StsOutOfRange, "" );
len = CV_IMIN( len, mat->height );
CV_CALL( cvInitMatHeader( submat, len, 1, mat->type,
mat->data.ptr + diag*pix_size,
mat->step + pix_size ));
}
else
{
int len = mat->height + diag;
diag = -diag;
if( len <= 0 )
CV_ERROR( CV_StsOutOfRange, "" );
len = CV_IMIN( len, mat->width );
CV_CALL( cvInitMatHeader( submat, len, 1, mat->type,
mat->data.ptr + diag*mat->step,
mat->step + pix_size ));
}
res = submat;
__END__;
return res;
}
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));
}
}
