Merge pull request #576 from kwagyeman/kwabena/apriltag_fix

Allow apriltag detection on vflip and hmirrored images.
This commit is contained in:
Ibrahim Abd Elkader 2019-09-03 13:21:52 +02:00 committed by GitHub
commit 5c4cfb61bd
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23

View File

@ -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]);