mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
Merge pull request #671 from bitbank2/master
find_line_segment and apriltags speedup
This commit is contained in:
commit
2962c0ce0c
@ -10705,7 +10705,7 @@ zarray_t *apriltag_quad_thresh(apriltag_detector_t *td, image_u8_t *im, bool ove
|
||||
while (entry) {
|
||||
// free any leaked cluster (zarray_add_fail_ok)
|
||||
bool leaked = true;
|
||||
for (int j = 0; j < sz; j++) {
|
||||
for (int j = 0; j < sz && leaked; j++) {
|
||||
zarray_t *cluster;
|
||||
zarray_get(clusters, j, &cluster);
|
||||
leaked &= entry->cluster != cluster;
|
||||
|
||||
@ -40,8 +40,8 @@
|
||||
#define sin(x) sinf(x)
|
||||
#define pow(x,y) powf((x),(y))
|
||||
#define sinh(x) sinhf(x)
|
||||
#define radToDeg(x) ((x) * (180.0 / PI))
|
||||
#define degToRad(x) ((x) * (PI / 180.0))
|
||||
#define radToDeg(x) ((x) * (180.0f / PI))
|
||||
#define degToRad(x) ((x) * (PI / 180.0f))
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////// "lsd.h"
|
||||
@ -428,12 +428,12 @@ float * lsd(int * n_out, unsigned char * img, int X, int Y);
|
||||
|
||||
/** ln(10) */
|
||||
#ifndef M_LN10
|
||||
#define M_LN10 2.30258509299404568402
|
||||
#define M_LN10 2.30258509299404568402f
|
||||
#endif /* !M_LN10 */
|
||||
|
||||
/** PI */
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#define M_PI 3.14159265358979323846f
|
||||
#endif /* !M_PI */
|
||||
|
||||
#ifndef FALSE
|
||||
@ -445,13 +445,14 @@ float * lsd(int * n_out, unsigned char * img, int X, int Y);
|
||||
#endif /* !TRUE */
|
||||
|
||||
/** Label for pixels with undefined gradient. */
|
||||
#define NOTDEF -512.0 // -1024.0
|
||||
#define NOTDEF -512.0f // -1024.0f
|
||||
#define NOTDEF_INT -29335
|
||||
|
||||
/** 3/2 pi */
|
||||
#define M_3_2_PI 4.71238898038
|
||||
#define M_3_2_PI 4.71238898038f
|
||||
|
||||
/** 2 pi */
|
||||
#define M_2__PI 6.28318530718
|
||||
#define M_2__PI 6.28318530718f
|
||||
|
||||
/** Label for pixels not used in yet. */
|
||||
#define NOTUSED 0
|
||||
@ -490,7 +491,7 @@ struct lsd_point {int16_t x,y;};
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/** Doubles relative error factor
|
||||
*/
|
||||
#define RELATIVE_ERROR_FACTOR 100.0
|
||||
#define RELATIVE_ERROR_FACTOR 100.0f
|
||||
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/** Compare doubles by relative error.
|
||||
@ -511,6 +512,9 @@ static int double_equal(float a, float b)
|
||||
if( a == b ) return TRUE;
|
||||
|
||||
abs_diff = fabs(a-b);
|
||||
// For the numbers we work with, this is valid test that avoids some calculations.
|
||||
// The error threshold tested below is 1/1000 of the diff/max_val
|
||||
if (abs_diff > 0.1f) return FALSE;
|
||||
aa = fabs(a);
|
||||
bb = fabs(b);
|
||||
abs_max = aa > bb ? aa : bb;
|
||||
@ -929,7 +933,7 @@ static void gaussian_kernel(ntuple_list kernel, float sigma, float mean)
|
||||
}
|
||||
|
||||
/* normalization */
|
||||
if( sum >= 0.0 ) for(i=0;i<kernel->dim;i++) kernel->values[i] /= sum;
|
||||
if( sum >= 0.0f ) for(i=0;i<kernel->dim;i++) kernel->values[i] /= sum;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------------------*/
|
||||
@ -1190,7 +1194,7 @@ static image_int ll_angle( image_char in, float threshold,
|
||||
(*modgrad)->data[adr] = norm; /* store gradient norm */
|
||||
|
||||
if( norm <= threshold ) /* norm too small, gradient no defined */
|
||||
g->data[adr] = radToDeg(NOTDEF); /* gradient angle not defined */
|
||||
g->data[adr] = NOTDEF_INT; //radToDeg(NOTDEF); /* gradient angle not defined */
|
||||
else
|
||||
{
|
||||
/* gradient angle computation */
|
||||
@ -1287,6 +1291,36 @@ static int isaligned( int x, int y, image_int angles, float theta,
|
||||
return theta <= prec;
|
||||
}
|
||||
|
||||
static int isaligned_fast(int angle, float theta,
|
||||
float prec )
|
||||
{
|
||||
float a;
|
||||
|
||||
if (angle == NOTDEF_INT) return FALSE; // faster to test the integer value
|
||||
|
||||
/* angle at pixel (x,y) */
|
||||
a = degToRad(angle);
|
||||
|
||||
/* pixels whose level-line angle is not defined
|
||||
are considered as NON-aligned */
|
||||
// if( a == NOTDEF ) return FALSE; /* there is no need to call the function
|
||||
// 'double_equal' here because there is
|
||||
// no risk of problems related to the
|
||||
// comparison doubles, we are only
|
||||
// interested in the exact NOTDEF value */
|
||||
|
||||
/* it is assumed that 'theta' and 'a' are in the range [-pi,pi] */
|
||||
theta -= a;
|
||||
if( theta < 0.0 ) theta = -theta;
|
||||
if( theta > M_3_2_PI )
|
||||
{
|
||||
theta -= M_2__PI;
|
||||
if( theta < 0.0 ) theta = -theta;
|
||||
}
|
||||
|
||||
return theta <= prec;
|
||||
} /* isaligned_fast() */
|
||||
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/** Absolute value angle difference.
|
||||
*/
|
||||
@ -1691,10 +1725,10 @@ static void ri_del(rect_iter * iter)
|
||||
|
||||
See details in \ref rect_iter
|
||||
*/
|
||||
static int ri_end(rect_iter * i)
|
||||
static inline int ri_end(rect_iter * i)
|
||||
{
|
||||
/* check input */
|
||||
if( i == NULL ) error("ri_end: NULL iterator.");
|
||||
// if( i == NULL ) error("ri_end: NULL iterator.");
|
||||
|
||||
/* if the current x value is larger than the largest
|
||||
x value in the rectangle (vx[2]), we know the full
|
||||
@ -1710,7 +1744,7 @@ static int ri_end(rect_iter * i)
|
||||
static void ri_inc(rect_iter * i)
|
||||
{
|
||||
/* check input */
|
||||
if( i == NULL ) error("ri_inc: NULL iterator.");
|
||||
// if( i == NULL ) error("ri_inc: NULL iterator.");
|
||||
|
||||
/* if not at end of exploration,
|
||||
increase y value for next pixel in the 'column' */
|
||||
@ -1844,30 +1878,91 @@ static rect_iter * ri_ini(struct rect * r)
|
||||
|
||||
return i;
|
||||
}
|
||||
// We don't need to spend time allocating and freeing the interator structure
|
||||
// since we only use 1 at a time and it's small enough to safely use as a stack var
|
||||
void ri_ini_fast(rect_iter *i, struct rect * r)
|
||||
{
|
||||
float vx[4],vy[4];
|
||||
int n,offset;
|
||||
|
||||
/* build list of rectangle corners ordered
|
||||
in a circular way around the rectangle */
|
||||
vx[0] = r->x1 - r->dy * r->width / 2.0;
|
||||
vy[0] = r->y1 + r->dx * r->width / 2.0;
|
||||
vx[1] = r->x2 - r->dy * r->width / 2.0;
|
||||
vy[1] = r->y2 + r->dx * r->width / 2.0;
|
||||
vx[2] = r->x2 + r->dy * r->width / 2.0;
|
||||
vy[2] = r->y2 - r->dx * r->width / 2.0;
|
||||
vx[3] = r->x1 + r->dy * r->width / 2.0;
|
||||
vy[3] = r->y1 - r->dx * r->width / 2.0;
|
||||
|
||||
/* compute rotation of index of corners needed so that the first
|
||||
point has the smaller x.
|
||||
|
||||
if one side is vertical, thus two corners have the same smaller x
|
||||
value, the one with the largest y value is selected as the first.
|
||||
*/
|
||||
if( r->x1 < r->x2 && r->y1 <= r->y2 ) offset = 0;
|
||||
else if( r->x1 >= r->x2 && r->y1 < r->y2 ) offset = 1;
|
||||
else if( r->x1 > r->x2 && r->y1 >= r->y2 ) offset = 2;
|
||||
else offset = 3;
|
||||
|
||||
/* apply rotation of index. */
|
||||
for(n=0; n<4; n++)
|
||||
{
|
||||
i->vx[n] = vx[(n+offset) & 3];
|
||||
i->vy[n] = vy[(n+offset) & 3];
|
||||
}
|
||||
|
||||
/* Set an initial condition.
|
||||
|
||||
The values are set to values that will cause 'ri_inc' (that will
|
||||
be called immediately) to initialize correctly the first 'column'
|
||||
and compute the limits 'ys' and 'ye'.
|
||||
|
||||
'y' is set to the integer value of vy[0], the starting corner.
|
||||
|
||||
'ys' and 'ye' are set to very small values, so 'ri_inc' will
|
||||
notice that it needs to start a new 'column'.
|
||||
|
||||
The smallest integer coordinate inside of the rectangle is
|
||||
'ceil(vx[0])'. The current 'x' value is set to that value minus
|
||||
one, so 'ri_inc' (that will increase x by one) will advance to
|
||||
the first 'column'.
|
||||
*/
|
||||
i->x = (int) ceil(i->vx[0]) - 1;
|
||||
i->y = (int) ceil(i->vy[0]);
|
||||
i->ys = i->ye = -FLT_MAX;
|
||||
|
||||
/* advance to the first pixel */
|
||||
ri_inc(i);
|
||||
|
||||
} /* ri_ini_fast() */
|
||||
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/** Compute a rectangle's NFA value.
|
||||
*/
|
||||
static float rect_nfa(struct rect * rec, image_int angles, float logNT)
|
||||
{
|
||||
rect_iter * i;
|
||||
rect_iter i;
|
||||
int pts = 0;
|
||||
int alg = 0;
|
||||
|
||||
int xsize = angles->xsize, ysize = angles->ysize;
|
||||
/* check parameters */
|
||||
if( rec == NULL ) error("rect_nfa: invalid rectangle.");
|
||||
if( angles == NULL ) error("rect_nfa: invalid 'angles'.");
|
||||
// if( rec == NULL ) error("rect_nfa: invalid rectangle.");
|
||||
// if( angles == NULL ) error("rect_nfa: invalid 'angles'.");
|
||||
|
||||
/* compute the total number of pixels and of aligned points in 'rec' */
|
||||
for(i=ri_ini(rec); !ri_end(i); ri_inc(i)) /* rectangle iterator */
|
||||
if( i->x >= 0 && i->y >= 0 &&
|
||||
i->x < (int) angles->xsize && i->y < (int) angles->ysize )
|
||||
ri_ini_fast(&i, rec);
|
||||
for(; !ri_end(&i); ri_inc(&i)) /* rectangle iterator */
|
||||
if( i.x >= 0 && i.y >= 0 &&
|
||||
i.x < xsize && i.y < ysize )
|
||||
{
|
||||
++pts; /* total number of pixels counter */
|
||||
if( isaligned(i->x, i->y, angles, rec->theta, rec->prec) )
|
||||
if( isaligned_fast((float)angles->data[(i.y*xsize)+i.x], rec->theta, rec->prec) )
|
||||
++alg; /* aligned points counter */
|
||||
}
|
||||
ri_del(i); /* delete iterator */
|
||||
// ri_del(i); /* delete iterator */
|
||||
|
||||
return nfa(pts,alg,rec->p,logNT); /* compute NFA value */
|
||||
}
|
||||
@ -1983,7 +2078,7 @@ static void region2rect( struct lsd_point * reg, int reg_size,
|
||||
{
|
||||
float x,y,dx,dy,l,w,theta,weight,sum,l_min,l_max,w_min,w_max;
|
||||
int i;
|
||||
|
||||
int ix,iy,isum,iweight;
|
||||
/* check parameters */
|
||||
if( reg == NULL ) error("region2rect: invalid region.");
|
||||
if( reg_size <= 1 ) error("region2rect: region size <= 1.");
|
||||
@ -2001,15 +2096,17 @@ static void region2rect( struct lsd_point * reg, int reg_size,
|
||||
where G(i) is the norm of the gradient of pixel i
|
||||
and x_i,y_i are its coordinates.
|
||||
*/
|
||||
x = y = sum = 0.0;
|
||||
// x = y = sum = 0.0;
|
||||
ix = iy = isum = 0; // integers are faster since the source data is integer
|
||||
for(i=0; i<reg_size; i++)
|
||||
{
|
||||
weight = modgrad->data[ reg[i].x + reg[i].y * modgrad->xsize ];
|
||||
x += (float) reg[i].x * weight;
|
||||
y += (float) reg[i].y * weight;
|
||||
sum += weight;
|
||||
iweight = modgrad->data[ reg[i].x + reg[i].y * modgrad->xsize ];
|
||||
ix += reg[i].x * iweight;
|
||||
iy += reg[i].y * iweight;
|
||||
isum += iweight;
|
||||
}
|
||||
if( sum <= 0.0 ) error("region2rect: weights sum equal to zero.");
|
||||
if( isum <= 0 ) error("region2rect: weights sum equal to zero.");
|
||||
x = (float)ix; y = (float)iy; sum = (float)isum;
|
||||
x /= sum;
|
||||
y /= sum;
|
||||
|
||||
@ -2076,48 +2173,65 @@ static void region_grow( int x, int y, image_int angles, struct lsd_point * reg,
|
||||
{
|
||||
float sumdx,sumdy;
|
||||
int xx,yy,i;
|
||||
|
||||
int l_size; // local copy
|
||||
float l_angle; // local copy
|
||||
int xsize = used->xsize;
|
||||
/* check parameters */
|
||||
if( x < 0 || y < 0 || x >= (int) angles->xsize || y >= (int) angles->ysize )
|
||||
error("region_grow: (x,y) out of the image.");
|
||||
if( angles == NULL || angles->data == NULL )
|
||||
error("region_grow: invalid image 'angles'.");
|
||||
if( reg == NULL ) error("region_grow: invalid 'reg'.");
|
||||
if( reg_size == NULL ) error("region_grow: invalid pointer 'reg_size'.");
|
||||
if( reg_angle == NULL ) error("region_grow: invalid pointer 'reg_angle'.");
|
||||
if( used == NULL || used->data == NULL )
|
||||
error("region_grow: invalid image 'used'.");
|
||||
// if( angles == NULL || angles->data == NULL )
|
||||
// error("region_grow: invalid image 'angles'.");
|
||||
// if( reg == NULL ) error("region_grow: invalid 'reg'.");
|
||||
// if( reg_size == NULL ) error("region_grow: invalid pointer 'reg_size'.");
|
||||
// if( reg_angle == NULL ) error("region_grow: invalid pointer 'reg_angle'.");
|
||||
// if( used == NULL || used->data == NULL )
|
||||
// error("region_grow: invalid image 'used'.");
|
||||
|
||||
/* first point of the region */
|
||||
*reg_size = 1;
|
||||
l_size = 1;
|
||||
reg[0].x = x;
|
||||
reg[0].y = y;
|
||||
*reg_angle = degToRad(angles->data[x+y*angles->xsize]); /* region's angle */
|
||||
sumdx = cos(*reg_angle);
|
||||
sumdy = sin(*reg_angle);
|
||||
l_angle = degToRad(angles->data[x+y*angles->xsize]); /* region's angle */
|
||||
sumdx = cos(l_angle);
|
||||
sumdy = sin(l_angle);
|
||||
used->data[x+y*used->xsize] = USED;
|
||||
|
||||
/* try neighbors as new region points */
|
||||
for(i=0; i<*reg_size; i++)
|
||||
for(xx=reg[i].x-1; xx<=reg[i].x+1; xx++)
|
||||
for(yy=reg[i].y-1; yy<=reg[i].y+1; yy++)
|
||||
if( xx>=0 && yy>=0 && xx<(int)used->xsize && yy<(int)used->ysize &&
|
||||
used->data[xx+yy*used->xsize] != USED &&
|
||||
isaligned(xx,yy,angles,*reg_angle,prec) )
|
||||
for(i=0; i<l_size; i++) {
|
||||
int dx=3, dy=3; // assume 3x3 region to try
|
||||
int ty = reg[i].y-1;
|
||||
int tx = reg[i].x-1;
|
||||
if (tx < 0) {
|
||||
tx = 0; dx--;
|
||||
} else if (tx+dx >= xsize) {
|
||||
dx--;
|
||||
}
|
||||
if (ty < 0) {
|
||||
ty = 0; dy--;
|
||||
} else if (ty+dy >= used->ysize) {
|
||||
dy--;
|
||||
}
|
||||
for(xx=tx; xx<tx+dx; xx++)
|
||||
for(yy=ty; yy<ty+dy; yy++)
|
||||
if( used->data[xx+yy*xsize] != USED &&
|
||||
isaligned_fast((float)angles->data[(yy*xsize)+xx],l_angle,prec) )
|
||||
{
|
||||
/* add point */
|
||||
used->data[xx+yy*used->xsize] = USED;
|
||||
reg[*reg_size].x = xx;
|
||||
reg[*reg_size].y = yy;
|
||||
++(*reg_size);
|
||||
used->data[xx+yy*xsize] = USED;
|
||||
reg[l_size].x = xx;
|
||||
reg[l_size].y = yy;
|
||||
++l_size;
|
||||
|
||||
/* update region's angle */
|
||||
int16_t angle = angles->data[xx+yy*angles->xsize] % 360;
|
||||
int16_t angle = angles->data[xx+yy*xsize] % 360;
|
||||
if (angle < 0) angle += 360;
|
||||
sumdx += cos_table[angle];
|
||||
sumdy += sin_table[angle];
|
||||
*reg_angle = atan2(sumdy,sumdx);
|
||||
l_angle = atan2(sumdy,sumdx);
|
||||
}
|
||||
}
|
||||
*reg_size = l_size;
|
||||
*reg_angle = l_angle;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------------------*/
|
||||
@ -2479,7 +2593,8 @@ float * LineSegmentDetection( int * n_out,
|
||||
/* search for line segments */
|
||||
for(; list_p != NULL; list_p = list_p->next )
|
||||
if( used->data[ list_p->x + list_p->y * used->xsize ] == NOTUSED &&
|
||||
degToRad(angles->data[ list_p->x + list_p->y * angles->xsize ]) != NOTDEF )
|
||||
angles->data[ list_p->x + list_p->y * angles->xsize ] != NOTDEF_INT )
|
||||
// degToRad(angles->data[ list_p->x + list_p->y * angles->xsize ]) != NOTDEF )
|
||||
/* there is no risk of float comparison problems here
|
||||
because we are only interested in the exact NOTDEF value */
|
||||
{
|
||||
|
||||
Loading…
Reference in New Issue
Block a user