mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
Merge pull request #576 from kwagyeman/kwabena/apriltag_fix
Allow apriltag detection on vflip and hmirrored images.
This commit is contained in:
commit
5c4cfb61bd
@ -10632,6 +10632,8 @@ struct quick_decode_entry
|
||||
uint16_t id; // the tag ID (a small integer)
|
||||
uint8_t hamming; // how many errors corrected?
|
||||
uint8_t rotation; // number of rotations [0, 3]
|
||||
bool hmirror;
|
||||
bool vflip;
|
||||
};
|
||||
|
||||
struct quick_decode
|
||||
@ -10666,6 +10668,42 @@ static uint64_t rotate90(uint64_t w, uint32_t d)
|
||||
return wr;
|
||||
}
|
||||
|
||||
static uint64_t hmirror_code(uint64_t w, uint32_t d)
|
||||
{
|
||||
uint64_t wr = 0;
|
||||
|
||||
for (int32_t r = d-1; r >=0; r--) {
|
||||
for (int32_t c = 0; c < d; c++) {
|
||||
int32_t b = c + d*r;
|
||||
|
||||
wr = wr << 1;
|
||||
|
||||
if ((w & (((uint64_t) 1) << b))!=0)
|
||||
wr |= 1;
|
||||
}
|
||||
}
|
||||
|
||||
return wr;
|
||||
}
|
||||
|
||||
static uint64_t vflip_code(uint64_t w, uint32_t d)
|
||||
{
|
||||
uint64_t wr = 0;
|
||||
|
||||
for (int32_t r = 0; r < d; r++) {
|
||||
for (int32_t c = d-1; c >=0; c--) {
|
||||
int32_t b = c + d*r;
|
||||
|
||||
wr = wr << 1;
|
||||
|
||||
if ((w & (((uint64_t) 1) << b))!=0)
|
||||
wr |= 1;
|
||||
}
|
||||
}
|
||||
|
||||
return wr;
|
||||
}
|
||||
|
||||
void quad_destroy(struct quad *quad)
|
||||
{
|
||||
if (!quad)
|
||||
@ -10754,6 +10792,68 @@ static void quick_decode_codeword(apriltag_family_t *tf, uint64_t rcode,
|
||||
entry->id = i;
|
||||
entry->hamming = hamming;
|
||||
entry->rotation = ridx;
|
||||
entry->hmirror = false;
|
||||
entry->vflip = false;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
rcode = rotate90(rcode, tf->d);
|
||||
}
|
||||
|
||||
rcode = hmirror_code(rcode, tf->d); // handle hmirror
|
||||
|
||||
for (int ridx = 0; ridx < 4; ridx++) {
|
||||
|
||||
for (int i = 0, j = tf->ncodes; i < j; i++) {
|
||||
int hamming = popcount64c(tf->codes[i] ^ rcode);
|
||||
if(hamming <= threshold) {
|
||||
entry->rcode = rcode;
|
||||
entry->id = i;
|
||||
entry->hamming = hamming;
|
||||
entry->rotation = ridx;
|
||||
entry->hmirror = true;
|
||||
entry->vflip = false;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
rcode = rotate90(rcode, tf->d);
|
||||
}
|
||||
|
||||
rcode = vflip_code(rcode, tf->d); // handle hmirror+vflip
|
||||
|
||||
for (int ridx = 0; ridx < 4; ridx++) {
|
||||
|
||||
for (int i = 0, j = tf->ncodes; i < j; i++) {
|
||||
int hamming = popcount64c(tf->codes[i] ^ rcode);
|
||||
if(hamming <= threshold) {
|
||||
entry->rcode = rcode;
|
||||
entry->id = i;
|
||||
entry->hamming = hamming;
|
||||
entry->rotation = ridx;
|
||||
entry->hmirror = true;
|
||||
entry->vflip = true;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
rcode = rotate90(rcode, tf->d);
|
||||
}
|
||||
|
||||
rcode = hmirror_code(rcode, tf->d); // handle vflip
|
||||
|
||||
for (int ridx = 0; ridx < 4; ridx++) {
|
||||
|
||||
for (int i = 0, j = tf->ncodes; i < j; i++) {
|
||||
int hamming = popcount64c(tf->codes[i] ^ rcode);
|
||||
if(hamming <= threshold) {
|
||||
entry->rcode = rcode;
|
||||
entry->id = i;
|
||||
entry->hamming = hamming;
|
||||
entry->rotation = ridx;
|
||||
entry->hmirror = false;
|
||||
entry->vflip = true;
|
||||
return;
|
||||
}
|
||||
}
|
||||
@ -10765,6 +10865,8 @@ static void quick_decode_codeword(apriltag_family_t *tf, uint64_t rcode,
|
||||
entry->id = 65535;
|
||||
entry->hamming = 255;
|
||||
entry->rotation = 0;
|
||||
entry->hmirror = false;
|
||||
entry->vflip = false;
|
||||
}
|
||||
|
||||
static inline int detection_compare_function(const void *_a, const void *_b)
|
||||
@ -11492,9 +11594,21 @@ zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig)
|
||||
MATD_EL(R, 1, 1) = c;
|
||||
MATD_EL(R, 2, 2) = 1;
|
||||
|
||||
det->H = matd_op("M*M", quad->H, R);
|
||||
matd_t *RHMirror = matd_create(3,3);
|
||||
MATD_EL(RHMirror, 0, 0) = entry.hmirror ? -1 : 1;
|
||||
MATD_EL(RHMirror, 1, 1) = 1;
|
||||
MATD_EL(RHMirror, 2, 2) = entry.hmirror ? -1 : 1;
|
||||
|
||||
matd_t *RVFlip = matd_create(3,3);
|
||||
MATD_EL(RVFlip, 0, 0) = 1;
|
||||
MATD_EL(RVFlip, 1, 1) = entry.vflip ? -1 : 1;
|
||||
MATD_EL(RVFlip, 2, 2) = entry.vflip ? -1 : 1;
|
||||
|
||||
det->H = matd_op("M*M*M*M", quad->H, R, RHMirror, RVFlip);
|
||||
|
||||
matd_destroy(R);
|
||||
matd_destroy(RHMirror);
|
||||
matd_destroy(RVFlip);
|
||||
|
||||
homography_project(det->H, 0, 0, &det->c[0], &det->c[1]);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user