mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
Additional speed for lsd and apriltags
This commit is contained in:
parent
49ba439610
commit
e2709ac408
@ -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;
|
||||||
|
|||||||
@ -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
|
||||||
@ -491,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.
|
||||||
@ -512,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;
|
||||||
@ -930,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;
|
||||||
}
|
}
|
||||||
|
|
||||||
/*----------------------------------------------------------------------------*/
|
/*----------------------------------------------------------------------------*/
|
||||||
@ -1293,7 +1296,7 @@ static int isaligned_fast(int angle, float theta,
|
|||||||
{
|
{
|
||||||
float a;
|
float a;
|
||||||
|
|
||||||
if (angle == NOTDEF_INT) return FALSE;
|
if (angle == NOTDEF_INT) return FALSE; // faster to test the integer value
|
||||||
|
|
||||||
/* angle at pixel (x,y) */
|
/* angle at pixel (x,y) */
|
||||||
a = degToRad(angle);
|
a = degToRad(angle);
|
||||||
@ -1722,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
|
||||||
@ -1741,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' */
|
||||||
@ -1875,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_fast((float)angles->data[(i->y*angles->xsize)+i->x], 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 */
|
||||||
}
|
}
|
||||||
@ -2014,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.");
|
||||||
@ -2032,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;
|
||||||
|
|
||||||
@ -2113,13 +2179,13 @@ static void region_grow( int x, int y, image_int angles, struct lsd_point * reg,
|
|||||||
/* 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 */
|
||||||
l_size = 1;
|
l_size = 1;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user