AprilTags C++

Avevo gia' usato gli Apriltags per stimare le distanze 

Stavolta ho modificato il programma di esempio opencv_demo di Apriltag in modo da estrarre le matrici di rotazione e traslazione 

Per cercare di minimizzare il rumore di misura si sono fatto un dodecaedro con le fatte ricoperte di apriltag (le distanze relative dei tag sulle facce devono rimanere costanti ed dodecaedro permette sempre di esporre al minimo 2 ma quasi sempre 3 facce)

/* Copyright (C) 2013-2016, The Regents of The University of Michigan.

All rights reserved.

This software was developed in the APRIL Robotics Lab under the

direction of Edwin Olson, ebolson@umich.edu. This software may be

available under alternative licensing terms; contact the address above.

Redistribution and use in source and binary forms, with or without

modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this

list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice,

this list of conditions and the following disclaimer in the documentation

and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND

ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED

WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE

DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR

ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES

(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND

ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT

(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS

SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

The views and conclusions contained in the software and documentation are those

of the authors and should not be interpreted as representing official policies,

either expressed or implied, of the Regents of The University of Michigan.

*/

#include <stdio.h>

#include <stdint.h>

#include <inttypes.h>

#include <ctype.h>

#include <math.h>

#include <errno.h>

#ifdef __linux__

#include <unistd.h>

#endif

#include "apriltag.h"

#include "tag36h11.h"

#include "tag25h9.h"

#include "tag16h5.h"

#include "tagCircle21h7.h"

#include "tagCircle49h12.h"

#include "tagCustom48h12.h"

#include "tagStandard41h12.h"

#include "tagStandard52h13.h"

#include "common/getopt.h"

#include "common/image_u8.h"

#include "common/pjpeg.h"

#include "common/zarray.h"

#include "apriltag_pose.h"

#define HAMM_HIST_MAX 10

int main(int argc, char *argv[])

