Merge pull request #671 from bitbank2/master

find_line_segment and apriltags speedup
This commit is contained in:
Ibrahim Abd Elkader 2020-01-21 02:27:06 +02:00 committed by GitHub
commit 2962c0ce0c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 170 additions and 55 deletions

View File

@ -10705,7 +10705,7 @@ zarray_t *apriltag_quad_thresh(apriltag_detector_t *td, image_u8_t *im, bool ove
while (entry) { while (entry) {
// free any leaked cluster (zarray_add_fail_ok) // free any leaked cluster (zarray_add_fail_ok)
bool leaked = true; bool leaked = true;
for (int j = 0; j < sz; j++) { for (int j = 0; j < sz && leaked; j++) {
zarray_t *cluster; zarray_t *cluster;
zarray_get(clusters, j, &cluster); zarray_get(clusters, j, &cluster);
leaked &= entry->cluster != cluster; leaked &= entry->cluster != cluster;

View File

@ -40,8 +40,8 @@
#define sin(x) sinf(x) #define sin(x) sinf(x)
#define pow(x,y) powf((x),(y)) #define pow(x,y) powf((x),(y))
#define sinh(x) sinhf(x) #define sinh(x) sinhf(x)
#define radToDeg(x) ((x) * (180.0 / PI)) #define radToDeg(x) ((x) * (180.0f / PI))
#define degToRad(x) ((x) * (PI / 180.0)) #define degToRad(x) ((x) * (PI / 180.0f))
//////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////
//////// "lsd.h" //////// "lsd.h"
@ -428,12 +428,12 @@ float * lsd(int * n_out, unsigned char * img, int X, int Y);
/** ln(10) */ /** ln(10) */
#ifndef M_LN10 #ifndef M_LN10
#define M_LN10 2.30258509299404568402 #define M_LN10 2.30258509299404568402f
#endif /* !M_LN10 */ #endif /* !M_LN10 */
/** PI */ /** PI */
#ifndef M_PI #ifndef M_PI
#define M_PI 3.14159265358979323846 #define M_PI 3.14159265358979323846f
#endif /* !M_PI */ #endif /* !M_PI */
#ifndef FALSE #ifndef FALSE
@ -445,13 +445,14 @@ float * lsd(int * n_out, unsigned char * img, int X, int Y);
#endif /* !TRUE */ #endif /* !TRUE */
/** Label for pixels with undefined gradient. */ /** 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 */ /** 3/2 pi */
#define M_3_2_PI 4.71238898038 #define M_3_2_PI 4.71238898038f
/** 2 pi */ /** 2 pi */
#define M_2__PI 6.28318530718 #define M_2__PI 6.28318530718f
/** Label for pixels not used in yet. */ /** Label for pixels not used in yet. */
#define NOTUSED 0 #define NOTUSED 0
@ -490,7 +491,7 @@ struct lsd_point {int16_t x,y;};
/*----------------------------------------------------------------------------*/ /*----------------------------------------------------------------------------*/
/** Doubles relative error factor /** Doubles relative error factor
*/ */
#define RELATIVE_ERROR_FACTOR 100.0 #define RELATIVE_ERROR_FACTOR 100.0f
/*----------------------------------------------------------------------------*/ /*----------------------------------------------------------------------------*/
/** Compare doubles by relative error. /** Compare doubles by relative error.
@ -511,6 +512,9 @@ static int double_equal(float a, float b)
if( a == b ) return TRUE; if( a == b ) return TRUE;
abs_diff = fabs(a-b); 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); aa = fabs(a);
bb = fabs(b); bb = fabs(b);
abs_max = aa > bb ? aa : bb; abs_max = aa > bb ? aa : bb;
@ -929,7 +933,7 @@ static void gaussian_kernel(ntuple_list kernel, float sigma, float mean)
} }
/* normalization */ /* 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 */ (*modgrad)->data[adr] = norm; /* store gradient norm */
if( norm <= threshold ) /* norm too small, gradient no defined */ 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 else
{ {
/* gradient angle computation */ /* gradient angle computation */
@ -1287,6 +1291,36 @@ static int isaligned( int x, int y, image_int angles, float theta,
return theta <= prec; 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. /** Absolute value angle difference.
*/ */
@ -1691,10 +1725,10 @@ static void ri_del(rect_iter * iter)
See details in \ref rect_iter See details in \ref rect_iter
*/ */
static int ri_end(rect_iter * i) static inline int ri_end(rect_iter * i)
{ {
/* check input */ /* 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 /* if the current x value is larger than the largest
x value in the rectangle (vx[2]), we know the full 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) static void ri_inc(rect_iter * i)
{ {
/* check input */ /* check input */
if( i == NULL ) error("ri_inc: NULL iterator."); // if( i == NULL ) error("ri_inc: NULL iterator.");
/* if not at end of exploration, /* if not at end of exploration,
increase y value for next pixel in the 'column' */ increase y value for next pixel in the 'column' */
@ -1844,30 +1878,91 @@ static rect_iter * ri_ini(struct rect * r)
return i; 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. /** Compute a rectangle's NFA value.
*/ */
static float rect_nfa(struct rect * rec, image_int angles, float logNT) static float rect_nfa(struct rect * rec, image_int angles, float logNT)
{ {
rect_iter * i; rect_iter i;
int pts = 0; int pts = 0;
int alg = 0; int alg = 0;
int xsize = angles->xsize, ysize = angles->ysize;
/* check parameters */ /* check parameters */
if( rec == NULL ) error("rect_nfa: invalid rectangle."); // if( rec == NULL ) error("rect_nfa: invalid rectangle.");
if( angles == NULL ) error("rect_nfa: invalid 'angles'."); // if( angles == NULL ) error("rect_nfa: invalid 'angles'.");
/* compute the total number of pixels and of aligned points in 'rec' */ /* 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 */ ri_ini_fast(&i, rec);
if( i->x >= 0 && i->y >= 0 && for(; !ri_end(&i); ri_inc(&i)) /* rectangle iterator */
i->x < (int) angles->xsize && i->y < (int) angles->ysize ) if( i.x >= 0 && i.y >= 0 &&
i.x < xsize && i.y < ysize )
{ {
++pts; /* total number of pixels counter */ ++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 */ ++alg; /* aligned points counter */
} }
ri_del(i); /* delete iterator */ // ri_del(i); /* delete iterator */
return nfa(pts,alg,rec->p,logNT); /* compute NFA value */ 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; float x,y,dx,dy,l,w,theta,weight,sum,l_min,l_max,w_min,w_max;
int i; int i;
int ix,iy,isum,iweight;
/* check parameters */ /* check parameters */
if( reg == NULL ) error("region2rect: invalid region."); if( reg == NULL ) error("region2rect: invalid region.");
if( reg_size <= 1 ) error("region2rect: region size <= 1."); 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 where G(i) is the norm of the gradient of pixel i
and x_i,y_i are its coordinates. 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++) for(i=0; i<reg_size; i++)
{ {
weight = modgrad->data[ reg[i].x + reg[i].y * modgrad->xsize ]; iweight = modgrad->data[ reg[i].x + reg[i].y * modgrad->xsize ];
x += (float) reg[i].x * weight; ix += reg[i].x * iweight;
y += (float) reg[i].y * weight; iy += reg[i].y * iweight;
sum += weight; 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; x /= sum;
y /= 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; float sumdx,sumdy;
int xx,yy,i; int xx,yy,i;
int l_size; // local copy
float l_angle; // local copy
int xsize = used->xsize;
/* check parameters */ /* check parameters */
if( x < 0 || y < 0 || x >= (int) angles->xsize || y >= (int) angles->ysize ) if( x < 0 || y < 0 || x >= (int) angles->xsize || y >= (int) angles->ysize )
error("region_grow: (x,y) out of the image."); error("region_grow: (x,y) out of the image.");
if( angles == NULL || angles->data == NULL ) // if( angles == NULL || angles->data == NULL )
error("region_grow: invalid image 'angles'."); // error("region_grow: invalid image 'angles'.");
if( reg == NULL ) error("region_grow: invalid 'reg'."); // if( reg == NULL ) error("region_grow: invalid 'reg'.");
if( reg_size == NULL ) error("region_grow: invalid pointer 'reg_size'."); // if( reg_size == NULL ) error("region_grow: invalid pointer 'reg_size'.");
if( reg_angle == NULL ) error("region_grow: invalid pointer 'reg_angle'."); // if( reg_angle == NULL ) error("region_grow: invalid pointer 'reg_angle'.");
if( used == NULL || used->data == NULL ) // if( used == NULL || used->data == NULL )
error("region_grow: invalid image 'used'."); // error("region_grow: invalid image 'used'.");
/* first point of the region */ /* first point of the region */
*reg_size = 1; l_size = 1;
reg[0].x = x; reg[0].x = x;
reg[0].y = y; reg[0].y = y;
*reg_angle = degToRad(angles->data[x+y*angles->xsize]); /* region's angle */ l_angle = degToRad(angles->data[x+y*angles->xsize]); /* region's angle */
sumdx = cos(*reg_angle); sumdx = cos(l_angle);
sumdy = sin(*reg_angle); sumdy = sin(l_angle);
used->data[x+y*used->xsize] = USED; used->data[x+y*used->xsize] = USED;
/* try neighbors as new region points */ /* try neighbors as new region points */
for(i=0; i<*reg_size; i++) for(i=0; i<l_size; i++) {
for(xx=reg[i].x-1; xx<=reg[i].x+1; xx++) int dx=3, dy=3; // assume 3x3 region to try
for(yy=reg[i].y-1; yy<=reg[i].y+1; yy++) int ty = reg[i].y-1;
if( xx>=0 && yy>=0 && xx<(int)used->xsize && yy<(int)used->ysize && int tx = reg[i].x-1;
used->data[xx+yy*used->xsize] != USED && if (tx < 0) {
isaligned(xx,yy,angles,*reg_angle,prec) ) 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 */ /* add point */
used->data[xx+yy*used->xsize] = USED; used->data[xx+yy*xsize] = USED;
reg[*reg_size].x = xx; reg[l_size].x = xx;
reg[*reg_size].y = yy; reg[l_size].y = yy;
++(*reg_size); ++l_size;
/* update region's angle */ /* 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; if (angle < 0) angle += 360;
sumdx += cos_table[angle]; sumdx += cos_table[angle];
sumdy += sin_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 */ /* search for line segments */
for(; list_p != NULL; list_p = list_p->next ) for(; list_p != NULL; list_p = list_p->next )
if( used->data[ list_p->x + list_p->y * used->xsize ] == NOTUSED && 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 /* there is no risk of float comparison problems here
because we are only interested in the exact NOTDEF value */ because we are only interested in the exact NOTDEF value */
{ {