diff --git a/src/omv/img/apriltag.c b/src/omv/img/apriltag.c index abe710eae..8baaf7838 100644 --- a/src/omv/img/apriltag.c +++ b/src/omv/img/apriltag.c @@ -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; diff --git a/src/omv/img/lsd.c b/src/omv/img/lsd.c index 17dafbab4..3c6948abb 100644 --- a/src/omv/img/lsd.c +++ b/src/omv/img/lsd.c @@ -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;idim;i++) kernel->values[i] /= sum; + if( sum >= 0.0f ) for(i=0;idim;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; idata[ 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= xsize) { + dx--; + } + if (ty < 0) { + ty = 0; dy--; + } else if (ty+dy >= used->ysize) { + dy--; + } + for(xx=tx; xxdata[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 */ {