{

getopt_t *getopt = getopt_create();

getopt_add_bool(getopt, 'h', "help", 0, "Show this help");

getopt_add_bool(getopt, 'd', "debug", 0, "Enable debugging output (slow)");

getopt_add_bool(getopt, 'q', "quiet", 0, "Reduce output");

getopt_add_string(getopt, 'f', "family", "tag36h11", "Tag family to use");

getopt_add_int(getopt, 'i', "iters", "1", "Repeat processing on input set this many times");

getopt_add_int(getopt, 't', "threads", "1", "Use this many CPU threads");

getopt_add_int(getopt, 'a', "hamming", "1", "Detect tags with up to this many bit errors.");

getopt_add_double(getopt, 'x', "decimate", "2.0", "Decimate input image by this factor");

getopt_add_double(getopt, 'b', "blur", "0.0", "Apply low-pass blur to input; negative sharpens");

getopt_add_bool(getopt, '0', "refine-edges", 1, "Spend more time trying to align edges of tags");

if (!getopt_parse(getopt, argc, argv, 1) || getopt_get_bool(getopt, "help")) {

printf("Usage: %s [options] <input files>\n", argv[0]);

getopt_do_usage(getopt);

exit(0);

}

const zarray_t *inputs = getopt_get_extra_args(getopt);

apriltag_family_t *tf = NULL;

const char *famname = getopt_get_string(getopt, "family");

if (!strcmp(famname, "tag36h11")) {

tf = tag36h11_create();

} else if (!strcmp(famname, "tag25h9")) {

tf = tag25h9_create();

} else if (!strcmp(famname, "tag16h5")) {

tf = tag16h5_create();

} else if (!strcmp(famname, "tagCircle21h7")) {

tf = tagCircle21h7_create();

} else if (!strcmp(famname, "tagCircle49h12")) {

tf = tagCircle49h12_create();

} else if (!strcmp(famname, "tagStandard41h12")) {

tf = tagStandard41h12_create();

} else if (!strcmp(famname, "tagStandard52h13")) {

tf = tagStandard52h13_create();

} else if (!strcmp(famname, "tagCustom48h12")) {

tf = tagCustom48h12_create();

} else {

printf("Unrecognized tag family name. Use e.g. \"tag36h11\".\n");

exit(-1);

}

apriltag_detector_t *td = apriltag_detector_create();

apriltag_detector_add_family_bits(td, tf, getopt_get_int(getopt, "hamming"));

switch(errno){

case EINVAL:

printf("\"hamming\" parameter is out-of-range.\n");

exit(-1);

case ENOMEM:

printf("Unable to add family to detector due to insufficient memory to allocate the tag-family decoder. Try reducing \"hamming\" from %d or choose an alternative tag family.\n", getopt_get_int(getopt, "hamming"));

exit(-1);

}

td->quad_decimate = getopt_get_double(getopt, "decimate");

td->quad_sigma = getopt_get_double(getopt, "blur");

td->nthreads = getopt_get_int(getopt, "threads");

td->debug = getopt_get_bool(getopt, "debug");

td->refine_edges = getopt_get_bool(getopt, "refine-edges");

int quiet = getopt_get_bool(getopt, "quiet");

int maxiters = getopt_get_int(getopt, "iters");

for (int iter = 0; iter < maxiters; iter++) {

int total_quads = 0;

int total_hamm_hist[HAMM_HIST_MAX];

memset(total_hamm_hist, 0, sizeof(int)*HAMM_HIST_MAX);

double total_time = 0;

if (maxiters > 1)

printf("iter %d / %d\n", iter + 1, maxiters);

for (int input = 0; input < zarray_size(inputs); input++) {

int hamm_hist[HAMM_HIST_MAX];

memset(hamm_hist, 0, sizeof(hamm_hist));

char *path;

zarray_get(inputs, input, &path);

if (!quiet)

printf("loading %s\n", path);

else

printf("%20s ", path);

image_u8_t *im = NULL;

if (str_ends_with(path, "pnm") || str_ends_with(path, "PNM") ||

str_ends_with(path, "pgm") || str_ends_with(path, "PGM"))

im = image_u8_create_from_pnm(path);

else if (str_ends_with(path, "jpg") || str_ends_with(path, "JPG")) {

int err = 0;

pjpeg_t *pjpeg = pjpeg_create_from_file(path, 0, &err);

if (pjpeg == NULL) {

printf("pjpeg failed to load: %s, error %d\n", path, err);

continue;

}

if (1) {

im = pjpeg_to_u8_baseline(pjpeg);

} else {

printf("illumination invariant\n");

image_u8x3_t *imc = pjpeg_to_u8x3_baseline(pjpeg);

im = image_u8_create(imc->width, imc->height);

for (int y = 0; y < imc->height; y++) {

for (int x = 0; x < imc->width; x++) {

double r = imc->buf[y*imc->stride + 3*x + 0] / 255.0;

double g = imc->buf[y*imc->stride + 3*x + 1] / 255.0;

double b = imc->buf[y*imc->stride + 3*x + 2] / 255.0;

double alpha = 0.42;

double v = 0.5 + log(g) - alpha*log(b) - (1-alpha)*log(r);

int iv = v * 255;

if (iv < 0)

iv = 0;

if (iv > 255)

iv = 255;

im->buf[y*im->stride + x] = iv;

}

}

image_u8x3_destroy(imc);

if (td->debug)

image_u8_write_pnm(im, "debug_invariant.pnm");

}

pjpeg_destroy(pjpeg);

}

if (im == NULL) {

printf("couldn't load %s\n", path);

continue;

}

printf("image: %s %dx%d\n", path, im->width, im->height);

zarray_t *detections = apriltag_detector_detect(td, im);

if (errno == EAGAIN) {

printf("Unable to create the %d threads requested.\n",td->nthreads);

exit(-1);

}

for (int i = 0; i < zarray_size(detections); i++) {

apriltag_detection_t *det;

zarray_get(detections, i, &det);

// ********************************************************

/*apriltag_detection_info_t info;

info.det = det;

info.tagsize = 5; //dimensione del tag

info.fx = 10; // lunghezza focale camera in pixels asse x

info.fy = 10; // lunghezza focale camera in pixels asse y

info.cx = 10; // centro focale camera in pixels asse x

info.cy = 10; // centro focale camera in pixels asse x

*/

//apriltag_pose_t pose;

//double err = estimate_tag_pose(&info, &pose);

//double x = pose.R->data[0];

//double y = pose.R->data[1];

//double z = pose.R->data[2];

//double translation_on_axes_1 = pose.t->data[0];

//double translation_on_axes_2 = pose.t->data[1];

//double translation_on_axes_3 = pose.t->data[3];

// ********************************************************

if (!quiet)

printf("detection %3d: id (%2dx%2d)-%-4d, hamming %d, margin %8.3f\n",

i, det->family->nbits, det->family->h, det->id, det->hamming, det->decision_margin);

hamm_hist[det->hamming]++;

total_hamm_hist[det->hamming]++;

}

apriltag_detections_destroy(detections);

if (!quiet) {

timeprofile_display(td->tp);

}

total_quads += td->nquads;

if (!quiet)

printf("hamm ");

for (int i = 0; i < HAMM_HIST_MAX; i++)

printf("%5d ", hamm_hist[i]);

double t = timeprofile_total_utime(td->tp) / 1.0E3;

total_time += t;

printf("%12.3f ", t);

printf("%5d", td->nquads);

printf("\n");

image_u8_destroy(im);

}

printf("Summary\n");

printf("hamm ");

for (int i = 0; i < HAMM_HIST_MAX; i++)

printf("%5d ", total_hamm_hist[i]);

printf("%12.3f ", total_time);

printf("%5d", total_quads);

printf("\n");

}

// don't deallocate contents of inputs; those are the argv

apriltag_detector_destroy(td);

if (!strcmp(famname, "tag36h11")) {

tag36h11_destroy(tf);

} else if (!strcmp(famname, "tag25h9")) {

tag25h9_destroy(tf);

} else if (!strcmp(famname, "tag16h5")) {

tag16h5_destroy(tf);

} else if (!strcmp(famname, "tagCircle21h7")) {

tagCircle21h7_destroy(tf);

} else if (!strcmp(famname, "tagCircle49h12")) {

tagCircle49h12_destroy(tf);

} else if (!strcmp(famname, "tagStandard41h12")) {

tagStandard41h12_destroy(tf);

} else if (!strcmp(famname, "tagStandard52h13")) {

tagStandard52h13_destroy(tf);

} else if (!strcmp(famname, "tagCustom48h12")) {

tagCustom48h12_destroy(tf);

}

getopt_destroy(getopt);

return 0;

}

