mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
Apriltag optimizations.
This commit is contained in:
parent
73c64abb22
commit
33655b9ece
@ -13,6 +13,9 @@
|
|||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include "imlib.h"
|
#include "imlib.h"
|
||||||
|
|
||||||
|
// Enable new code optimizations
|
||||||
|
#define OPTIMIZED
|
||||||
|
|
||||||
#ifdef IMLIB_ENABLE_APRILTAGS
|
#ifdef IMLIB_ENABLE_APRILTAGS
|
||||||
#pragma GCC diagnostic push
|
#pragma GCC diagnostic push
|
||||||
#pragma GCC diagnostic ignored "-Wunused-variable"
|
#pragma GCC diagnostic ignored "-Wunused-variable"
|
||||||
@ -10126,6 +10129,34 @@ int fit_quad(apriltag_detector_t *td, image_u8_t *im, zarray_t *cluster, struct
|
|||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#ifdef OPTIMIZED
|
||||||
|
#define DO_UNIONFIND(dx, dy) if (im->buf[y*s + dy*s + x + dx] == v) { broot = unionfind_get_representative(uf, y*w + dy*w + x + dx); if (aroot != broot) uf->data[broot].parent = aroot; }
|
||||||
|
|
||||||
|
static void do_unionfind_line(unionfind_t *uf, image_u8_t *im, int h, int w, int s, int y)
|
||||||
|
{
|
||||||
|
assert(y+1 < im->height);
|
||||||
|
uint8_t v, *p;
|
||||||
|
p = &im->buf[y*s + 1];
|
||||||
|
for (int x = 1; x < w - 1; x++) {
|
||||||
|
v = *p++; //im->buf[y*s + x];
|
||||||
|
|
||||||
|
if (v == 127)
|
||||||
|
continue;
|
||||||
|
uint32_t broot;
|
||||||
|
uint32_t aroot = unionfind_get_representative(uf, y*w+x);
|
||||||
|
// (dx,dy) pairs for 8 connectivity:
|
||||||
|
// (REFERENCE) (1, 0)
|
||||||
|
// (-1, 1) (0, 1) (1, 1)
|
||||||
|
//
|
||||||
|
DO_UNIONFIND(1, 0);
|
||||||
|
DO_UNIONFIND(0, 1);
|
||||||
|
if (v == 255) {
|
||||||
|
DO_UNIONFIND(-1, 1);
|
||||||
|
DO_UNIONFIND(1, 1);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#else // not optimized
|
||||||
#define DO_UNIONFIND(dx, dy) if (im->buf[y*s + dy*s + x + dx] == v) unionfind_connect(uf, y*w + x, y*w + dy*w + x + dx);
|
#define DO_UNIONFIND(dx, dy) if (im->buf[y*s + dy*s + x + dx] == v) unionfind_connect(uf, y*w + x, y*w + dy*w + x + dx);
|
||||||
|
|
||||||
static void do_unionfind_line(unionfind_t *uf, image_u8_t *im, int h, int w, int s, int y)
|
static void do_unionfind_line(unionfind_t *uf, image_u8_t *im, int h, int w, int s, int y)
|
||||||
@ -10151,6 +10182,7 @@ static void do_unionfind_line(unionfind_t *uf, image_u8_t *im, int h, int w, int
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
#undef DO_UNIONFIND
|
#undef DO_UNIONFIND
|
||||||
|
#endif // OPTIMIZED
|
||||||
|
|
||||||
image_u8_t *threshold(apriltag_detector_t *td, image_u8_t *im)
|
image_u8_t *threshold(apriltag_detector_t *td, image_u8_t *im)
|
||||||
{
|
{
|
||||||
@ -10201,12 +10233,34 @@ image_u8_t *threshold(apriltag_detector_t *td, image_u8_t *im)
|
|||||||
// first, collect min/max statistics for each tile
|
// first, collect min/max statistics for each tile
|
||||||
for (int ty = 0; ty < th; ty++) {
|
for (int ty = 0; ty < th; ty++) {
|
||||||
for (int tx = 0; tx < tw; tx++) {
|
for (int tx = 0; tx < tw; tx++) {
|
||||||
|
#if defined( OPTIMIZED ) && (defined(ARM_MATH_CM7) || defined(ARM_MATH_CM4))
|
||||||
|
uint32_t tmp, max32 = 0, min32 = 0xffffffff;
|
||||||
|
for (int dy=0; dy < tilesz; dy++) {
|
||||||
|
uint32_t v = *(uint32_t *)&im->buf[(ty*tilesz+dy)*s + tx*tilesz];
|
||||||
|
tmp = __USUB8(v, max32);
|
||||||
|
max32 = __SEL(v, max32);
|
||||||
|
tmp = __USUB8(min32, v);
|
||||||
|
min32 = __SEL(v, min32);
|
||||||
|
}
|
||||||
|
// find the min/max of the 4 remaining values
|
||||||
|
tmp = max32 >> 16;
|
||||||
|
__USUB8(max32, tmp); // 4->2
|
||||||
|
max32 = __SEL(max32, tmp);
|
||||||
|
tmp = max32 >> 8;
|
||||||
|
__USUB8(max32, tmp); // 2->1
|
||||||
|
max32 = __SEL(max32, tmp);
|
||||||
|
tmp = min32 >> 16;
|
||||||
|
__USUB8(min32, tmp);
|
||||||
|
min32 = __SEL(tmp, min32); // 4-->2
|
||||||
|
tmp = min32 >> 8;
|
||||||
|
__USUB8(min32, tmp);
|
||||||
|
min32 = __SEL(tmp, min32); // 2-->1
|
||||||
|
im_max[ty*tw+tx] = (uint8_t)max32;
|
||||||
|
im_min[ty*tw+tx] = (uint8_t)min32;
|
||||||
|
#else
|
||||||
uint8_t max = 0, min = 255;
|
uint8_t max = 0, min = 255;
|
||||||
|
|
||||||
for (int dy = 0; dy < tilesz; dy++) {
|
for (int dy = 0; dy < tilesz; dy++) {
|
||||||
|
|
||||||
for (int dx = 0; dx < tilesz; dx++) {
|
for (int dx = 0; dx < tilesz; dx++) {
|
||||||
|
|
||||||
uint8_t v = im->buf[(ty*tilesz+dy)*s + tx*tilesz + dx];
|
uint8_t v = im->buf[(ty*tilesz+dy)*s + tx*tilesz + dx];
|
||||||
if (v < min)
|
if (v < min)
|
||||||
min = v;
|
min = v;
|
||||||
@ -10214,9 +10268,9 @@ image_u8_t *threshold(apriltag_detector_t *td, image_u8_t *im)
|
|||||||
max = v;
|
max = v;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
im_max[ty*tw+tx] = max;
|
im_max[ty*tw+tx] = max;
|
||||||
im_min[ty*tw+tx] = min;
|
im_min[ty*tw+tx] = min;
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -10256,7 +10310,46 @@ image_u8_t *threshold(apriltag_detector_t *td, image_u8_t *im)
|
|||||||
fb_free(); // im_min_tmp
|
fb_free(); // im_min_tmp
|
||||||
fb_free(); // im_max_tmp
|
fb_free(); // im_max_tmp
|
||||||
}
|
}
|
||||||
|
#if defined( OPTIMIZED ) && (defined(ARM_MATH_CM7) || defined(ARM_MATH_CM4))
|
||||||
|
if ((s & 0x3) == 0 && tilesz == 4) // if each line is a multiple of 4, we can do this faster
|
||||||
|
{
|
||||||
|
const uint32_t lowcontrast = 0x7f7f7f7f;
|
||||||
|
const int s32 = s/4; // pitch for 32-bit values
|
||||||
|
const int minmax = td->qtp.min_white_black_diff; // local var to avoid constant dereferencing of the pointer
|
||||||
|
for (int ty = 0; ty < th; ty++) {
|
||||||
|
for (int tx = 0; tx < tw; tx++) {
|
||||||
|
|
||||||
|
int min = im_min[ty*tw + tx];
|
||||||
|
int max = im_max[ty*tw + tx];
|
||||||
|
|
||||||
|
// low contrast region? (no edges)
|
||||||
|
if (max - min < minmax) {
|
||||||
|
uint32_t *d32 = (uint32_t *)&threshim->buf[ty*tilesz*s + tx*tilesz];
|
||||||
|
d32[0] = d32[s32] = d32[s32*2] = d32[s32*3] = lowcontrast;
|
||||||
|
continue;
|
||||||
|
} // if low contrast
|
||||||
|
// otherwise, actually threshold this tile.
|
||||||
|
|
||||||
|
// argument for biasing towards dark; specular highlights
|
||||||
|
// can be substantially brighter than white tag parts
|
||||||
|
uint32_t thresh32 = (min + (max - min) / 2) + 1; // plus 1 to make GT become GE for the __USUB8 and __SEL instructions
|
||||||
|
uint32_t u32tmp;
|
||||||
|
thresh32 *= 0x01010101; // spread value to all 4 slots
|
||||||
|
for (int dy = 0; dy < tilesz; dy++) {
|
||||||
|
uint32_t *d32 = (uint32_t *)&threshim->buf[(ty*tilesz+dy)*s + tx*tilesz];
|
||||||
|
uint32_t *s32 = (uint32_t *)&im->buf[(ty*tilesz+dy)*s + tx*tilesz];
|
||||||
|
// process 4 pixels at a time
|
||||||
|
u32tmp = s32[0];
|
||||||
|
u32tmp = __USUB8(u32tmp, thresh32);
|
||||||
|
u32tmp = __SEL(0xffffffff, 0x00000000); // 4 thresholded pixels
|
||||||
|
d32[0] = u32tmp;
|
||||||
|
} // dy
|
||||||
|
} // tx
|
||||||
|
} // ty
|
||||||
|
}
|
||||||
|
else // need to do it the slow way
|
||||||
|
#endif // OPTIMIZED
|
||||||
|
{
|
||||||
for (int ty = 0; ty < th; ty++) {
|
for (int ty = 0; ty < th; ty++) {
|
||||||
for (int tx = 0; tx < tw; tx++) {
|
for (int tx = 0; tx < tw; tx++) {
|
||||||
|
|
||||||
@ -10298,6 +10391,7 @@ image_u8_t *threshold(apriltag_detector_t *td, image_u8_t *im)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// we skipped over the non-full-sized tiles above. Fix those now.
|
// we skipped over the non-full-sized tiles above. Fix those now.
|
||||||
if (1) {
|
if (1) {
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user