Aruco vs Apriltag condizioni reali

Aggiornamento:

Dopo aver provato un po' di tutto per correggere i dati ho scoperto che le immagini originali non sono state riprese in modo corretto. La camera satura in alcuni condizioni di luce come si vede dai due esempi sottostanti rendendo inutile l'elaborazione

Prova comparativa per misuare distanze tramite April ed Aruco tag in condizioni reali 

Lo scopo e' quello di verificare la ripetibilita' delle misure di distanza mediante tag a condizioni di luce variabile e per questo sono state effettuate misure con i tag in posizione stazionaria

E' stata impiegata una camera di sorveglianza a fuoco fisso con acquisizione ogni 10 minuti anche di notte grazie all'illuminazione ad infrarossi

I tag sono stati di dimensione di 25 cm in formato 4x4 per Aruco e 36h11 per gli Apriltag

Per determinare la distanza sono state impiegate le librerie OpenCV su Python per agli Aruco Tags mentre la libreria Apriltags3 in C++

in generale gli Apriltag risultano meglio individuabili rispetto agli Aruco tag. Di 430 immagini totali gli Apriltags sono stati individuati al 99% ad una distanza di 6 m mentre gli Aruco tag hanno prestazioni simili ma solo sulla breve distanza (4 m)

Aruco

l'elaborazione dei tag aruco indica che l'algoritmo genera molti outliers che possono essere facilmente separati dai dati corretti

Provando a smussare i dati con una media mobile a finetra oraria la situazione non migliora e si osserva un comportamento legato all'illuminazione che si ritrovera' anche dopo con gli April Tags 

Apriltag

Dall'analisi dei grafici si vede che le condizioni di illuminazione condizionano fortemente la misura della distanza mediante Apriltag. Le misure piu' stabili sono di notte quando e' attiva l'illuminazione dei led ad infrarossi

Rispetto ad Aruco ci sono molti meno outliers ed i dati sono meno rumorosi

In queste condizioni e' stato registrato un errore di standard devitation pari a 0.96% della distanza per il tag a 6.5m e dell'1% per il tag a 10 m

Se si plottano i dati a parita' di ora del giorno si vede ancora piu' chiaramente come la presenza di ombra influenza il dato di distanza

Se si fa la differenza tra le due curve l'errore scende al 0.18%

Un sistema per rimuovere l'effetto dell'illuminazione e' di correlare i dati dei due tag (sono vicini quindi sono illuminati in modo comparabile)

Per cercare di risolvere il problema delle differenti illuminazione ho provato ad elaborare le immagini mediante l'algoritmo presentato in questo articolo (Illumination Invariant Imaging: Applications in Robust Vision-based Localisation, Mapping and Classification for Autonomous Vehicles)

In estrema sintesi l'algoritmo appiattisce una immagine RGB e cerca di annullare gli effetti di differente illuminaizone (questo algoritmo funziona solo sulle immagini diurne perche' la camera di notte acquisisce a scala di grigi)

Per le elaborazioni ho usato questo progetto su Github.  (ho dovuto fare una leggere modifica perche' le immagini della camera avevano dei valori zero che ovviamente non potevano essere usati in un logaritmo)

import cv2

import numpy as np

import os

from argparse import ArgumentParser

from multiprocessing import Pool

ROBOTCAR_ALPHA = 0.4642

def transform(im):

# Assumes image is RGB-ordered

image = cv2.imread(read_dir + "/" + im, cv2.IMREAD_UNCHANGED)

r, g, b = image[:, :, 0], image[:, :, 1], image[:, :, 2]

// per eliminare gli eventuali zeri dalla matrice

r = np.where(r==0, 0.1, r)

g = np.where(g==0, 0.1, g)

b = np.where(b==0, 0.1, b)

ii_image = 0.5 + np.log(g) - ROBOTCAR_ALPHA*np.log(b) - (1-ROBOTCAR_ALPHA)*np.log(r)

# Lastly, convert from float to uint8 space

max_ii = np.max(ii_image)

min_ii = np.min(ii_image)

uii = np.uint8((ii_image - min_ii) * 256 / (max_ii - min_ii))

ii_name = write_dir + "/" + im

cv2.imwrite(ii_name, uii)

return ii_image

def transform_loop(directory):

image_names = os.listdir(directory)

# Spawn 4 worker processes to transform in parallel

p = Pool(4)

p.map(transform, image_names)

if __name__ == '__main__':

parser = ArgumentParser(

description=

'Transform images in a directory into lighting invariant color space')

parser.add_argument('--read-dir', action="store", type=str, required=True)

parser.add_argument('--write-dir', action="store", type=str, required=True)

args = parser.parse_args()

global read_dir, write_dir

read_dir = args.read_dir

write_dir = args.write_dir

transform_loop(read_dir)

dopo l'applicazione della elaborazione l'algoritmo di riconoscimento dei tag risulta molto piu' in difficolta' nel riconoscere i taf e sono state estratte solo 71 misure di distanza del tag1 e 16 misure del tag 2

Aggiornamento:

Frugando dentro al codice della demo di Apritag3 c'e' un porzione di codice che non puo' mai essere eseguito (c'e' un IF sempre True) e la condizione Falsa e' appunto l'algoritmo di Illumination Invariant 

Basta modificare la riga 156 per esempio aggiungendo un NOT si introduce il calcolo

questi sono i grafici risultanti dopo l'algoritmo. Si e' oersa la ritmicita' dell'illuminazione ma si e' persa anche la capacita' di riconoscere i tag nelle immagini trattate (per il tag 1 circa il 50%, tag2 decisamente peggio)

L'errore percentuale delle standard deviation e' pari a 1.79% per il tag1 e 1.08% per il tag 2

La differenza risiede nel valore del parametro utilizzato nell'elaborazione delle immagini

Apriltag 3 48h12

Per vedere se c'era un motivo per scegliere una diversa famiglia di Apriltag rispetto al 36h11 ho provato la famiglia consigliata 48h12 (che rientra nelle custom) 

La standard deviation su circa 200 misure (un giorno) e' stata dello 0.25% (1.15 cm ad una distanza di 4.6 m) con un lievissimo miglioramento rispetto alle prove precedenti

ps : per usare la famiglia Custom48h12 si deve modificare il codice del file di esempio

Confronto ArucoTags ed AprilTags

 Ho provato una comparazione sulla stabilita' della misura di distanza usando Aruco Tags (con Opencv) e AprilTags (con AprilTag v2 e v3 modificata per avere la pose estimation)

Sono state acquisite 576 immagini con una camera Reolink E1 Pro (con immagini sia notturne che diurne) di due tag delle medesime dimensioni (4x4_100 per Aruco e 36h11 per Apriltag 10 cm)

Le nuove versioni di OpenCV hanno modificate le API per cui gli script che ho usato l'anno scorso non funzionano piu'. Per fare prima ho usato il docker sottostante che monta OpenCV 4.5.5

docker run -it -v /home/luca/ottico/:/home/ottico hdgigante/python-opencv:4.5.5-debian  sh

OpenCV ed Aruco mostrano outliers, Apriltag3 ne mostra solo uno mentre Apriltag 2 ha mostrato dati molto piu' stabili)

Rimossi gli outlier si evidenzia che l'errore quadratico medio di Arpriltag e' circa la meta' di quella di Aruco (l'errore quadratico  migliore e' circa lo 0,3 % della distanza)

Apriltag

 Dopo questa prova con Aruco Tags ho provato un progetto con AprilTag basato sulla libreria basato sulla libreria https://github.com/reinhartmeg/AprilTag-Distance-Calculator modificato per renderlo compatibile con OpenCV4

https://github.com/c1p81/distance_apriltag