/* * Copyright (C) 2013-2016, The Regents of The University of Michigan. * * 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. */ // *INDENT-OFF* #include #include #include #include "imlib.h" // Enable new code optimizations #define OPTIMIZED #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" #define fprintf(format, ...) #define free(ptr) ({ umm_free(ptr); }) #define malloc(size) ({ void *_r = umm_malloc(size); if(!_r) umm_alloc_fail(); _r; }) #define realloc(ptr, size) ({ void *_r = umm_realloc((ptr), (size)); if(!_r) umm_alloc_fail(); _r; }) #define calloc(num, item_size) ({ void *_r = umm_calloc((num), (item_size)); if(!_r) umm_alloc_fail(); _r; }) #undef assert #define assert(expression) #define sqrt(x) fast_sqrtf(x) #define sqrtf(x) fast_sqrtf(x) #define floor(x) fast_floorf(x) #define floorf(x) fast_floorf(x) #define ceil(x) fast_ceilf(x) #define ceilf(x) fast_ceilf(x) #define round(x) fast_roundf(x) #define roundf(x) fast_roundf(x) #define atan(x) fast_atanf(x) #define atanf(x) fast_atanf(x) #define atan2(y, x) fast_atan2f((y), (x)) #define atan2f(y, x) fast_atan2f((y), (x)) #define exp(x) fast_expf(x) #define expf(x) fast_expf(x) #define cbrt(x) fast_cbrtf(x) #define cbrtf(x) fast_cbrtf(x) #define fabs(x) fast_fabsf(x) #define fabsf(x) fast_fabsf(x) #define log(x) fast_log(x) #define logf(x) fast_log(x) #undef log2 #define log2(x) fast_log2(x) #undef log2f #define log2f(x) fast_log2(x) #define fmin(a, b) ({ __typeof__ (a) _a = (a); __typeof__ (b) _b = (b); _a < _b ? _a : _b; }) #define fminf(a, b) ({ __typeof__ (a) _a = (a); __typeof__ (b) _b = (b); _a < _b ? _a : _b; }) #define fmax(a, b) ({ __typeof__ (a) _a = (a); __typeof__ (b) _b = (b); _a > _b ? _a : _b; }) #define fmaxf(a, b) ({ __typeof__ (a) _a = (a); __typeof__ (b) _b = (b); _a > _b ? _a : _b; }) //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "zarray.h" //////////////////////////////////////////////////////////////////////////////////////////////////// /** * Defines a structure which acts as a resize-able array ala Java's ArrayList. */ typedef struct zarray zarray_t; struct zarray { size_t el_sz; // size of each element int size; // how many elements? int alloc; // we've allocated storage for how many elements? char *data; }; /** * Creates and returns a variable array structure capable of holding elements of * the specified size. It is the caller's responsibility to call zarray_destroy() * on the returned array when it is no longer needed. */ static inline zarray_t *zarray_create(size_t el_sz) { assert(el_sz > 0); zarray_t *za = (zarray_t*) calloc(1, sizeof(zarray_t)); za->el_sz = el_sz; return za; } /** * Creates and returns a variable array structure capable of holding elements of * the specified size. It is the caller's responsibility to call zarray_destroy() * on the returned array when it is no longer needed. */ static inline zarray_t *zarray_create_fail_ok(size_t el_sz) { assert(el_sz > 0); zarray_t *za = (zarray_t*) umm_calloc(1, sizeof(zarray_t)); if (za) za->el_sz = el_sz; return za; } /** * Frees all resources associated with the variable array structure which was * created by zarray_create(). After calling, 'za' will no longer be valid for storage. */ static inline void zarray_destroy(zarray_t *za) { if (za == NULL) return; if (za->data != NULL) free(za->data); memset(za, 0, sizeof(zarray_t)); free(za); } /** Allocate a new zarray that contains a copy of the data in the argument. **/ static inline zarray_t *zarray_copy(const zarray_t *za) { assert(za != NULL); zarray_t *zb = (zarray_t*) calloc(1, sizeof(zarray_t)); zb->el_sz = za->el_sz; zb->size = za->size; zb->alloc = za->alloc; zb->data = (char*) malloc(zb->alloc * zb->el_sz); memcpy(zb->data, za->data, za->size * za->el_sz); return zb; } static int iceillog2(int v) { v--; v |= v >> 1; v |= v >> 2; v |= v >> 4; v |= v >> 8; v |= v >> 16; v++; return v; } /** * Allocate a new zarray that contains a subset of the original * elements. NOTE: end index is EXCLUSIVE, that is one past the last * element you want. */ static inline zarray_t *zarray_copy_subset(const zarray_t *za, int start_idx, int end_idx_exclusive) { zarray_t *out = (zarray_t*) calloc(1, sizeof(zarray_t)); out->el_sz = za->el_sz; out->size = end_idx_exclusive - start_idx; out->alloc = iceillog2(out->size); // round up pow 2 out->data = (char*) malloc(out->alloc * out->el_sz); memcpy(out->data, za->data +(start_idx*out->el_sz), out->size*out->el_sz); return out; } /** * Retrieves the number of elements currently being contained by the passed * array, which may be different from its capacity. The index of the last element * in the array will be one less than the returned value. */ static inline int zarray_size(const zarray_t *za) { assert(za != NULL); return za->size; } /** * Returns 1 if zarray_size(za) == 0, * returns 0 otherwise. */ /* JUST CALL zarray_size int zarray_isempty(const zarray_t *za) { assert(za != NULL); if (za->size <= 0) return 1; else return 0; } */ /** * Allocates enough internal storage in the supplied variable array structure to * guarantee that the supplied number of elements (capacity) can be safely stored. */ static inline void zarray_ensure_capacity(zarray_t *za, int capacity) { assert(za != NULL); if (capacity <= za->alloc) return; while (za->alloc < capacity) { za->alloc += 8; // use less memory // *= 2; if (za->alloc < 8) za->alloc = 8; } za->data = (char*) realloc(za->data, za->alloc * za->el_sz); } /** * Adds a new element to the end of the supplied array, and sets its value * (by copying) from the data pointed to by the supplied pointer 'p'. * Automatically ensures that enough storage space is available for the new element. */ static inline void zarray_add(zarray_t *za, const void *p) { assert(za != NULL); assert(p != NULL); zarray_ensure_capacity(za, za->size + 1); memcpy(&za->data[za->size*za->el_sz], p, za->el_sz); za->size++; } /** * Adds a new element to the end of the supplied array, and sets its value * (by copying) from the data pointed to by the supplied pointer 'p'. * Automatically ensures that enough storage space is available for the new element. */ static inline void zarray_add_fail_ok(zarray_t *za, const void *p) { assert(za != NULL); assert(p != NULL); if ((za->size + 1) > za->alloc) { char *old_data = za->data; int old_alloc = za->alloc; while (za->alloc < (za->size + 1)) { za->alloc += 8; // use less memory // *= 2; if (za->alloc < 8) za->alloc = 8; } za->data = (char*) umm_realloc(za->data, za->alloc * za->el_sz); if (!za->data) { za->data = old_data; za->alloc = old_alloc; return; } } memcpy(&za->data[za->size*za->el_sz], p, za->el_sz); za->size++; } /** * Retrieves the element from the supplied array located at the zero-based * index of 'idx' and copies its value into the variable pointed to by the pointer * 'p'. */ static inline void zarray_get(const zarray_t *za, int idx, void *p) { assert(za != NULL); assert(p != NULL); assert(idx >= 0); assert(idx < za->size); memcpy(p, &za->data[idx*za->el_sz], za->el_sz); } /** * Similar to zarray_get(), but returns a "live" pointer to the internal * storage, avoiding a memcpy. This pointer is not valid across * operations which might move memory around (i.e. zarray_remove_value(), * zarray_remove_index(), zarray_insert(), zarray_sort(), zarray_clear()). * 'p' should be a pointer to the pointer which will be set to the internal address. */ inline static void zarray_get_volatile(const zarray_t *za, int idx, void *p) { assert(za != NULL); assert(p != NULL); assert(idx >= 0); assert(idx < za->size); *((void**) p) = &za->data[idx*za->el_sz]; } inline static void zarray_truncate(zarray_t *za, int sz) { assert(za != NULL); assert(sz <= za->size); za->size = sz; } /** * Copies the memory array used internally by zarray to store its owned * elements to the address pointed by 'buffer'. It is the caller's responsibility * to allocate zarray_size()*el_sz bytes for the copy to be stored and * to free the memory when no longer needed. The memory allocated at 'buffer' * and the internal zarray storage must not overlap. 'buffer_bytes' should be * the size of the 'buffer' memory space, in bytes, and must be at least * zarray_size()*el_sz. * * Returns the number of bytes copied into 'buffer'. */ static inline size_t zarray_copy_data(const zarray_t *za, void *buffer, size_t buffer_bytes) { assert(za != NULL); assert(buffer != NULL); assert(buffer_bytes >= za->el_sz * za->size); memcpy(buffer, za->data, za->el_sz * za->size); return za->el_sz * za->size; } /** * Removes the entry at index 'idx'. * If shuffle is true, the last element in the array will be placed in * the newly-open space; if false, the zarray is compacted. */ static inline void zarray_remove_index(zarray_t *za, int idx, int shuffle) { assert(za != NULL); assert(idx >= 0); assert(idx < za->size); if (shuffle) { if (idx < za->size-1) memcpy(&za->data[idx*za->el_sz], &za->data[(za->size-1)*za->el_sz], za->el_sz); za->size--; return; } else { // size = 10, idx = 7. Should copy 2 entries (at idx=8 and idx=9). // size = 10, idx = 9. Should copy 0 entries. int ncopy = za->size - idx - 1; if (ncopy > 0) memmove(&za->data[idx*za->el_sz], &za->data[(idx+1)*za->el_sz], ncopy*za->el_sz); za->size--; return; } } /** * Remove the entry whose value is equal to the value pointed to by 'p'. * If shuffle is true, the last element in the array will be placed in * the newly-open space; if false, the zarray is compacted. At most * one element will be removed. * * Note that objects will be compared using memcmp over the full size * of the value. If the value is a struct that contains padding, * differences in the padding bytes can cause comparisons to * fail. Thus, it remains best practice to bzero all structs so that * the padding is set to zero. * * Returns the number of elements removed (0 or 1). */ // remove the entry whose value is equal to the value pointed to by p. // if shuffle is true, the last element in the array will be placed in // the newly-open space; if false, the zarray is compacted. static inline int zarray_remove_value(zarray_t *za, const void *p, int shuffle) { assert(za != NULL); assert(p != NULL); for (int idx = 0; idx < za->size; idx++) { if (!memcmp(p, &za->data[idx*za->el_sz], za->el_sz)) { zarray_remove_index(za, idx, shuffle); return 1; } } return 0; } /** * Creates a new entry and inserts it into the array so that it will have the * index 'idx' (i.e. before the item which currently has that index). The value * of the new entry is set to (copied from) the data pointed to by 'p'. 'idx' * can be one larger than the current max index to place the new item at the end * of the array, or zero to add it to an empty array. */ static inline void zarray_insert(zarray_t *za, int idx, const void *p) { assert(za != NULL); assert(p != NULL); assert(idx >= 0); assert(idx <= za->size); zarray_ensure_capacity(za, za->size + 1); // size = 10, idx = 7. Should copy three entries (idx=7, idx=8, idx=9) int ncopy = za->size - idx; memmove(&za->data[(idx+1)*za->el_sz], &za->data[idx*za->el_sz], ncopy*za->el_sz); memcpy(&za->data[idx*za->el_sz], p, za->el_sz); za->size++; } /** * Sets the value of the current element at index 'idx' by copying its value from * the data pointed to by 'p'. The previous value of the changed element will be * copied into the data pointed to by 'outp' if it is not null. */ static inline void zarray_set(zarray_t *za, int idx, const void *p, void *outp) { assert(za != NULL); assert(p != NULL); assert(idx >= 0); assert(idx < za->size); if (outp != NULL) memcpy(outp, &za->data[idx*za->el_sz], za->el_sz); memcpy(&za->data[idx*za->el_sz], p, za->el_sz); } /** * Calls the supplied function for every element in the array in index order. * The map function will be passed a pointer to each element in turn and must * have the following format: * * void map_function(element_type *element) */ static inline void zarray_map(zarray_t *za, void (*f)(void*)) { assert(za != NULL); assert(f != NULL); for (int idx = 0; idx < za->size; idx++) f(&za->data[idx*za->el_sz]); } /** * Calls the supplied function for every element in the array in index order. * HOWEVER values are passed to the function, not pointers to values. In the * case where the zarray stores object pointers, zarray_vmap allows you to * pass in the object's destroy function (or free) directly. Can only be used * with zarray's which contain pointer data. The map function should have the * following format: * * void map_function(element_type *element) */ void zarray_vmap(zarray_t *za, void (*f)()); /** * Removes all elements from the array and sets its size to zero. Pointers to * any data elements obtained i.e. by zarray_get_volatile() will no longer be * valid. */ static inline void zarray_clear(zarray_t *za) { assert(za != NULL); za->size = 0; } /** * Determines whether any element in the array has a value which matches the * data pointed to by 'p'. * * Returns 1 if a match was found anywhere in the array, else 0. */ static inline int zarray_contains(const zarray_t *za, const void *p) { assert(za != NULL); assert(p != NULL); for (int idx = 0; idx < za->size; idx++) { if (!memcmp(p, &za->data[idx*za->el_sz], za->el_sz)) { return 1; } } return 0; } /** * Uses qsort() to sort the elements contained by the array in ascending order. * Uses the supplied comparison function to determine the appropriate order. * * The comparison function will be passed a pointer to two elements to be compared * and should return a measure of the difference between them (see strcmp()). * I.e. it should return a negative number if the first element is 'less than' * the second, zero if they are equivalent, and a positive number if the first * element is 'greater than' the second. The function should have the following format: * * int comparison_function(const element_type *first, const element_type *second) * * zstrcmp() can be used as the comparison function for string elements, which * will call strcmp() internally. */ static inline void zarray_sort(zarray_t *za, int (*compar)(const void*, const void*)) { assert(za != NULL); assert(compar != NULL); if (za->size == 0) return; qsort(za->data, za->size, za->el_sz, compar); } /** * A comparison function for comparing strings which can be used by zarray_sort() * to sort arrays with char* elements. */ int zstrcmp(const void * a_pp, const void * b_pp); /** * Find the index of an element, or return -1 if not found. Remember that p is * a pointer to the element. **/ // returns -1 if not in array. Remember p is a pointer to the item. static inline int zarray_index_of(const zarray_t *za, const void *p) { assert(za != NULL); assert(p != NULL); for (int i = 0; i < za->size; i++) { if (!memcmp(p, &za->data[i*za->el_sz], za->el_sz)) return i; } return -1; } /** * Add all elements from 'source' into 'dest'. el_size must be the same * for both lists **/ static inline void zarray_add_all(zarray_t * dest, const zarray_t * source) { assert(dest->el_sz == source->el_sz); // Don't allocate on stack because el_sz could be larger than ~8 MB // stack size char *tmp = (char*)calloc(1, dest->el_sz); for (int i = 0; i < zarray_size(source); i++) { zarray_get(source, i, tmp); zarray_add(dest, tmp); } free(tmp); } //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "zarray.c" //////////////////////////////////////////////////////////////////////////////////////////////////// int zstrcmp(const void * a_pp, const void * b_pp) { assert(a_pp != NULL); assert(b_pp != NULL); char * a = *(void**)a_pp; char * b = *(void**)b_pp; return strcmp(a,b); } void zarray_vmap(zarray_t *za, void (*f)()) { assert(za != NULL); assert(f != NULL); assert(za->el_sz == sizeof(void*)); for (int idx = 0; idx < za->size; idx++) { void *pp = &za->data[idx*za->el_sz]; void *p = *(void**) pp; f(p); } } //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "math_util.h" //////////////////////////////////////////////////////////////////////////////////////////////////// #ifndef M_TWOPI # define M_TWOPI 6.2831853071795862319959 /* 2*pi */ #endif #ifndef M_PI # define M_PI 3.141592653589793238462643383279502884196 #endif #define to_radians(x) ( (x) * (M_PI / 180.0 )) #define to_degrees(x) ( (x) * (180.0 / M_PI )) #define max(A, B) (A < B ? B : A) #define min(A, B) (A < B ? A : B) /* DEPRECATE, threshold meaningless without context. static inline int dequals(float a, float b) { float thresh = 1e-9; return (fabs(a-b) < thresh); } */ static inline int dequals_mag(float a, float b, float thresh) { return (fabs(a-b) < thresh); } static inline int isq(int v) { return v*v; } static inline float fsq(float v) { return v*v; } static inline float sq(float v) { return v*v; } static inline float sgn(float v) { return (v>=0) ? 1 : -1; } // random number between [0, 1) static inline float randf() { return ((float) rand()) / (RAND_MAX + 1.0); } static inline float signed_randf() { return randf()*2 - 1; } // return a random integer between [0, bound) static inline int irand(int bound) { int v = (int) (randf()*bound); if (v == bound) return (bound-1); //assert(v >= 0); //assert(v < bound); return v; } /** Map vin to [0, 2*PI) **/ static inline float mod2pi_positive(float vin) { return vin - M_TWOPI * floor(vin / M_TWOPI); } /** Map vin to [-PI, PI) **/ static inline float mod2pi(float vin) { return mod2pi_positive(vin + M_PI) - M_PI; } /** Return vin such that it is within PI degrees of ref **/ static inline float mod2pi_ref(float ref, float vin) { return ref + mod2pi(vin - ref); } /** Map vin to [0, 360) **/ static inline float mod360_positive(float vin) { return vin - 360 * floor(vin / 360); } /** Map vin to [-180, 180) **/ static inline float mod360(float vin) { return mod360_positive(vin + 180) - 180; } static inline int theta_to_int(float theta, int max) { theta = mod2pi_ref(M_PI, theta); int v = (int) (theta / M_TWOPI * max); if (v == max) v = 0; assert (v >= 0 && v < max); return v; } static inline int imin(int a, int b) { return (a < b) ? a : b; } static inline int imax(int a, int b) { return (a > b) ? a : b; } static inline int64_t imin64(int64_t a, int64_t b) { return (a < b) ? a : b; } static inline int64_t imax64(int64_t a, int64_t b) { return (a > b) ? a : b; } static inline int iclamp(int v, int minv, int maxv) { return imax(minv, imin(v, maxv)); } static inline float dclamp(float a, float min, float max) { if (a < min) return min; if (a > max) return max; return a; } static inline int fltcmp (float f1, float f2) { float epsilon = f1-f2; if (epsilon < 0.0) return -1; else if (epsilon > 0.0) return 1; else return 0; } static inline int dblcmp (float d1, float d2) { float epsilon = d1-d2; if (epsilon < 0.0) return -1; else if (epsilon > 0.0) return 1; else return 0; } //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "svd22.h" //////////////////////////////////////////////////////////////////////////////////////////////////// void svd22(const float A[4], float U[4], float S[2], float V[4]); // for the matrix [a b; b d] void svd_sym_singular_values(float A00, float A01, float A11, float *Lmin, float *Lmax); //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "svd22.c" //////////////////////////////////////////////////////////////////////////////////////////////////// /** SVD 2x2. Computes singular values and vectors without squaring the input matrix. With double precision math, results are accurate to about 1E-16. U = [ cos(theta) -sin(theta) ] [ sin(theta) cos(theta) ] S = [ e 0 ] [ 0 f ] V = [ cos(phi) -sin(phi) ] [ sin(phi) cos(phi) ] Our strategy is basically to analytically multiply everything out and then rearrange so that we can solve for theta, phi, e, and f. (Derivation by ebolson@umich.edu 5/2016) V' = [ CP SP ] [ -SP CP ] USV' = [ CT -ST ][ e*CP e*SP ] [ ST CT ][ -f*SP f*CP ] = [e*CT*CP + f*ST*SP e*CT*SP - f*ST*CP ] [e*ST*CP - f*SP*CT e*SP*ST + f*CP*CT ] A00+A11 = e*CT*CP + f*ST*SP + e*SP*ST + f*CP*CT = e*(CP*CT + SP*ST) + f*(SP*ST + CP*CT) = (e+f)(CP*CT + SP*ST) B0 = (e+f)*cos(P-T) A00-A11 = e*CT*CP + f*ST*SP - e*SP*ST - f*CP*CT = e*(CP*CT - SP*ST) - f*(-ST*SP + CP*CT) = (e-f)(CP*CT - SP*ST) B1 = (e-f)*cos(P+T) A01+A10 = e*CT*SP - f*ST*CP + e*ST*CP - f*SP*CT = e(CT*SP + ST*CP) - f*(ST*CP + SP*CT) = (e-f)*(CT*SP + ST*CP) B2 = (e-f)*sin(P+T) A01-A10 = e*CT*SP - f*ST*CP - e*ST*CP + f*SP*CT = e*(CT*SP - ST*CP) + f(SP*CT - ST*CP) = (e+f)*(CT*SP - ST*CP) B3 = (e+f)*sin(P-T) B0 = (e+f)*cos(P-T) B1 = (e-f)*cos(P+T) B2 = (e-f)*sin(P+T) B3 = (e+f)*sin(P-T) B3/B0 = tan(P-T) B2/B1 = tan(P+T) **/ void svd22(const float A[4], float U[4], float S[2], float V[4]) { float A00 = A[0]; float A01 = A[1]; float A10 = A[2]; float A11 = A[3]; float B0 = A00 + A11; float B1 = A00 - A11; float B2 = A01 + A10; float B3 = A01 - A10; float PminusT = atan2(B3, B0); float PplusT = atan2(B2, B1); float P = (PminusT + PplusT) / 2; float T = (-PminusT + PplusT) / 2; float CP = cos(P), SP = sin(P); float CT = cos(T), ST = sin(T); U[0] = CT; U[1] = -ST; U[2] = ST; U[3] = CT; V[0] = CP; V[1] = -SP; V[2] = SP; V[3] = CP; // C0 = e+f. There are two ways to compute C0; we pick the one // that is better conditioned. float CPmT = cos(P-T), SPmT = sin(P-T); float C0 = 0; if (fabs(CPmT) > fabs(SPmT)) C0 = B0 / CPmT; else C0 = B3 / SPmT; // C1 = e-f. There are two ways to compute C1; we pick the one // that is better conditioned. float CPpT = cos(P+T), SPpT = sin(P+T); float C1 = 0; if (fabs(CPpT) > fabs(SPpT)) C1 = B1 / CPpT; else C1 = B2 / SPpT; // e and f are the singular values float e = (C0 + C1) / 2; float f = (C0 - C1) / 2; if (e < 0) { e = -e; U[0] = -U[0]; U[2] = -U[2]; } if (f < 0) { f = -f; U[1] = -U[1]; U[3] = -U[3]; } // sort singular values. if (e > f) { // already in big-to-small order. S[0] = e; S[1] = f; } else { // Curiously, this code never seems to get invoked. Why is it // that S[0] always ends up the dominant vector? However, // this code has been tested (flipping the logic forces us to // sort the singular values in ascending order). // // P = [ 0 1 ; 1 0 ] // USV' = (UP)(PSP)(PV') // = (UP)(PSP)(VP)' // = (UP)(PSP)(P'V')' S[0] = f; S[1] = e; // exchange columns of U and V float tmp[2]; tmp[0] = U[0]; tmp[1] = U[2]; U[0] = U[1]; U[2] = U[3]; U[1] = tmp[0]; U[3] = tmp[1]; tmp[0] = V[0]; tmp[1] = V[2]; V[0] = V[1]; V[2] = V[3]; V[1] = tmp[0]; V[3] = tmp[1]; } /* float SM[4] = { S[0], 0, 0, S[1] }; doubles_print_mat(U, 2, 2, "%20.10g"); doubles_print_mat(SM, 2, 2, "%20.10g"); doubles_print_mat(V, 2, 2, "%20.10g"); printf("A:\n"); doubles_print_mat(A, 2, 2, "%20.10g"); float SVt[4]; doubles_mat_ABt(SM, 2, 2, V, 2, 2, SVt, 2, 2); float USVt[4]; doubles_mat_AB(U, 2, 2, SVt, 2, 2, USVt, 2, 2); printf("USVt\n"); doubles_print_mat(USVt, 2, 2, "%20.10g"); float diff[4]; for (int i = 0; i < 4; i++) diff[i] = A[i] - USVt[i]; printf("diff\n"); doubles_print_mat(diff, 2, 2, "%20.10g"); */ } // for the matrix [a b; b d] void svd_sym_singular_values(float A00, float A01, float A11, float *Lmin, float *Lmax) { float A10 = A01; float B0 = A00 + A11; float B1 = A00 - A11; float B2 = A01 + A10; float B3 = A01 - A10; float PminusT = atan2(B3, B0); float PplusT = atan2(B2, B1); float P = (PminusT + PplusT) / 2; float T = (-PminusT + PplusT) / 2; // C0 = e+f. There are two ways to compute C0; we pick the one // that is better conditioned. float CPmT = cos(P-T), SPmT = sin(P-T); float C0 = 0; if (fabs(CPmT) > fabs(SPmT)) C0 = B0 / CPmT; else C0 = B3 / SPmT; // C1 = e-f. There are two ways to compute C1; we pick the one // that is better conditioned. float CPpT = cos(P+T), SPpT = sin(P+T); float C1 = 0; if (fabs(CPpT) > fabs(SPpT)) C1 = B1 / CPpT; else C1 = B2 / SPpT; // e and f are the singular values float e = (C0 + C1) / 2; float f = (C0 - C1) / 2; *Lmin = fmin(e, f); *Lmax = fmax(e, f); } //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "matd.h" //////////////////////////////////////////////////////////////////////////////////////////////////// /** * Defines a matrix structure for holding float-precision values with * data in row-major order (i.e. index = row*ncols + col). * * nrows and ncols are 1-based counts with the exception that a scalar (non-matrix) * is represented with nrows=0 and/or ncols=0. */ typedef struct { unsigned int nrows, ncols; float data[]; // float *data; } matd_t; #define MATD_ALLOC(name, nrows, ncols) float name ## _storage [nrows*ncols]; matd_t name = { .nrows = nrows, .ncols = ncols, .data = &name ## _storage }; /** * Defines a small value which can be used in place of zero for approximating * calculations which are singular at zero values (i.e. inverting a matrix with * a zero or near-zero determinant). */ #define MATD_EPS 1e-8 /** * A macro to reference a specific matd_t data element given it's zero-based * row and column indexes. Suitable for both retrieval and assignment. */ #define MATD_EL(m, row, col) (m)->data[((row)*(m)->ncols + (col))] /** * Creates a float matrix with the given number of rows and columns (or a scalar * in the case where rows=0 and/or cols=0). All data elements will be initialized * to zero. It is the caller's responsibility to call matd_destroy() on the * returned matrix. */ matd_t *matd_create(int rows, int cols); /** * Creates a float matrix with the given number of rows and columns (or a scalar * in the case where rows=0 and/or cols=0). All data elements will be initialized * using the supplied array of data, which must contain at least rows*cols elements, * arranged in row-major order (i.e. index = row*ncols + col). It is the caller's * responsibility to call matd_destroy() on the returned matrix. */ matd_t *matd_create_data(int rows, int cols, const float *data); /** * Creates a float matrix with the given number of rows and columns (or a scalar * in the case where rows=0 and/or cols=0). All data elements will be initialized * using the supplied array of float data, which must contain at least rows*cols elements, * arranged in row-major order (i.e. index = row*ncols + col). It is the caller's * responsibility to call matd_destroy() on the returned matrix. */ matd_t *matd_create_dataf(int rows, int cols, const float *data); /** * Creates a square identity matrix with the given number of rows (and * therefore columns), or a scalar with value 1 in the case where dim=0. * It is the caller's responsibility to call matd_destroy() on the * returned matrix. */ matd_t *matd_identity(int dim); /** * Creates a scalar with the supplied value 'v'. It is the caller's responsibility * to call matd_destroy() on the returned matrix. * * NOTE: Scalars are different than 1x1 matrices (implementation note: * they are encoded as 0x0 matrices). For example: for matrices A*B, A * and B must both have specific dimensions. However, if A is a * scalar, there are no restrictions on the size of B. */ matd_t *matd_create_scalar(float v); /** * Retrieves the cell value for matrix 'm' at the given zero-based row and column index. * Performs more thorough validation checking than MATD_EL(). */ float matd_get(const matd_t *m, int row, int col); /** * Assigns the given value to the matrix cell at the given zero-based row and * column index. Performs more thorough validation checking than MATD_EL(). */ void matd_put(matd_t *m, int row, int col, float value); /** * Retrieves the scalar value of the given element ('m' must be a scalar). * Performs more thorough validation checking than MATD_EL(). */ float matd_get_scalar(const matd_t *m); /** * Assigns the given value to the supplied scalar element ('m' must be a scalar). * Performs more thorough validation checking than MATD_EL(). */ void matd_put_scalar(matd_t *m, float value); /** * Creates an exact copy of the supplied matrix 'm'. It is the caller's * responsibility to call matd_destroy() on the returned matrix. */ matd_t *matd_copy(const matd_t *m); /** * Creates a copy of a subset of the supplied matrix 'a'. The subset will include * rows 'r0' through 'r1', inclusive ('r1' >= 'r0'), and columns 'c0' through 'c1', * inclusive ('c1' >= 'c0'). All parameters are zero-based (i.e. matd_select(a, 0, 0, 0, 0) * will return only the first cell). Cannot be used on scalars or to extend * beyond the number of rows/columns of 'a'. It is the caller's responsibility to * call matd_destroy() on the returned matrix. */ matd_t *matd_select(const matd_t *a, int r0, int r1, int c0, int c1); /** * Prints the supplied matrix 'm' to standard output by applying the supplied * printf format specifier 'fmt' for each individual element. Each row will * be printed on a separate newline. */ void matd_print(const matd_t *m, const char *fmt); /** * Prints the transpose of the supplied matrix 'm' to standard output by applying * the supplied printf format specifier 'fmt' for each individual element. Each * row will be printed on a separate newline. */ void matd_print_transpose(const matd_t *m, const char *fmt); /** * Adds the two supplied matrices together, cell-by-cell, and returns the results * as a new matrix of the same dimensions. The supplied matrices must have * identical dimensions. It is the caller's responsibility to call matd_destroy() * on the returned matrix. */ matd_t *matd_add(const matd_t *a, const matd_t *b); /** * Adds the values of 'b' to matrix 'a', cell-by-cell, and overwrites the * contents of 'a' with the results. The supplied matrices must have * identical dimensions. */ void matd_add_inplace(matd_t *a, const matd_t *b); /** * Subtracts matrix 'b' from matrix 'a', cell-by-cell, and returns the results * as a new matrix of the same dimensions. The supplied matrices must have * identical dimensions. It is the caller's responsibility to call matd_destroy() * on the returned matrix. */ matd_t *matd_subtract(const matd_t *a, const matd_t *b); /** * Subtracts the values of 'b' from matrix 'a', cell-by-cell, and overwrites the * contents of 'a' with the results. The supplied matrices must have * identical dimensions. */ void matd_subtract_inplace(matd_t *a, const matd_t *b); /** * Scales all cell values of matrix 'a' by the given scale factor 's' and * returns the result as a new matrix of the same dimensions. It is the caller's * responsibility to call matd_destroy() on the returned matrix. */ matd_t *matd_scale(const matd_t *a, float s); /** * Scales all cell values of matrix 'a' by the given scale factor 's' and * overwrites the contents of 'a' with the results. */ void matd_scale_inplace(matd_t *a, float s); /** * Multiplies the two supplied matrices together (matrix product), and returns the * results as a new matrix. The supplied matrices must have dimensions such that * columns(a) = rows(b). The returned matrix will have a row count of rows(a) * and a column count of columns(b). It is the caller's responsibility to call * matd_destroy() on the returned matrix. */ matd_t *matd_multiply(const matd_t *a, const matd_t *b); /** * Creates a matrix which is the transpose of the supplied matrix 'a'. It is the * caller's responsibility to call matd_destroy() on the returned matrix. */ matd_t *matd_transpose(const matd_t *a); /** * Calculates the determinant of the supplied matrix 'a'. */ float matd_det(const matd_t *a); /** * Attempts to compute an inverse of the supplied matrix 'a' and return it as * a new matrix. This is strictly only possible if the determinant of 'a' is * non-zero (matd_det(a) != 0). * * If the determinant is zero, NULL is returned. It is otherwise the * caller's responsibility to cope with the results caused by poorly * conditioned matrices. (E.g.., if such a situation is likely to arise, compute * the pseudo-inverse from the SVD.) **/ matd_t *matd_inverse(const matd_t *a); static inline void matd_set_data(matd_t *m, const float *data) { memcpy(m->data, data, m->nrows * m->ncols * sizeof(float)); } /** * Determines whether the supplied matrix 'a' is a scalar (positive return) or * not (zero return, indicating a matrix of dimensions at least 1x1). */ static inline int matd_is_scalar(const matd_t *a) { assert(a != NULL); return a->ncols == 0 || a->nrows == 0; } /** * Determines whether the supplied matrix 'a' is a row or column vector * (positive return) or not (zero return, indicating either 'a' is a scalar or a * matrix with at least one dimension > 1). */ static inline int matd_is_vector(const matd_t *a) { assert(a != NULL); return a->ncols == 1 || a->nrows == 1; } /** * Determines whether the supplied matrix 'a' is a row or column vector * with a dimension of 'len' (positive return) or not (zero return). */ static inline int matd_is_vector_len(const matd_t *a, int len) { assert(a != NULL); return (a->ncols == 1 && a->nrows == len) || (a->ncols == len && a->nrows == 1); } /** * Calculates the magnitude of the supplied matrix 'a'. */ float matd_vec_mag(const matd_t *a); /** * Calculates the magnitude of the distance between the points represented by * matrices 'a' and 'b'. Both 'a' and 'b' must be vectors and have the same * dimension (although one may be a row vector and one may be a column vector). */ float matd_vec_dist(const matd_t *a, const matd_t *b); /** * Same as matd_vec_dist, but only uses the first 'n' terms to compute distance */ float matd_vec_dist_n(const matd_t *a, const matd_t *b, int n); /** * Calculates the dot product of two vectors. Both 'a' and 'b' must be vectors * and have the same dimension (although one may be a row vector and one may be * a column vector). */ float matd_vec_dot_product(const matd_t *a, const matd_t *b); /** * Calculates the normalization of the supplied vector 'a' (i.e. a unit vector * of the same dimension and orientation as 'a' with a magnitude of 1) and returns * it as a new vector. 'a' must be a vector of any dimension and must have a * non-zero magnitude. It is the caller's responsibility to call matd_destroy() * on the returned matrix. */ matd_t *matd_vec_normalize(const matd_t *a); /** * Calculates the cross product of supplied matrices 'a' and 'b' (i.e. a x b) * and returns it as a new matrix. Both 'a' and 'b' must be vectors of dimension * 3, but can be either row or column vectors. It is the caller's responsibility * to call matd_destroy() on the returned matrix. */ matd_t *matd_crossproduct(const matd_t *a, const matd_t *b); float matd_err_inf(const matd_t *a, const matd_t *b); /** * Creates a new matrix by applying a series of matrix operations, as expressed * in 'expr', to the supplied list of matrices. Each matrix to be operated upon * must be represented in the expression by a separate matrix placeholder, 'M', * and there must be one matrix supplied as an argument for each matrix * placeholder in the expression. All rules and caveats of the corresponding * matrix operations apply to the operated-on matrices. It is the caller's * responsibility to call matd_destroy() on the returned matrix. * * Available operators (in order of increasing precedence): * M+M add two matrices together * M-M subtract one matrix from another * M*M multiply to matrices together (matrix product) * MM multiply to matrices together (matrix product) * -M negate a matrix * M^-1 take the inverse of a matrix * M' take the transpose of a matrix * * Expressions can be combined together and grouped by enclosing them in * parenthesis, i.e.: * -M(M+M+M)-(M*M)^-1 * * Scalar values can be generated on-the-fly, i.e.: * M*2.2 scales M by 2.2 * -2+M adds -2 to all elements of M * * All whitespace in the expression is ignored. */ matd_t *matd_op(const char *expr, ...); /** * Frees the memory associated with matrix 'm', being the result of an earlier * call to a matd_*() function, after which 'm' will no longer be usable. */ void matd_destroy(matd_t *m); typedef struct { matd_t *U; matd_t *S; matd_t *V; } matd_svd_t; /** Compute a complete SVD of a matrix. The SVD exists for all * matrices. For a matrix MxN, we will have: * * A = U*S*V' * * where A is MxN, U is MxM (and is an orthonormal basis), S is MxN * (and is diagonal up to machine precision), and V is NxN (and is an * orthonormal basis). * * The caller is responsible for destroying U, S, and V. **/ matd_svd_t matd_svd(matd_t *A); #define MATD_SVD_NO_WARNINGS 1 matd_svd_t matd_svd_flags(matd_t *A, int flags); //////////////////////////////// // PLU Decomposition // All square matrices (even singular ones) have a partially-pivoted // LU decomposition such that A = PLU, where P is a permutation // matrix, L is a lower triangular matrix, and U is an upper // triangular matrix. // typedef struct { // was the input matrix singular? When a zero pivot is found, this // flag is set to indicate that this has happened. int singular; unsigned int *piv; // permutation indices int pivsign; // either +1 or -1 // The matd_plu_t object returned "owns" the enclosed LU matrix. It // is not expected that the returned object is itself useful to // users: it contains the L and U information all smushed // together. matd_t *lu; // combined L and U matrices, permuted so they can be triangular. } matd_plu_t; matd_plu_t *matd_plu(const matd_t *a); void matd_plu_destroy(matd_plu_t *mlu); float matd_plu_det(const matd_plu_t *lu); matd_t *matd_plu_p(const matd_plu_t *lu); matd_t *matd_plu_l(const matd_plu_t *lu); matd_t *matd_plu_u(const matd_plu_t *lu); matd_t *matd_plu_solve(const matd_plu_t *mlu, const matd_t *b); // uses LU decomposition internally. matd_t *matd_solve(matd_t *A, matd_t *b); //////////////////////////////// // Cholesky Factorization /** * Creates a float matrix with the Cholesky lower triangular matrix * of A. A must be symmetric, positive definite. It is the caller's * responsibility to call matd_destroy() on the returned matrix. */ //matd_t *matd_cholesky(const matd_t *A); typedef struct { int is_spd; matd_t *u; } matd_chol_t; matd_chol_t *matd_chol(matd_t *A); matd_t *matd_chol_solve(const matd_chol_t *chol, const matd_t *b); void matd_chol_destroy(matd_chol_t *chol); // only sensible on PSD matrices matd_t *matd_chol_inverse(matd_t *a); void matd_ltransposetriangle_solve(matd_t *u, const float *b, float *x); void matd_ltriangle_solve(matd_t *u, const float *b, float *x); void matd_utriangle_solve(matd_t *u, const float *b, float *x); float matd_max(matd_t *m); //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "matd.c" //////////////////////////////////////////////////////////////////////////////////////////////////// // a matd_t with rows=0 cols=0 is a SCALAR. // to ease creating mati, matf, etc. in the future. #define TYPE float matd_t *matd_create(int rows, int cols) { assert(rows >= 0); assert(cols >= 0); if (rows == 0 || cols == 0) return matd_create_scalar(0); matd_t *m = calloc(1, sizeof(matd_t) + (rows*cols*sizeof(float))); m->nrows = rows; m->ncols = cols; return m; } matd_t *matd_create_scalar(TYPE v) { matd_t *m = calloc(1, sizeof(matd_t) + sizeof(float)); m->nrows = 0; m->ncols = 0; m->data[0] = v; return m; } matd_t *matd_create_data(int rows, int cols, const TYPE *data) { if (rows == 0 || cols == 0) return matd_create_scalar(data[0]); matd_t *m = matd_create(rows, cols); for (int i = 0; i < rows * cols; i++) m->data[i] = data[i]; return m; } matd_t *matd_create_dataf(int rows, int cols, const float *data) { if (rows == 0 || cols == 0) return matd_create_scalar(data[0]); matd_t *m = matd_create(rows, cols); for (int i = 0; i < rows * cols; i++) m->data[i] = (float)data[i]; return m; } matd_t *matd_identity(int dim) { if (dim == 0) return matd_create_scalar(1); matd_t *m = matd_create(dim, dim); for (int i = 0; i < dim; i++) MATD_EL(m, i, i) = 1; return m; } // row and col are zero-based TYPE matd_get(const matd_t *m, int row, int col) { assert(m != NULL); assert(!matd_is_scalar(m)); assert(row >= 0); assert(row < m->nrows); assert(col >= 0); assert(col < m->ncols); return MATD_EL(m, row, col); } // row and col are zero-based void matd_put(matd_t *m, int row, int col, TYPE value) { assert(m != NULL); if (matd_is_scalar(m)) { matd_put_scalar(m, value); return; } assert(row >= 0); assert(row < m->nrows); assert(col >= 0); assert(col < m->ncols); MATD_EL(m, row, col) = value; } TYPE matd_get_scalar(const matd_t *m) { assert(m != NULL); assert(matd_is_scalar(m)); return (m->data[0]); } void matd_put_scalar(matd_t *m, TYPE value) { assert(m != NULL); assert(matd_is_scalar(m)); m->data[0] = value; } matd_t *matd_copy(const matd_t *m) { assert(m != NULL); matd_t *x = matd_create(m->nrows, m->ncols); if (matd_is_scalar(m)) x->data[0] = m->data[0]; else memcpy(x->data, m->data, sizeof(TYPE)*m->ncols*m->nrows); return x; } matd_t *matd_select(const matd_t * a, int r0, int r1, int c0, int c1) { assert(a != NULL); assert(r0 >= 0 && r0 < a->nrows); assert(c0 >= 0 && c0 < a->ncols); int nrows = r1 - r0 + 1; int ncols = c1 - c0 + 1; matd_t * r = matd_create(nrows, ncols); for (int row = r0; row <= r1; row++) for (int col = c0; col <= c1; col++) MATD_EL(r,row-r0,col-c0) = MATD_EL(a,row,col); return r; } void matd_print(const matd_t *m, const char *fmt) { assert(m != NULL); assert(fmt != NULL); if (matd_is_scalar(m)) { printf(fmt, (double) MATD_EL(m, 0, 0)); printf("\n"); } else { for (int i = 0; i < m->nrows; i++) { for (int j = 0; j < m->ncols; j++) { printf(fmt, (double) MATD_EL(m, i, j)); } printf("\n"); } } } void matd_print_transpose(const matd_t *m, const char *fmt) { assert(m != NULL); assert(fmt != NULL); if (matd_is_scalar(m)) { printf(fmt, (double) MATD_EL(m, 0, 0)); printf("\n"); } else { for (int j = 0; j < m->ncols; j++) { for (int i = 0; i < m->nrows; i++) { printf(fmt, (double) MATD_EL(m, i, j)); } printf("\n"); } } } void matd_destroy(matd_t *m) { if (!m) return; assert(m != NULL); free(m); } matd_t *matd_multiply(const matd_t *a, const matd_t *b) { assert(a != NULL); assert(b != NULL); if (matd_is_scalar(a)) return matd_scale(b, a->data[0]); if (matd_is_scalar(b)) return matd_scale(a, b->data[0]); assert(a->ncols == b->nrows); matd_t *m = matd_create(a->nrows, b->ncols); for (int i = 0; i < m->nrows; i++) { for (int j = 0; j < m->ncols; j++) { TYPE acc = 0; for (int k = 0; k < a->ncols; k++) { acc += MATD_EL(a, i, k) * MATD_EL(b, k, j); } MATD_EL(m, i, j) = acc; } } return m; } matd_t *matd_scale(const matd_t *a, float s) { assert(a != NULL); if (matd_is_scalar(a)) return matd_create_scalar(a->data[0] * s); matd_t *m = matd_create(a->nrows, a->ncols); for (int i = 0; i < m->nrows; i++) { for (int j = 0; j < m->ncols; j++) { MATD_EL(m, i, j) = s * MATD_EL(a, i, j); } } return m; } void matd_scale_inplace(matd_t *a, float s) { assert(a != NULL); if (matd_is_scalar(a)) { a->data[0] *= s; return; } for (int i = 0; i < a->nrows; i++) { for (int j = 0; j < a->ncols; j++) { MATD_EL(a, i, j) *= s; } } } matd_t *matd_add(const matd_t *a, const matd_t *b) { assert(a != NULL); assert(b != NULL); assert(a->nrows == b->nrows); assert(a->ncols == b->ncols); if (matd_is_scalar(a)) return matd_create_scalar(a->data[0] + b->data[0]); matd_t *m = matd_create(a->nrows, a->ncols); for (int i = 0; i < m->nrows; i++) { for (int j = 0; j < m->ncols; j++) { MATD_EL(m, i, j) = MATD_EL(a, i, j) + MATD_EL(b, i, j); } } return m; } void matd_add_inplace(matd_t *a, const matd_t *b) { assert(a != NULL); assert(b != NULL); assert(a->nrows == b->nrows); assert(a->ncols == b->ncols); if (matd_is_scalar(a)) { a->data[0] += b->data[0]; return; } for (int i = 0; i < a->nrows; i++) { for (int j = 0; j < a->ncols; j++) { MATD_EL(a, i, j) += MATD_EL(b, i, j); } } } matd_t *matd_subtract(const matd_t *a, const matd_t *b) { assert(a != NULL); assert(b != NULL); assert(a->nrows == b->nrows); assert(a->ncols == b->ncols); if (matd_is_scalar(a)) return matd_create_scalar(a->data[0] - b->data[0]); matd_t *m = matd_create(a->nrows, a->ncols); for (int i = 0; i < m->nrows; i++) { for (int j = 0; j < m->ncols; j++) { MATD_EL(m, i, j) = MATD_EL(a, i, j) - MATD_EL(b, i, j); } } return m; } void matd_subtract_inplace(matd_t *a, const matd_t *b) { assert(a != NULL); assert(b != NULL); assert(a->nrows == b->nrows); assert(a->ncols == b->ncols); if (matd_is_scalar(a)) { a->data[0] -= b->data[0]; return; } for (int i = 0; i < a->nrows; i++) { for (int j = 0; j < a->ncols; j++) { MATD_EL(a, i, j) -= MATD_EL(b, i, j); } } } matd_t *matd_transpose(const matd_t *a) { assert(a != NULL); if (matd_is_scalar(a)) return matd_create_scalar(a->data[0]); matd_t *m = matd_create(a->ncols, a->nrows); for (int i = 0; i < a->nrows; i++) { for (int j = 0; j < a->ncols; j++) { MATD_EL(m, j, i) = MATD_EL(a, i, j); } } return m; } static float matd_det_general(const matd_t *a) { // Use LU decomposition to calculate the determinant matd_plu_t *mlu = matd_plu(a); matd_t *L = matd_plu_l(mlu); matd_t *U = matd_plu_u(mlu); // The determinants of the L and U matrices are the products of // their respective diagonal elements float detL = 1; float detU = 1; for (int i = 0; i < a->nrows; i++) { detL *= matd_get(L, i, i); detU *= matd_get(U, i, i); } // The determinant of a can be calculated as // epsilon*det(L)*det(U), // where epsilon is just the sign of the corresponding permutation // (which is +1 for an even number of permutations and is −1 // for an uneven number of permutations). float det = mlu->pivsign * detL * detU; // Cleanup matd_plu_destroy(mlu); matd_destroy(L); matd_destroy(U); return det; } float matd_det(const matd_t *a) { assert(a != NULL); assert(a->nrows == a->ncols); switch(a->nrows) { case 0: // scalar: invalid assert(a->nrows > 0); break; case 1: // 1x1 matrix return a->data[0]; case 2: // 2x2 matrix return a->data[0] * a->data[3] - a->data[1] * a->data[2]; case 3: // 3x3 matrix return a->data[0]*a->data[4]*a->data[8] - a->data[0]*a->data[5]*a->data[7] + a->data[1]*a->data[5]*a->data[6] - a->data[1]*a->data[3]*a->data[8] + a->data[2]*a->data[3]*a->data[7] - a->data[2]*a->data[4]*a->data[6]; case 4: { // 4x4 matrix float m00 = MATD_EL(a,0,0), m01 = MATD_EL(a,0,1), m02 = MATD_EL(a,0,2), m03 = MATD_EL(a,0,3); float m10 = MATD_EL(a,1,0), m11 = MATD_EL(a,1,1), m12 = MATD_EL(a,1,2), m13 = MATD_EL(a,1,3); float m20 = MATD_EL(a,2,0), m21 = MATD_EL(a,2,1), m22 = MATD_EL(a,2,2), m23 = MATD_EL(a,2,3); float m30 = MATD_EL(a,3,0), m31 = MATD_EL(a,3,1), m32 = MATD_EL(a,3,2), m33 = MATD_EL(a,3,3); return m00 * m11 * m22 * m33 - m00 * m11 * m23 * m32 - m00 * m21 * m12 * m33 + m00 * m21 * m13 * m32 + m00 * m31 * m12 * m23 - m00 * m31 * m13 * m22 - m10 * m01 * m22 * m33 + m10 * m01 * m23 * m32 + m10 * m21 * m02 * m33 - m10 * m21 * m03 * m32 - m10 * m31 * m02 * m23 + m10 * m31 * m03 * m22 + m20 * m01 * m12 * m33 - m20 * m01 * m13 * m32 - m20 * m11 * m02 * m33 + m20 * m11 * m03 * m32 + m20 * m31 * m02 * m13 - m20 * m31 * m03 * m12 - m30 * m01 * m12 * m23 + m30 * m01 * m13 * m22 + m30 * m11 * m02 * m23 - m30 * m11 * m03 * m22 - m30 * m21 * m02 * m13 + m30 * m21 * m03 * m12; } default: return matd_det_general(a); } assert(0); return 0; } // returns NULL if the matrix is (exactly) singular. Caller is // otherwise responsible for knowing how to cope with badly // conditioned matrices. matd_t *matd_inverse(const matd_t *x) { matd_t *m = NULL; assert(x != NULL); assert(x->nrows == x->ncols); if (matd_is_scalar(x)) { if (x->data[0] == 0) return NULL; return matd_create_scalar(1.0 / x->data[0]); } switch(x->nrows) { case 1: { float det = x->data[0]; if (det == 0) return NULL; float invdet = 1.0 / det; m = matd_create(x->nrows, x->nrows); MATD_EL(m, 0, 0) = 1.0 * invdet; return m; } case 2: { float det = x->data[0] * x->data[3] - x->data[1] * x->data[2]; if (det == 0) return NULL; float invdet = 1.0 / det; m = matd_create(x->nrows, x->nrows); MATD_EL(m, 0, 0) = MATD_EL(x, 1, 1) * invdet; MATD_EL(m, 0, 1) = - MATD_EL(x, 0, 1) * invdet; MATD_EL(m, 1, 0) = - MATD_EL(x, 1, 0) * invdet; MATD_EL(m, 1, 1) = MATD_EL(x, 0, 0) * invdet; return m; } default: { matd_plu_t *plu = matd_plu(x); matd_t *inv = NULL; if (!plu->singular) { matd_t *ident = matd_identity(x->nrows); inv = matd_plu_solve(plu, ident); matd_destroy(ident); } matd_plu_destroy(plu); return inv; } } return NULL; // unreachable } // TODO Optimization: Some operations we could perform in-place, // saving some memory allocation work. E.g., ADD, SUBTRACT. Just need // to make sure that we don't do an in-place modification on a matrix // that was an input argument! // handle right-associative operators, greedily consuming them. These // include transpose and inverse. This is called by the main recursion // method. static inline matd_t *matd_op_gobble_right(const char *expr, int *pos, matd_t *acc, matd_t **garb, int *garbpos) { while (expr[*pos] != 0) { switch (expr[*pos]) { case '\'': { assert(acc != NULL); // either a syntax error or a math op failed, producing null matd_t *res = matd_transpose(acc); garb[*garbpos] = res; (*garbpos)++; acc = res; (*pos)++; break; } // handle inverse ^-1. No other exponents are allowed. case '^': { assert(acc != NULL); assert(expr[*pos+1] == '-'); assert(expr[*pos+2] == '1'); matd_t *res = matd_inverse(acc); garb[*garbpos] = res; (*garbpos)++; acc = res; (*pos)+=3; break; } default: return acc; } } return acc; } // @garb, garbpos A list of every matrix allocated during evaluation... used to assist cleanup. // @oneterm: we should return at the end of this term (i.e., stop at a PLUS, MINUS, LPAREN). static matd_t *matd_op_recurse(const char *expr, int *pos, matd_t *acc, matd_t **args, int *argpos, matd_t **garb, int *garbpos, int oneterm) { while (expr[*pos] != 0) { switch (expr[*pos]) { case '(': { if (oneterm && acc != NULL) return acc; (*pos)++; matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 0); rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos); if (acc == NULL) { acc = rhs; } else { matd_t *res = matd_multiply(acc, rhs); garb[*garbpos] = res; (*garbpos)++; acc = res; } break; } case ')': { if (oneterm) return acc; (*pos)++; return acc; } case '*': { (*pos)++; matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 1); rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos); if (acc == NULL) { acc = rhs; } else { matd_t *res = matd_multiply(acc, rhs); garb[*garbpos] = res; (*garbpos)++; acc = res; } break; } case 'F': { matd_t *rhs = args[*argpos]; garb[*garbpos] = rhs; (*garbpos)++; (*pos)++; (*argpos)++; rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos); if (acc == NULL) { acc = rhs; } else { matd_t *res = matd_multiply(acc, rhs); garb[*garbpos] = res; (*garbpos)++; acc = res; } break; } case 'M': { matd_t *rhs = args[*argpos]; (*pos)++; (*argpos)++; rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos); if (acc == NULL) { acc = rhs; } else { matd_t *res = matd_multiply(acc, rhs); garb[*garbpos] = res; (*garbpos)++; acc = res; } break; } /* case 'D': { int rows = expr[*pos+1]-'0'; int cols = expr[*pos+2]-'0'; matd_t *rhs = matd_create(rows, cols); break; } */ // a constant (SCALAR) defined inline. Treat just like M, creating a matd_t on the fly. // case '0': // case '1': // case '2': // case '3': // case '4': // case '5': // case '6': // case '7': // case '8': // case '9': // case '.': { // const char *start = &expr[*pos]; // char *end; // float s = strtod(start, &end); // (*pos) += (end - start); // matd_t *rhs = matd_create_scalar(s); // garb[*garbpos] = rhs; // (*garbpos)++; // rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos); // if (acc == NULL) { // acc = rhs; // } else { // matd_t *res = matd_multiply(acc, rhs); // garb[*garbpos] = res; // (*garbpos)++; // acc = res; // } // break; // } case '+': { if (oneterm && acc != NULL) return acc; // don't support unary plus assert(acc != NULL); (*pos)++; matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 1); rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos); matd_t *res = matd_add(acc, rhs); garb[*garbpos] = res; (*garbpos)++; acc = res; break; } case '-': { if (oneterm && acc != NULL) return acc; if (acc == NULL) { // unary minus (*pos)++; matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 1); rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos); matd_t *res = matd_scale(rhs, -1); garb[*garbpos] = res; (*garbpos)++; acc = res; } else { // subtract (*pos)++; matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 1); rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos); matd_t *res = matd_subtract(acc, rhs); garb[*garbpos] = res; (*garbpos)++; acc = res; } break; } case ' ': { // nothing to do. spaces are meaningless. (*pos)++; break; } default: { fprintf(stderr, "matd_op(): Unknown character: '%c'\n", expr[*pos]); assert(expr[*pos] != expr[*pos]); } } } return acc; } // always returns a new matrix. matd_t *matd_op(const char *expr, ...) { int nargs = 0; int exprlen = 0; assert(expr != NULL); for (const char *p = expr; *p != 0; p++) { if (*p == 'M' || *p == 'F') nargs++; exprlen++; } assert(nargs > 0); if (!exprlen) // expr = "" return NULL; va_list ap; va_start(ap, expr); matd_t *args[nargs]; for (int i = 0; i < nargs; i++) { args[i] = va_arg(ap, matd_t*); // XXX: sanity check argument; emit warning/error if args[i] // doesn't look like a matd_t*. } va_end(ap); int pos = 0; int argpos = 0; int garbpos = 0; matd_t *garb[2*exprlen]; // can't create more than 2 new result per character // one result, and possibly one argument to free matd_t *res = matd_op_recurse(expr, &pos, NULL, args, &argpos, garb, &garbpos, 0); // 'res' may need to be freed as part of garbage collection (i.e. expr = "F") matd_t *res_copy = (res ? matd_copy(res) : NULL); for (int i = 0; i < garbpos; i++) { matd_destroy(garb[i]); } return res_copy; } float matd_vec_mag(const matd_t *a) { assert(a != NULL); assert(matd_is_vector(a)); float mag = 0.0; int len = a->nrows*a->ncols; for (int i = 0; i < len; i++) mag += sq(a->data[i]); return sqrt(mag); } float matd_vec_dist(const matd_t *a, const matd_t *b) { assert(a != NULL); assert(b != NULL); assert(matd_is_vector(a) && matd_is_vector(b)); assert(a->nrows*a->ncols == b->nrows*b->ncols); int lena = a->nrows*a->ncols; return matd_vec_dist_n(a, b, lena); } float matd_vec_dist_n(const matd_t *a, const matd_t *b, int n) { assert(a != NULL); assert(b != NULL); assert(matd_is_vector(a) && matd_is_vector(b)); int lena = a->nrows*a->ncols; int lenb = b->nrows*b->ncols; assert(n <= lena && n <= lenb); float mag = 0.0; for (int i = 0; i < n; i++) mag += sq(a->data[i] - b->data[i]); return sqrt(mag); } // find the index of the off-diagonal element with the largest mag static inline int max_idx(const matd_t *A, int row, int maxcol) { int maxi = 0; float maxv = -1; for (int i = 0; i < maxcol; i++) { if (i == row) continue; float v = fabs(MATD_EL(A, row, i)); if (v > maxv) { maxi = i; maxv = v; } } return maxi; } float matd_vec_dot_product(const matd_t *a, const matd_t *b) { assert(a != NULL); assert(b != NULL); assert(matd_is_vector(a) && matd_is_vector(b)); int adim = a->ncols*a->nrows; int bdim = b->ncols*b->nrows; assert(adim == bdim); float acc = 0; for (int i = 0; i < adim; i++) { acc += a->data[i] * b->data[i]; } return acc; } matd_t *matd_vec_normalize(const matd_t *a) { assert(a != NULL); assert(matd_is_vector(a)); float mag = matd_vec_mag(a); assert(mag > 0); matd_t *b = matd_create(a->nrows, a->ncols); int len = a->nrows*a->ncols; for(int i = 0; i < len; i++) b->data[i] = a->data[i] / mag; return b; } matd_t *matd_crossproduct(const matd_t *a, const matd_t *b) { // only defined for vecs (col or row) of length 3 assert(a != NULL); assert(b != NULL); assert(matd_is_vector_len(a, 3) && matd_is_vector_len(b, 3)); matd_t * r = matd_create(a->nrows, a->ncols); r->data[0] = a->data[1] * b->data[2] - a->data[2] * b->data[1]; r->data[1] = a->data[2] * b->data[0] - a->data[0] * b->data[2]; r->data[2] = a->data[0] * b->data[1] - a->data[1] * b->data[0]; return r; } TYPE matd_err_inf(const matd_t *a, const matd_t *b) { assert(a->nrows == b->nrows); assert(a->ncols == b->ncols); TYPE maxf = 0; for (int i = 0; i < a->nrows; i++) { for (int j = 0; j < a->ncols; j++) { TYPE av = MATD_EL(a, i, j); TYPE bv = MATD_EL(b, i, j); TYPE err = fabs(av - bv); maxf = fmax(maxf, err); } } return maxf; } // Computes an SVD for square or tall matrices. This code doesn't work // for wide matrices, because the bidiagonalization results in one // non-zero element too far to the right for us to rotate away. // // Caller is responsible for destroying U, S, and V. static matd_svd_t matd_svd_tall(matd_t *A, int flags) { matd_t *B = matd_copy(A); // Apply householder reflections on each side to reduce A to // bidiagonal form. Specifically: // // A = LS*B*RS' // // Where B is bidiagonal, and LS/RS are unitary. // // Why are we doing this? Some sort of transformation is necessary // to reduce the matrix's nz elements to a square region. QR could // work too. We need nzs confined to a square region so that the // subsequent iterative process, which is based on rotations, can // work. (To zero out a term at (i,j), our rotations will also // affect (j,i). // // We prefer bidiagonalization over QR because it gets us "closer" // to the SVD, which should mean fewer iterations. // LS: cumulative left-handed transformations matd_t *LS = matd_identity(A->nrows); // RS: cumulative right-handed transformations. matd_t *RS = matd_identity(A->ncols); for (int hhidx = 0; hhidx < A->nrows; hhidx++) { if (hhidx < A->ncols) { // We construct the normal of the reflection plane: let u // be the vector to reflect, x =[ M 0 0 0 ] the target // location for u (u') after reflection (with M = ||u||). // // The normal vector is then n = (u - x), but since we // could equally have the target location be x = [-M 0 0 0 // ], we could use n = (u + x). // // We then normalize n. To ensure a reasonable magnitude, // we select the sign of M so as to maximize the magnitude // of the first element of (x +/- M). (Otherwise, we could // end up with a divide-by-zero if u[0] and M cancel.) // // The householder reflection matrix is then H=(I - nn'), and // u' = Hu. // // int vlen = A->nrows - hhidx; float v[vlen]; float mag2 = 0; for (int i = 0; i < vlen; i++) { v[i] = MATD_EL(B, hhidx+i, hhidx); mag2 += v[i]*v[i]; } float oldv0 = v[0]; if (oldv0 < 0) v[0] -= sqrt(mag2); else v[0] += sqrt(mag2); mag2 += -oldv0*oldv0 + v[0]*v[0]; // normalize v float mag = sqrt(mag2); // this case arises with matrices of all zeros, for example. if (mag == 0) continue; for (int i = 0; i < vlen; i++) v[i] /= mag; // Q = I - 2vv' //matd_t *Q = matd_identity(A->nrows); //for (int i = 0; i < vlen; i++) // for (int j = 0; j < vlen; j++) // MATD_EL(Q, i+hhidx, j+hhidx) -= 2*v[i]*v[j]; // LS = matd_op("F*M", LS, Q); // Implementation: take each row of LS, compute dot product with n, // subtract n (scaled by dot product) from it. for (int i = 0; i < LS->nrows; i++) { float dot = 0; for (int j = 0; j < vlen; j++) dot += MATD_EL(LS, i, hhidx+j) * v[j]; for (int j = 0; j < vlen; j++) MATD_EL(LS, i, hhidx+j) -= 2*dot*v[j]; } // B = matd_op("M*F", Q, B); // should be Q', but Q is symmetric. for (int i = 0; i < B->ncols; i++) { float dot = 0; for (int j = 0; j < vlen; j++) dot += MATD_EL(B, hhidx+j, i) * v[j]; for (int j = 0; j < vlen; j++) MATD_EL(B, hhidx+j, i) -= 2*dot*v[j]; } } if (hhidx+2 < A->ncols) { int vlen = A->ncols - hhidx - 1; float v[vlen]; float mag2 = 0; for (int i = 0; i < vlen; i++) { v[i] = MATD_EL(B, hhidx, hhidx+i+1); mag2 += v[i]*v[i]; } float oldv0 = v[0]; if (oldv0 < 0) v[0] -= sqrt(mag2); else v[0] += sqrt(mag2); mag2 += -oldv0*oldv0 + v[0]*v[0]; // compute magnitude of ([1 0 0..]+v) float mag = sqrt(mag2); // this case can occur when the vectors are already perpendicular if (mag == 0) continue; for (int i = 0; i < vlen; i++) v[i] /= mag; // TODO: optimize these multiplications // matd_t *Q = matd_identity(A->ncols); // for (int i = 0; i < vlen; i++) // for (int j = 0; j < vlen; j++) // MATD_EL(Q, i+1+hhidx, j+1+hhidx) -= 2*v[i]*v[j]; // RS = matd_op("F*M", RS, Q); for (int i = 0; i < RS->nrows; i++) { float dot = 0; for (int j = 0; j < vlen; j++) dot += MATD_EL(RS, i, hhidx+1+j) * v[j]; for (int j = 0; j < vlen; j++) MATD_EL(RS, i, hhidx+1+j) -= 2*dot*v[j]; } // B = matd_op("F*M", B, Q); // should be Q', but Q is symmetric. for (int i = 0; i < B->nrows; i++) { float dot = 0; for (int j = 0; j < vlen; j++) dot += MATD_EL(B, i, hhidx+1+j) * v[j]; for (int j = 0; j < vlen; j++) MATD_EL(B, i, hhidx+1+j) -= 2*dot*v[j]; } } } // maxiters used to be smaller to prevent us from looping forever, // but this doesn't seem to happen any more with our more stable // svd22 implementation. int maxiters = 1UL << 5; // 1UL << 30; assert(maxiters > 0); // reassure clang int iter; float maxv; // maximum non-zero value being reduced this iteration float tol = 1E-5; // 1E-10; // which method will we use to find the largest off-diagonal // element of B? const int find_max_method = 1; //(B->ncols < 6) ? 2 : 1; // for each of the first B->ncols rows, which index has the // maximum absolute value? (used by method 1) int maxrowidx[B->ncols]; int lastmaxi, lastmaxj; if (find_max_method == 1) { for (int i = 2; i < B->ncols; i++) maxrowidx[i] = max_idx(B, i, B->ncols); // note that we started the array at 2. That's because by setting // these values below, we'll recompute first two entries on the // first iteration! lastmaxi = 0, lastmaxj = 1; } for (iter = 0; iter < maxiters; iter++) { // No diagonalization required for 0x0 and 1x1 matrices. if (B->ncols < 2) break; // find the largest off-diagonal element of B, and put its // coordinates in maxi, maxj. int maxi, maxj; if (find_max_method == 1) { // method 1 is the "smarter" method which does at least // 4*ncols work. More work might be needed (up to // ncols*ncols), depending on data. Thus, this might be a // bit slower than the default method for very small // matrices. maxi = -1; maxv = -1; // every iteration, we must deal with the fact that rows // and columns lastmaxi and lastmaxj have been // modified. Update maxrowidx accordingly. // now, EVERY row also had columns lastmaxi and lastmaxj modified. for (int rowi = 0; rowi < B->ncols; rowi++) { // the magnitude of the largest off-diagonal element // in this row. float thismaxv; // row 'lastmaxi' and 'lastmaxj' have been completely // changed. compute from scratch. if (rowi == lastmaxi || rowi == lastmaxj) { maxrowidx[rowi] = max_idx(B, rowi, B->ncols); thismaxv = fabs(MATD_EL(B, rowi, maxrowidx[rowi])); goto endrowi; } // our maximum entry was just modified. We don't know // if it went up or down, and so we don't know if it // is still the maximum. We have to update from // scratch. if (maxrowidx[rowi] == lastmaxi || maxrowidx[rowi] == lastmaxj) { maxrowidx[rowi] = max_idx(B, rowi, B->ncols); thismaxv = fabs(MATD_EL(B, rowi, maxrowidx[rowi])); goto endrowi; } // This row is unchanged, except for columns // 'lastmaxi' and 'lastmaxj', and those columns were // not previously the largest entry... just check to // see if they are now the maximum entry in their // row. (Remembering to consider off-diagonal entries // only!) thismaxv = fabs(MATD_EL(B, rowi, maxrowidx[rowi])); // check column lastmaxi. Is it now the maximum? if (lastmaxi != rowi) { float v = fabs(MATD_EL(B, rowi, lastmaxi)); if (v > thismaxv) { thismaxv = v; maxrowidx[rowi] = lastmaxi; } } // check column lastmaxj if (lastmaxj != rowi) { float v = fabs(MATD_EL(B, rowi, lastmaxj)); if (v > thismaxv) { thismaxv = v; maxrowidx[rowi] = lastmaxj; } } // does this row have the largest value we've seen so far? endrowi: if (thismaxv > maxv) { maxv = thismaxv; maxi = rowi; } } assert(maxi >= 0); maxj = maxrowidx[maxi]; // save these for the next iteration. lastmaxi = maxi; lastmaxj = maxj; if (maxv < tol) break; } else if (find_max_method == 2) { // brute-force (reference) version. maxv = -1; // only search top "square" portion for (int i = 0; i < B->ncols; i++) { for (int j = 0; j < B->ncols; j++) { if (i == j) continue; float v = fabs(MATD_EL(B, i, j)); if (v > maxv) { maxi = i; maxj = j; maxv = v; } } } // termination condition. if (maxv < tol) break; } else { assert(0); } // printf(">>> %5d %3d, %3d %15g\n", maxi, maxj, iter, maxv); // Now, solve the 2x2 SVD problem for the matrix // [ A0 A1 ] // [ A2 A3 ] float A0 = MATD_EL(B, maxi, maxi); float A1 = MATD_EL(B, maxi, maxj); float A2 = MATD_EL(B, maxj, maxi); float A3 = MATD_EL(B, maxj, maxj); if (1) { float AQ[4]; AQ[0] = A0; AQ[1] = A1; AQ[2] = A2; AQ[3] = A3; float U[4], S[2], V[4]; svd22(AQ, U, S, V); /* Reference (slow) implementation... // LS = LS * ROT(theta) = LS * QL matd_t *QL = matd_identity(A->nrows); MATD_EL(QL, maxi, maxi) = U[0]; MATD_EL(QL, maxi, maxj) = U[1]; MATD_EL(QL, maxj, maxi) = U[2]; MATD_EL(QL, maxj, maxj) = U[3]; matd_t *QR = matd_identity(A->ncols); MATD_EL(QR, maxi, maxi) = V[0]; MATD_EL(QR, maxi, maxj) = V[1]; MATD_EL(QR, maxj, maxi) = V[2]; MATD_EL(QR, maxj, maxj) = V[3]; LS = matd_op("F*M", LS, QL); RS = matd_op("F*M", RS, QR); // remember we'll transpose RS. B = matd_op("M'*F*M", QL, B, QR); matd_destroy(QL); matd_destroy(QR); */ // LS = matd_op("F*M", LS, QL); for (int i = 0; i < LS->nrows; i++) { float vi = MATD_EL(LS, i, maxi); float vj = MATD_EL(LS, i, maxj); MATD_EL(LS, i, maxi) = U[0]*vi + U[2]*vj; MATD_EL(LS, i, maxj) = U[1]*vi + U[3]*vj; } // RS = matd_op("F*M", RS, QR); // remember we'll transpose RS. for (int i = 0; i < RS->nrows; i++) { float vi = MATD_EL(RS, i, maxi); float vj = MATD_EL(RS, i, maxj); MATD_EL(RS, i, maxi) = V[0]*vi + V[2]*vj; MATD_EL(RS, i, maxj) = V[1]*vi + V[3]*vj; } // B = matd_op("M'*F*M", QL, B, QR); // The QL matrix mixes rows of B. for (int i = 0; i < B->ncols; i++) { float vi = MATD_EL(B, maxi, i); float vj = MATD_EL(B, maxj, i); MATD_EL(B, maxi, i) = U[0]*vi + U[2]*vj; MATD_EL(B, maxj, i) = U[1]*vi + U[3]*vj; } // The QR matrix mixes columns of B. for (int i = 0; i < B->nrows; i++) { float vi = MATD_EL(B, i, maxi); float vj = MATD_EL(B, i, maxj); MATD_EL(B, i, maxi) = V[0]*vi + V[2]*vj; MATD_EL(B, i, maxj) = V[1]*vi + V[3]*vj; } } } if (!(flags & MATD_SVD_NO_WARNINGS) && iter == maxiters) { printf("WARNING: maximum iters (maximum = %d, matrix %d x %d, max=%.15f)\n", iter, A->nrows, A->ncols, (double) maxv); // matd_print(A, "%15f"); } // them all positive by flipping the corresponding columns of // U/LS. int idxs[A->ncols]; float vals[A->ncols]; for (int i = 0; i < A->ncols; i++) { idxs[i] = i; vals[i] = MATD_EL(B, i, i); } // A bubble sort. Seriously. int changed; do { changed = 0; for (int i = 0; i + 1 < A->ncols; i++) { if (fabs(vals[i+1]) > fabs(vals[i])) { int tmpi = idxs[i]; idxs[i] = idxs[i+1]; idxs[i+1] = tmpi; float tmpv = vals[i]; vals[i] = vals[i+1]; vals[i+1] = tmpv; changed = 1; } } } while (changed); matd_t *LP = matd_identity(A->nrows); matd_t *RP = matd_identity(A->ncols); for (int i = 0; i < A->ncols; i++) { MATD_EL(LP, idxs[i], idxs[i]) = 0; // undo the identity above MATD_EL(RP, idxs[i], idxs[i]) = 0; MATD_EL(LP, idxs[i], i) = vals[i] < 0 ? -1 : 1; MATD_EL(RP, idxs[i], i) = 1; //vals[i] < 0 ? -1 : 1; } // we've factored: // LP*(something)*RP' // solve for (something) B = matd_op("M'*F*M", LP, B, RP); // update LS and RS, remembering that RS will be transposed. LS = matd_op("F*M", LS, LP); RS = matd_op("F*M", RS, RP); matd_destroy(LP); matd_destroy(RP); matd_svd_t res; memset(&res, 0, sizeof(res)); // make B exactly diagonal for (int i = 0; i < B->nrows; i++) { for (int j = 0; j < B->ncols; j++) { if (i != j) MATD_EL(B, i, j) = 0; } } res.U = LS; res.S = B; res.V = RS; return res; } matd_svd_t matd_svd(matd_t *A) { return matd_svd_flags(A, 0); } matd_svd_t matd_svd_flags(matd_t *A, int flags) { matd_svd_t res; if (A->ncols <= A->nrows) { res = matd_svd_tall(A, flags); } else { matd_t *At = matd_transpose(A); // A =U S V' // A'=V S' U' matd_svd_t tmp = matd_svd_tall(At, flags); memset(&res, 0, sizeof(res)); res.U = tmp.V; //matd_transpose(tmp.V); res.S = matd_transpose(tmp.S); res.V = tmp.U; //matd_transpose(tmp.U); matd_destroy(tmp.S); matd_destroy(At); } /* matd_t *check = matd_op("M*M*M'-M", res.U, res.S, res.V, A); float maxerr = 0; for (int i = 0; i < check->nrows; i++) for (int j = 0; j < check->ncols; j++) maxerr = fmax(maxerr, fabs(MATD_EL(check, i, j))); matd_destroy(check); if (maxerr > 1e-7) { printf("bad maxerr: %15f\n", maxerr); } if (maxerr > 1e-5) { printf("bad maxerr: %15f\n", maxerr); matd_print(A, "%15f"); assert(0); } */ return res; } matd_plu_t *matd_plu(const matd_t *a) { unsigned int *piv = calloc(a->nrows, sizeof(unsigned int)); int pivsign = 1; matd_t *lu = matd_copy(a); // only for square matrices. assert(a->nrows == a->ncols); matd_plu_t *mlu = calloc(1, sizeof(matd_plu_t)); for (int i = 0; i < a->nrows; i++) piv[i] = i; for (int j = 0; j < a->ncols; j++) { for (int i = 0; i < a->nrows; i++) { int kmax = i < j ? i : j; // min(i,j) // compute dot product of row i with column j (up through element kmax) float acc = 0; for (int k = 0; k < kmax; k++) acc += MATD_EL(lu, i, k) * MATD_EL(lu, k, j); MATD_EL(lu, i, j) -= acc; } // find pivot and exchange if necessary. int p = j; if (1) { for (int i = j+1; i < lu->nrows; i++) { if (fabs(MATD_EL(lu,i,j)) > fabs(MATD_EL(lu, p, j))) { p = i; } } } // swap rows p and j? if (p != j) { TYPE tmp[lu->ncols]; memcpy(tmp, &MATD_EL(lu, p, 0), sizeof(TYPE) * lu->ncols); memcpy(&MATD_EL(lu, p, 0), &MATD_EL(lu, j, 0), sizeof(TYPE) * lu->ncols); memcpy(&MATD_EL(lu, j, 0), tmp, sizeof(TYPE) * lu->ncols); int k = piv[p]; piv[p] = piv[j]; piv[j] = k; pivsign = -pivsign; } float LUjj = MATD_EL(lu, j, j); // If our pivot is very small (which means the matrix is // singular or nearly singular), replace with a new pivot of the // right sign. if (fabs(LUjj) < MATD_EPS) { /* if (LUjj < 0) LUjj = -MATD_EPS; else LUjj = MATD_EPS; MATD_EL(lu, j, j) = LUjj; */ mlu->singular = 1; } if (j < lu->ncols && j < lu->nrows && LUjj != 0) { LUjj = 1.0 / LUjj; for (int i = j+1; i < lu->nrows; i++) MATD_EL(lu, i, j) *= LUjj; } } mlu->lu = lu; mlu->piv = piv; mlu->pivsign = pivsign; return mlu; } void matd_plu_destroy(matd_plu_t *mlu) { matd_destroy(mlu->lu); free(mlu->piv); memset(mlu, 0, sizeof(matd_plu_t)); free(mlu); } float matd_plu_det(const matd_plu_t *mlu) { matd_t *lu = mlu->lu; float det = mlu->pivsign; if (lu->nrows == lu->ncols) { for (int i = 0; i < lu->ncols; i++) det *= MATD_EL(lu, i, i); } return det; } matd_t *matd_plu_p(const matd_plu_t *mlu) { matd_t *lu = mlu->lu; matd_t *P = matd_create(lu->nrows, lu->nrows); for (int i = 0; i < lu->nrows; i++) { MATD_EL(P, mlu->piv[i], i) = 1; } return P; } matd_t *matd_plu_l(const matd_plu_t *mlu) { matd_t *lu = mlu->lu; matd_t *L = matd_create(lu->nrows, lu->ncols); for (int i = 0; i < lu->nrows; i++) { MATD_EL(L, i, i) = 1; for (int j = 0; j < i; j++) { MATD_EL(L, i, j) = MATD_EL(lu, i, j); } } return L; } matd_t *matd_plu_u(const matd_plu_t *mlu) { matd_t *lu = mlu->lu; matd_t *U = matd_create(lu->ncols, lu->ncols); for (int i = 0; i < lu->ncols; i++) { for (int j = 0; j < lu->ncols; j++) { if (i <= j) MATD_EL(U, i, j) = MATD_EL(lu, i, j); } } return U; } // PLU = A // Ax = B // PLUx = B // LUx = P'B matd_t *matd_plu_solve(const matd_plu_t *mlu, const matd_t *b) { matd_t *x = matd_copy(b); // permute right hand side for (int i = 0; i < mlu->lu->nrows; i++) memcpy(&MATD_EL(x, i, 0), &MATD_EL(b, mlu->piv[i], 0), sizeof(TYPE) * b->ncols); // solve Ly = b for (int k = 0; k < mlu->lu->nrows; k++) { for (int i = k+1; i < mlu->lu->nrows; i++) { float LUik = -MATD_EL(mlu->lu, i, k); for (int t = 0; t < b->ncols; t++) MATD_EL(x, i, t) += MATD_EL(x, k, t) * LUik; } } // solve Ux = y for (int k = mlu->lu->ncols-1; k >= 0; k--) { float LUkk = 1.0 / MATD_EL(mlu->lu, k, k); for (int t = 0; t < b->ncols; t++) MATD_EL(x, k, t) *= LUkk; for (int i = 0; i < k; i++) { float LUik = -MATD_EL(mlu->lu, i, k); for (int t = 0; t < b->ncols; t++) MATD_EL(x, i, t) += MATD_EL(x, k, t) *LUik; } } return x; } matd_t *matd_solve(matd_t *A, matd_t *b) { matd_plu_t *mlu = matd_plu(A); matd_t *x = matd_plu_solve(mlu, b); matd_plu_destroy(mlu); return x; } #if 0 static int randi() { int v = random()&31; v -= 15; return v; } static float randf() { float v = 1.0 *random() / RAND_MAX; return 2*v - 1; } int main(int argc, char *argv[]) { if (1) { int maxdim = 16; matd_t *A = matd_create(maxdim, maxdim); for (int iter = 0; 1; iter++) { srand(iter); if (iter % 1000 == 0) printf("%d\n", iter); int m = 1 + (random()%(maxdim-1)); int n = 1 + (random()%(maxdim-1)); for (int i = 0; i < m*n; i++) A->data[i] = randi(); A->nrows = m; A->ncols = n; // printf("%d %d ", m, n); matd_svd_t svd = matd_svd(A); matd_destroy(svd.U); matd_destroy(svd.S); matd_destroy(svd.V); } /* matd_t *A = matd_create_data(2, 5, (float[]) { 1, 5, 2, 6, 3, 3, 0, 7, 1, 1, 0, -2, 4, 0, 9, 9, 2, 6, 1, 3, 2, 5, 5, 4, -1, 2, 5, 9, 8, 2 }); matd_svd(A); */ return 0; } struct svd22 s; srand(0); matd_t *A = matd_create(2, 2); MATD_EL(A,0,0) = 4; MATD_EL(A,0,1) = 7; MATD_EL(A,1,0) = 2; MATD_EL(A,1,1) = 6; matd_t *U = matd_create(2, 2); matd_t *V = matd_create(2, 2); matd_t *S = matd_create(2, 2); for (int iter = 0; 1; iter++) { if (iter % 100000 == 0) printf("%d\n", iter); MATD_EL(A,0,0) = randf(); MATD_EL(A,0,1) = randf(); MATD_EL(A,1,0) = randf(); MATD_EL(A,1,1) = randf(); matd_svd22_impl(A->data, &s); memcpy(U->data, s.U, 4*sizeof(float)); memcpy(V->data, s.V, 4*sizeof(float)); MATD_EL(S,0,0) = s.S[0]; MATD_EL(S,1,1) = s.S[1]; assert(s.S[0] >= s.S[1]); assert(s.S[0] >= 0); assert(s.S[1] >= 0); if (s.S[0] == 0) { // printf("*"); fflush(NULL); // printf("%15f %15f %15f %15f\n", MATD_EL(A,0,0), MATD_EL(A,0,1), MATD_EL(A,1,0), MATD_EL(A,1,1)); } if (s.S[1] == 0) { // printf("#"); fflush(NULL); } matd_t *USV = matd_op("M*M*M'", U, S, V); float maxerr = 0; for (int i = 0; i < 4; i++) maxerr = fmax(maxerr, fabs(USV->data[i] - A->data[i])); if (0) { printf("------------------------------------\n"); printf("A:\n"); matd_print(A, "%15f"); printf("\nUSV':\n"); matd_print(USV, "%15f"); printf("maxerr: %.15f\n", maxerr); printf("\n\n"); } matd_destroy(USV); assert(maxerr < 0.00001); } } #endif // XXX NGV Cholesky /*static float *matd_cholesky_raw(float *A, int n) { float *L = (float*)calloc(n * n, sizeof(float)); for (int i = 0; i < n; i++) { for (int j = 0; j < (i+1); j++) { float s = 0; for (int k = 0; k < j; k++) s += L[i * n + k] * L[j * n + k]; L[i * n + j] = (i == j) ? sqrt(A[i * n + i] - s) : (1.0 / L[j * n + j] * (A[i * n + j] - s)); } } return L; } matd_t *matd_cholesky(const matd_t *A) { assert(A->nrows == A->ncols); float *L_data = matd_cholesky_raw(A->data, A->nrows); matd_t *L = matd_create_data(A->nrows, A->ncols, L_data); free(L_data); return L; }*/ // NOTE: The below implementation of Cholesky is different from the one // used in NGV. matd_chol_t *matd_chol(matd_t *A) { assert(A->nrows == A->ncols); int N = A->nrows; // make upper right matd_t *U = matd_copy(A); // don't actually need to clear lower-left... we won't touch it. /* for (int i = 0; i < U->nrows; i++) { for (int j = 0; j < i; j++) { // assert(MATD_EL(U, i, j) == MATD_EL(U, j, i)); MATD_EL(U, i, j) = 0; } } */ int is_spd = 1; // (A->nrows == A->ncols); for (int i = 0; i < N; i++) { float d = MATD_EL(U, i, i); is_spd &= (d > 0); if (d < MATD_EPS) d = MATD_EPS; d = 1.0 / sqrt(d); for (int j = i; j < N; j++) MATD_EL(U, i, j) *= d; for (int j = i+1; j < N; j++) { float s = MATD_EL(U, i, j); if (s == 0) continue; for (int k = j; k < N; k++) { MATD_EL(U, j, k) -= MATD_EL(U, i, k)*s; } } } matd_chol_t *chol = calloc(1, sizeof(matd_chol_t)); chol->is_spd = is_spd; chol->u = U; return chol; } void matd_chol_destroy(matd_chol_t *chol) { matd_destroy(chol->u); free(chol); } // Solve: (U')x = b, U is upper triangular void matd_ltransposetriangle_solve(matd_t *u, const TYPE *b, TYPE *x) { int n = u->ncols; memcpy(x, b, n*sizeof(TYPE)); for (int i = 0; i < n; i++) { x[i] /= MATD_EL(u, i, i); for (int j = i+1; j < u->ncols; j++) { x[j] -= x[i] * MATD_EL(u, i, j); } } } // Solve: Lx = b, L is lower triangular void matd_ltriangle_solve(matd_t *L, const TYPE *b, TYPE *x) { int n = L->ncols; for (int i = 0; i < n; i++) { float acc = b[i]; for (int j = 0; j < i; j++) { acc -= MATD_EL(L, i, j)*x[j]; } x[i] = acc / MATD_EL(L, i, i); } } // solve Ux = b, U is upper triangular void matd_utriangle_solve(matd_t *u, const TYPE *b, TYPE *x) { for (int i = u->ncols-1; i >= 0; i--) { float bi = b[i]; float diag = MATD_EL(u, i, i); for (int j = i+1; j < u->ncols; j++) bi -= MATD_EL(u, i, j)*x[j]; x[i] = bi / diag; } } matd_t *matd_chol_solve(const matd_chol_t *chol, const matd_t *b) { matd_t *u = chol->u; matd_t *x = matd_copy(b); // LUx = b // solve Ly = b ==> (U')y = b for (int i = 0; i < u->nrows; i++) { for (int j = 0; j < i; j++) { // b[i] -= L[i,j]*x[j]... replicated across columns of b // ==> i.e., ==> // b[i,k] -= L[i,j]*x[j,k] for (int k = 0; k < b->ncols; k++) { MATD_EL(x, i, k) -= MATD_EL(u, j, i)*MATD_EL(x, j, k); } } // x[i] = b[i] / L[i,i] for (int k = 0; k < b->ncols; k++) { MATD_EL(x, i, k) /= MATD_EL(u, i, i); } } // solve Ux = y for (int k = u->ncols-1; k >= 0; k--) { float LUkk = 1.0 / MATD_EL(u, k, k); for (int t = 0; t < b->ncols; t++) MATD_EL(x, k, t) *= LUkk; for (int i = 0; i < k; i++) { float LUik = -MATD_EL(u, i, k); for (int t = 0; t < b->ncols; t++) MATD_EL(x, i, t) += MATD_EL(x, k, t) *LUik; } } return x; } /*void matd_chol_solve(matd_chol_t *chol, const TYPE *b, TYPE *x) { matd_t *u = chol->u; TYPE y[u->ncols]; matd_ltransposetriangle_solve(u, b, y); matd_utriangle_solve(u, y, x); } */ // only sensible on PSD matrices. had expected it to be faster than // inverse via LU... for now, doesn't seem to be. matd_t *matd_chol_inverse(matd_t *a) { assert(a->nrows == a->ncols); matd_chol_t *chol = matd_chol(a); matd_t *eye = matd_identity(a->nrows); matd_t *inv = matd_chol_solve(chol, eye); matd_destroy(eye); matd_chol_destroy(chol); return inv; } float matd_max(matd_t *m) { float d = -FLT_MAX; for(int x=0; xnrows; x++) { for(int y=0; yncols; y++) { if(MATD_EL(m, x, y) > d) d = MATD_EL(m, x, y); } } return d; } //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "homography.h" //////////////////////////////////////////////////////////////////////////////////////////////////// /** Given a 3x3 homography matrix and the focal lengths of the * camera, compute the pose of the tag. The focal lengths should * be given in pixels. For example, if the camera's focal length * is twice the width of the sensor, and the sensor is 600 pixels * across, the focal length in pixels is 2*600. Note that the * focal lengths in the fx and fy direction will be approximately * equal for most lenses, and is not a function of aspect ratio. * * Theory: The homography matrix is the product of the camera * projection matrix and the tag's pose matrix (the matrix that * projects points from the tag's local coordinate system to the * camera's coordinate frame). * * [ h00 h01 h02 h03] = [ fx 0 cx 0 ] [ R00 R01 R02 TX ] * [ h10 h11 h12 h13] = [ 0 fy cy 0 ] [ R10 R11 R12 TY ] * [ h20 h21 h22 h23] = [ 0 0 s 0 ] [ R20 R21 R22 TZ ] * [ 0 0 0 1 ] * * fx is the focal length in the x direction of the camera * (typically measured in pixels), fy is the focal length. cx and * cy give the focal center (usually the middle of the image), and * s is either +1 or -1, depending on the conventions you use. (We * use 1.) * When observing a tag, the points we project in world space all * have z=0, so we can form a 3x3 matrix by eliminating the 3rd * column of the pose matrix. * * [ h00 h01 h02 ] = [ fx 0 cx 0 ] [ R00 R01 TX ] * [ h10 h11 h12 ] = [ 0 fy cy 0 ] [ R10 R11 TY ] * [ h20 h21 h22 ] = [ 0 0 s 0 ] [ R20 R21 TZ ] * [ 0 0 1 ] * * (note that these h's are different from the ones above.) * * We can multiply the right-hand side to yield a set of equations * relating the values of h to the values of the pose matrix. * * There are two wrinkles. The first is that the homography matrix * is known only up to scale. We recover the unknown scale by * constraining the magnitude of the first two columns of the pose * matrix to be 1. We use the geometric average scale. The sign of * the scale factor is recovered by constraining the observed tag * to be in front of the camera. Once scaled, we recover the first * two colmuns of the rotation matrix. The third column is the * cross product of these. * * The second wrinkle is that the computed rotation matrix might * not be exactly orthogonal, so we perform a polar decomposition * to find a good pure rotation approximation. * * Tagsize is the size of the tag in your desired units. I.e., if * your tag measures 0.25m along the side, your tag size is * 0.25. (The homography is computed in terms of *half* the tag * size, i.e., that a tag is 2 units wide as it spans from -1 to * +1, but this code makes the appropriate adjustment.) * * A note on signs: * * The code below incorporates no additional negative signs, but * respects the sign of any parameters that you pass in. Flipping * the signs allows you to modify the projection to suit a wide * variety of conditions. * * In the "pure geometry" projection matrix, the image appears * upside down; i.e., the x and y coordinates on the left hand * side are the opposite of those on the right of the camera * projection matrix. This would happen for all parameters * positive: recall that points in front of the camera have * negative Z values, which will cause the sign of all points to * flip. * * However, most cameras flip things so that the image appears * "right side up" as though you were looking through the lens * directly. This means that the projected points should have the * same sign as the points on the right of the camera projection * matrix. To achieve this, flip fx and fy. * * One further complication: cameras typically put y=0 at the top * of the image, instead of the bottom. Thus you generally want to * flip y yet again (so it's now positive again). * * General advice: you probably want fx negative, fy positive, cx * and cy positive, and s=1. **/ // correspondences is a list of float[4]s, consisting of the points x // and y concatenated. We will compute a homography such that y = Hx // Specifically, float [] { a, b, c, d } where x = [a b], y = [c d]. #define HOMOGRAPHY_COMPUTE_FLAG_INVERSE 1 #define HOMOGRAPHY_COMPUTE_FLAG_SVD 0 matd_t *homography_compute(zarray_t *correspondences, int flags); //void homography_project(const matd_t *H, float x, float y, float *ox, float *oy); static inline void homography_project(const matd_t *H, float x, float y, float *ox, float *oy) { float xx = MATD_EL(H, 0, 0)*x + MATD_EL(H, 0, 1)*y + MATD_EL(H, 0, 2); float yy = MATD_EL(H, 1, 0)*x + MATD_EL(H, 1, 1)*y + MATD_EL(H, 1, 2); float zz = MATD_EL(H, 2, 0)*x + MATD_EL(H, 2, 1)*y + MATD_EL(H, 2, 2); *ox = xx / zz; *oy = yy / zz; } // assuming that the projection matrix is: // [ fx 0 cx 0 ] // [ 0 fy cy 0 ] // [ 0 0 1 0 ] // // And that the homography is equal to the projection matrix times the model matrix, // recover the model matrix (which is returned). Note that the third column of the model // matrix is missing in the expresison below, reflecting the fact that the homography assumes // all points are at z=0 (i.e., planar) and that the element of z is thus omitted. // (3x1 instead of 4x1). // // [ fx 0 cx 0 ] [ R00 R01 TX ] [ H00 H01 H02 ] // [ 0 fy cy 0 ] [ R10 R11 TY ] = [ H10 H11 H12 ] // [ 0 0 1 0 ] [ R20 R21 TZ ] = [ H20 H21 H22 ] // [ 0 0 1 ] // // fx*R00 + cx*R20 = H00 (note, H only known up to scale; some additional adjustments required; see code.) // fx*R01 + cx*R21 = H01 // fx*TX + cx*TZ = H02 // fy*R10 + cy*R20 = H10 // fy*R11 + cy*R21 = H11 // fy*TY + cy*TZ = H12 // R20 = H20 // R21 = H21 // TZ = H22 matd_t *homography_to_pose(const matd_t *H, float fx, float fy, float cx, float cy); // Similar to above // Recover the model view matrix assuming that the projection matrix is: // // [ F 0 A 0 ] (see glFrustrum) // [ 0 G B 0 ] // [ 0 0 C D ] // [ 0 0 -1 0 ] matd_t *homography_to_model_view(const matd_t *H, float F, float G, float A, float B, float C, float D); //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "homography.c" //////////////////////////////////////////////////////////////////////////////////////////////////// // correspondences is a list of float[4]s, consisting of the points x // and y concatenated. We will compute a homography such that y = Hx matd_t *homography_compute(zarray_t *correspondences, int flags) { // compute centroids of both sets of points (yields a better // conditioned information matrix) float x_cx = 0, x_cy = 0; float y_cx = 0, y_cy = 0; for (int i = 0; i < zarray_size(correspondences); i++) { float *c; zarray_get_volatile(correspondences, i, &c); x_cx += c[0]; x_cy += c[1]; y_cx += c[2]; y_cy += c[3]; } int sz = zarray_size(correspondences); x_cx /= sz; x_cy /= sz; y_cx /= sz; y_cy /= sz; // NB We don't normalize scale; it seems implausible that it could // possibly make any difference given the dynamic range of IEEE // doubles. matd_t *A = matd_create(9,9); for (int i = 0; i < zarray_size(correspondences); i++) { float *c; zarray_get_volatile(correspondences, i, &c); // (below world is "x", and image is "y") float worldx = c[0] - x_cx; float worldy = c[1] - x_cy; float imagex = c[2] - y_cx; float imagey = c[3] - y_cy; float a03 = -worldx; float a04 = -worldy; float a05 = -1; float a06 = worldx*imagey; float a07 = worldy*imagey; float a08 = imagey; MATD_EL(A, 3, 3) += a03*a03; MATD_EL(A, 3, 4) += a03*a04; MATD_EL(A, 3, 5) += a03*a05; MATD_EL(A, 3, 6) += a03*a06; MATD_EL(A, 3, 7) += a03*a07; MATD_EL(A, 3, 8) += a03*a08; MATD_EL(A, 4, 4) += a04*a04; MATD_EL(A, 4, 5) += a04*a05; MATD_EL(A, 4, 6) += a04*a06; MATD_EL(A, 4, 7) += a04*a07; MATD_EL(A, 4, 8) += a04*a08; MATD_EL(A, 5, 5) += a05*a05; MATD_EL(A, 5, 6) += a05*a06; MATD_EL(A, 5, 7) += a05*a07; MATD_EL(A, 5, 8) += a05*a08; MATD_EL(A, 6, 6) += a06*a06; MATD_EL(A, 6, 7) += a06*a07; MATD_EL(A, 6, 8) += a06*a08; MATD_EL(A, 7, 7) += a07*a07; MATD_EL(A, 7, 8) += a07*a08; MATD_EL(A, 8, 8) += a08*a08; float a10 = worldx; float a11 = worldy; float a12 = 1; float a16 = -worldx*imagex; float a17 = -worldy*imagex; float a18 = -imagex; MATD_EL(A, 0, 0) += a10*a10; MATD_EL(A, 0, 1) += a10*a11; MATD_EL(A, 0, 2) += a10*a12; MATD_EL(A, 0, 6) += a10*a16; MATD_EL(A, 0, 7) += a10*a17; MATD_EL(A, 0, 8) += a10*a18; MATD_EL(A, 1, 1) += a11*a11; MATD_EL(A, 1, 2) += a11*a12; MATD_EL(A, 1, 6) += a11*a16; MATD_EL(A, 1, 7) += a11*a17; MATD_EL(A, 1, 8) += a11*a18; MATD_EL(A, 2, 2) += a12*a12; MATD_EL(A, 2, 6) += a12*a16; MATD_EL(A, 2, 7) += a12*a17; MATD_EL(A, 2, 8) += a12*a18; MATD_EL(A, 6, 6) += a16*a16; MATD_EL(A, 6, 7) += a16*a17; MATD_EL(A, 6, 8) += a16*a18; MATD_EL(A, 7, 7) += a17*a17; MATD_EL(A, 7, 8) += a17*a18; MATD_EL(A, 8, 8) += a18*a18; float a20 = -worldx*imagey; float a21 = -worldy*imagey; float a22 = -imagey; float a23 = worldx*imagex; float a24 = worldy*imagex; float a25 = imagex; MATD_EL(A, 0, 0) += a20*a20; MATD_EL(A, 0, 1) += a20*a21; MATD_EL(A, 0, 2) += a20*a22; MATD_EL(A, 0, 3) += a20*a23; MATD_EL(A, 0, 4) += a20*a24; MATD_EL(A, 0, 5) += a20*a25; MATD_EL(A, 1, 1) += a21*a21; MATD_EL(A, 1, 2) += a21*a22; MATD_EL(A, 1, 3) += a21*a23; MATD_EL(A, 1, 4) += a21*a24; MATD_EL(A, 1, 5) += a21*a25; MATD_EL(A, 2, 2) += a22*a22; MATD_EL(A, 2, 3) += a22*a23; MATD_EL(A, 2, 4) += a22*a24; MATD_EL(A, 2, 5) += a22*a25; MATD_EL(A, 3, 3) += a23*a23; MATD_EL(A, 3, 4) += a23*a24; MATD_EL(A, 3, 5) += a23*a25; MATD_EL(A, 4, 4) += a24*a24; MATD_EL(A, 4, 5) += a24*a25; MATD_EL(A, 5, 5) += a25*a25; } // make symmetric for (int i = 0; i < 9; i++) for (int j = i+1; j < 9; j++) MATD_EL(A, j, i) = MATD_EL(A, i, j); matd_t *H = matd_create(3,3); if (flags & HOMOGRAPHY_COMPUTE_FLAG_INVERSE) { // compute singular vector by (carefully) inverting the rank-deficient matrix. if (1) { matd_t *Ainv = matd_inverse(A); float scale = 0; for (int i = 0; i < 9; i++) scale += sq(MATD_EL(Ainv, i, 0)); scale = sqrt(scale); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0) / scale; matd_destroy(Ainv); } else { matd_t *b = matd_create_data(9, 1, (float[]) { 1, 0, 0, 0, 0, 0, 0, 0, 0 }); matd_t *Ainv = NULL; if (0) { matd_plu_t *lu = matd_plu(A); Ainv = matd_plu_solve(lu, b); matd_plu_destroy(lu); } else { matd_chol_t *chol = matd_chol(A); Ainv = matd_chol_solve(chol, b); matd_chol_destroy(chol); } float scale = 0; for (int i = 0; i < 9; i++) scale += sq(MATD_EL(Ainv, i, 0)); scale = sqrt(scale); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0) / scale; matd_destroy(b); matd_destroy(Ainv); } } else { // compute singular vector using SVD. A bit slower, but more accurate. matd_svd_t svd = matd_svd_flags(A, MATD_SVD_NO_WARNINGS); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) MATD_EL(H, i, j) = MATD_EL(svd.U, 3*i+j, 8); matd_destroy(svd.U); matd_destroy(svd.S); matd_destroy(svd.V); } matd_t *Tx = matd_identity(3); MATD_EL(Tx,0,2) = -x_cx; MATD_EL(Tx,1,2) = -x_cy; matd_t *Ty = matd_identity(3); MATD_EL(Ty,0,2) = y_cx; MATD_EL(Ty,1,2) = y_cy; matd_t *H2 = matd_op("M*M*M", Ty, H, Tx); matd_destroy(A); matd_destroy(Tx); matd_destroy(Ty); matd_destroy(H); return H2; } // assuming that the projection matrix is: // [ fx 0 cx 0 ] // [ 0 fy cy 0 ] // [ 0 0 1 0 ] // // And that the homography is equal to the projection matrix times the // model matrix, recover the model matrix (which is returned). Note // that the third column of the model matrix is missing in the // expresison below, reflecting the fact that the homography assumes // all points are at z=0 (i.e., planar) and that the element of z is // thus omitted. (3x1 instead of 4x1). // // [ fx 0 cx 0 ] [ R00 R01 TX ] [ H00 H01 H02 ] // [ 0 fy cy 0 ] [ R10 R11 TY ] = [ H10 H11 H12 ] // [ 0 0 1 0 ] [ R20 R21 TZ ] = [ H20 H21 H22 ] // [ 0 0 1 ] // // fx*R00 + cx*R20 = H00 (note, H only known up to scale; some additional adjustments required; see code.) // fx*R01 + cx*R21 = H01 // fx*TX + cx*TZ = H02 // fy*R10 + cy*R20 = H10 // fy*R11 + cy*R21 = H11 // fy*TY + cy*TZ = H12 // R20 = H20 // R21 = H21 // TZ = H22 matd_t *homography_to_pose(const matd_t *H, float fx, float fy, float cx, float cy) { // Note that every variable that we compute is proportional to the scale factor of H. float R20 = MATD_EL(H, 2, 0); float R21 = MATD_EL(H, 2, 1); float TZ = MATD_EL(H, 2, 2); float R00 = (MATD_EL(H, 0, 0) - cx*R20) / fx; float R01 = (MATD_EL(H, 0, 1) - cx*R21) / fx; float TX = (MATD_EL(H, 0, 2) - cx*TZ) / fx; float R10 = (MATD_EL(H, 1, 0) - cy*R20) / fy; float R11 = (MATD_EL(H, 1, 1) - cy*R21) / fy; float TY = (MATD_EL(H, 1, 2) - cy*TZ) / fy; // compute the scale by requiring that the rotation columns are unit length // (Use geometric average of the two length vectors we have) float length1 = sqrtf(R00*R00 + R10*R10 + R20*R20); float length2 = sqrtf(R01*R01 + R11*R11 + R21*R21); float s = 1.0 / sqrtf(length1 * length2); // get sign of S by requiring the tag to be in front the camera; // we assume camera looks in the -Z direction. if (TZ > 0) s *= -1; R20 *= s; R21 *= s; TZ *= s; R00 *= s; R01 *= s; TX *= s; R10 *= s; R11 *= s; TY *= s; // now recover [R02 R12 R22] by noting that it is the cross product of the other two columns. float R02 = R10*R21 - R20*R11; float R12 = R20*R01 - R00*R21; float R22 = R00*R11 - R10*R01; // Improve rotation matrix by applying polar decomposition. if (1) { // do polar decomposition. This makes the rotation matrix // "proper", but probably increases the reprojection error. An // iterative alignment step would be superior. matd_t *R = matd_create_data(3, 3, (float[]) { R00, R01, R02, R10, R11, R12, R20, R21, R22 }); matd_svd_t svd = matd_svd(R); matd_destroy(R); R = matd_op("M*M'", svd.U, svd.V); matd_destroy(svd.U); matd_destroy(svd.S); matd_destroy(svd.V); R00 = MATD_EL(R, 0, 0); R01 = MATD_EL(R, 0, 1); R02 = MATD_EL(R, 0, 2); R10 = MATD_EL(R, 1, 0); R11 = MATD_EL(R, 1, 1); R12 = MATD_EL(R, 1, 2); R20 = MATD_EL(R, 2, 0); R21 = MATD_EL(R, 2, 1); R22 = MATD_EL(R, 2, 2); matd_destroy(R); } return matd_create_data(4, 4, (float[]) { R00, R01, R02, TX, R10, R11, R12, TY, R20, R21, R22, TZ, 0, 0, 0, 1 }); } // Similar to above // Recover the model view matrix assuming that the projection matrix is: // // [ F 0 A 0 ] (see glFrustrum) // [ 0 G B 0 ] // [ 0 0 C D ] // [ 0 0 -1 0 ] matd_t *homography_to_model_view(const matd_t *H, float F, float G, float A, float B, float C, float D) { // Note that every variable that we compute is proportional to the scale factor of H. float R20 = -MATD_EL(H, 2, 0); float R21 = -MATD_EL(H, 2, 1); float TZ = -MATD_EL(H, 2, 2); float R00 = (MATD_EL(H, 0, 0) - A*R20) / F; float R01 = (MATD_EL(H, 0, 1) - A*R21) / F; float TX = (MATD_EL(H, 0, 2) - A*TZ) / F; float R10 = (MATD_EL(H, 1, 0) - B*R20) / G; float R11 = (MATD_EL(H, 1, 1) - B*R21) / G; float TY = (MATD_EL(H, 1, 2) - B*TZ) / G; // compute the scale by requiring that the rotation columns are unit length // (Use geometric average of the two length vectors we have) float length1 = sqrtf(R00*R00 + R10*R10 + R20*R20); float length2 = sqrtf(R01*R01 + R11*R11 + R21*R21); float s = 1.0 / sqrtf(length1 * length2); // get sign of S by requiring the tag to be in front of the camera // (which is Z < 0) for our conventions. if (TZ > 0) s *= -1; R20 *= s; R21 *= s; TZ *= s; R00 *= s; R01 *= s; TX *= s; R10 *= s; R11 *= s; TY *= s; // now recover [R02 R12 R22] by noting that it is the cross product of the other two columns. float R02 = R10*R21 - R20*R11; float R12 = R20*R01 - R00*R21; float R22 = R00*R11 - R10*R01; // TODO XXX: Improve rotation matrix by applying polar decomposition. return matd_create_data(4, 4, (float[]) { R00, R01, R02, TX, R10, R11, R12, TY, R20, R21, R22, TZ, 0, 0, 0, 1 }); } // Only uses the upper 3x3 matrix. /* static void matrix_to_quat(const matd_t *R, float q[4]) { // see: "from quaternion to matrix and back" // trace: get the same result if R is 4x4 or 3x3: float T = MATD_EL(R, 0, 0) + MATD_EL(R, 1, 1) + MATD_EL(R, 2, 2) + 1; float S = 0; float m0 = MATD_EL(R, 0, 0); float m1 = MATD_EL(R, 1, 0); float m2 = MATD_EL(R, 2, 0); float m4 = MATD_EL(R, 0, 1); float m5 = MATD_EL(R, 1, 1); float m6 = MATD_EL(R, 2, 1); float m8 = MATD_EL(R, 0, 2); float m9 = MATD_EL(R, 1, 2); float m10 = MATD_EL(R, 2, 2); if (T > 0.0000001) { S = sqrtf(T) * 2; q[1] = -( m9 - m6 ) / S; q[2] = -( m2 - m8 ) / S; q[3] = -( m4 - m1 ) / S; q[0] = 0.25 * S; } else if ( m0 > m5 && m0 > m10 ) { // Column 0: S = sqrtf( 1.0 + m0 - m5 - m10 ) * 2; q[1] = -0.25 * S; q[2] = -(m4 + m1 ) / S; q[3] = -(m2 + m8 ) / S; q[0] = (m9 - m6 ) / S; } else if ( m5 > m10 ) { // Column 1: S = sqrtf( 1.0 + m5 - m0 - m10 ) * 2; q[1] = -(m4 + m1 ) / S; q[2] = -0.25 * S; q[3] = -(m9 + m6 ) / S; q[0] = (m2 - m8 ) / S; } else { // Column 2: S = sqrtf( 1.0 + m10 - m0 - m5 ) * 2; q[1] = -(m2 + m8 ) / S; q[2] = -(m9 + m6 ) / S; q[3] = -0.25 * S; q[0] = (m4 - m1 ) / S; } float mag2 = 0; for (int i = 0; i < 4; i++) mag2 += q[i]*q[i]; float norm = 1.0 / sqrtf(mag2); for (int i = 0; i < 4; i++) q[i] *= norm; } */ // overwrites upper 3x3 area of matrix M. Doesn't touch any other elements of M. void quat_to_matrix(const float q[4], matd_t *M) { float w = q[0], x = q[1], y = q[2], z = q[3]; MATD_EL(M, 0, 0) = w*w + x*x - y*y - z*z; MATD_EL(M, 0, 1) = 2*x*y - 2*w*z; MATD_EL(M, 0, 2) = 2*x*z + 2*w*y; MATD_EL(M, 1, 0) = 2*x*y + 2*w*z; MATD_EL(M, 1, 1) = w*w - x*x + y*y - z*z; MATD_EL(M, 1, 2) = 2*y*z - 2*w*x; MATD_EL(M, 2, 0) = 2*x*z - 2*w*y; MATD_EL(M, 2, 1) = 2*y*z + 2*w*x; MATD_EL(M, 2, 2) = w*w - x*x - y*y + z*z; } //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "g2d.h" //////////////////////////////////////////////////////////////////////////////////////////////////// // This library tries to avoid needless proliferation of types. // // A point is a float[2]. (Note that when passing a float[2] as an // argument, it is passed by pointer, not by value.) // // A polygon is a zarray_t of float[2]. (Note that in this case, the // zarray contains the actual vertex data, and not merely a pointer to // some other data. IMPORTANT: A polygon must be specified in CCW // order. It is implicitly closed (do not list the same point at the // beginning at the end. // // Where sensible, it is assumed that objects should be allocated // sparingly; consequently "init" style methods, rather than "create" // methods are used. //////////////////////////////////////////////////////////////////// // Lines typedef struct { // Internal representation: a point that the line goes through (p) and // the direction of the line (u). float p[2]; float u[2]; // always a unit vector } g2d_line_t; // initialize a line object. void g2d_line_init_from_points(g2d_line_t *line, const float p0[2], const float p1[2]); // The line defines a one-dimensional coordinate system whose origin // is p. Where is q? (If q is not on the line, the point nearest q is // returned. float g2d_line_get_coordinate(const g2d_line_t *line, const float q[2]); // Intersect two lines. The intersection, if it exists, is written to // p (if not NULL), and 1 is returned. Else, zero is returned. int g2d_line_intersect_line(const g2d_line_t *linea, const g2d_line_t *lineb, float *p); //////////////////////////////////////////////////////////////////// // Line Segments. line.p is always one endpoint; p1 is the other // endpoint. typedef struct { g2d_line_t line; float p1[2]; } g2d_line_segment_t; void g2d_line_segment_init_from_points(g2d_line_segment_t *seg, const float p0[2], const float p1[2]); // Intersect two segments. The intersection, if it exists, is written // to p (if not NULL), and 1 is returned. Else, zero is returned. int g2d_line_segment_intersect_segment(const g2d_line_segment_t *sega, const g2d_line_segment_t *segb, float *p); void g2d_line_segment_closest_point(const g2d_line_segment_t *seg, const float *q, float *p); float g2d_line_segment_closest_point_distance(const g2d_line_segment_t *seg, const float *q); //////////////////////////////////////////////////////////////////// // Polygons zarray_t *g2d_polygon_create_data(float v[][2], int sz); zarray_t *g2d_polygon_create_zeros(int sz); zarray_t *g2d_polygon_create_empty(); void g2d_polygon_add(zarray_t *poly, float v[2]); // Takes a polygon in either CW or CCW and modifies it (if necessary) // to be CCW. void g2d_polygon_make_ccw(zarray_t *poly); // Return 1 if point q lies within poly. int g2d_polygon_contains_point(const zarray_t *poly, float q[2]); // Do the edges of the polygons cross? (Does not test for containment). int g2d_polygon_intersects_polygon(const zarray_t *polya, const zarray_t *polyb); // Does polya completely contain polyb? int g2d_polygon_contains_polygon(const zarray_t *polya, const zarray_t *polyb); // Is there some point which is in both polya and polyb? int g2d_polygon_overlaps_polygon(const zarray_t *polya, const zarray_t *polyb); // returns the number of points written to x. see comments. int g2d_polygon_rasterize(const zarray_t *poly, float y, float *x); //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "g2d.c" //////////////////////////////////////////////////////////////////////////////////////////////////// float g2d_distance(const float a[2], const float b[2]) { return sqrtf(sq(a[0]-b[0]) + sq(a[1]-b[1])); } zarray_t *g2d_polygon_create_empty() { return zarray_create(sizeof(float[2])); } void g2d_polygon_add(zarray_t *poly, float v[2]) { zarray_add(poly, v); } zarray_t *g2d_polygon_create_data(float v[][2], int sz) { zarray_t *points = g2d_polygon_create_empty(); for (int i = 0; i < sz; i++) g2d_polygon_add(points, v[i]); return points; } zarray_t *g2d_polygon_create_zeros(int sz) { zarray_t *points = zarray_create(sizeof(float[2])); float z[2] = { 0, 0 }; for (int i = 0; i < sz; i++) zarray_add(points, z); return points; } void g2d_polygon_make_ccw(zarray_t *poly) { // Step one: we want the points in counter-clockwise order. // If the points are in clockwise order, we'll reverse them. float total_theta = 0; float last_theta = 0; // Count the angle accumulated going around the polygon. If // the sum is +2pi, it's CCW. Otherwise, we'll get -2pi. int sz = zarray_size(poly); for (int i = 0; i <= sz; i++) { float p0[2], p1[2]; zarray_get(poly, i % sz, &p0); zarray_get(poly, (i+1) % sz, &p1); float this_theta = atan2(p1[1]-p0[1], p1[0]-p0[0]); if (i > 0) { float dtheta = mod2pi(this_theta-last_theta); total_theta += dtheta; } last_theta = this_theta; } int ccw = (total_theta > 0); // reverse order if necessary. if (!ccw) { for (int i = 0; i < sz / 2; i++) { float a[2], b[2]; zarray_get(poly, i, a); zarray_get(poly, sz-1-i, b); zarray_set(poly, i, b, NULL); zarray_set(poly, sz-1-i, a, NULL); } } } int g2d_polygon_contains_point_ref(const zarray_t *poly, float q[2]) { // use winding. If the point is inside the polygon, we'll wrap // around it (accumulating 6.28 radians). If we're outside the // polygon, we'll accumulate zero. int psz = zarray_size(poly); float acc_theta = 0; float last_theta; for (int i = 0; i <= psz; i++) { float p[2]; zarray_get(poly, i % psz, &p); float this_theta = atan2(q[1]-p[1], q[0]-p[0]); if (i != 0) acc_theta += mod2pi(this_theta - last_theta); last_theta = this_theta; } return acc_theta > M_PI; } /* // sort by x coordinate, ascending static int g2d_convex_hull_sort(const void *_a, const void *_b) { float *a = (float*) _a; float *b = (float*) _b; if (a[0] < b[0]) return -1; if (a[0] == b[0]) return 0; return 1; } */ /* zarray_t *g2d_convex_hull2(const zarray_t *points) { zarray_t *hull = zarray_copy(points); zarray_sort(hull, g2d_convex_hull_sort); int hsz = zarray_size(hull); int hout = 0; for (int hin = 1; hin < hsz; hin++) { float *p; zarray_get_volatile(hull, i, &p); // Everything to the right of hin is already convex. We now // add one point, p, which begins "connected" by two // (coincident) edges from the last right-most point to p. float *last; zarray_get_volatile(hull, hout, &last); // We now remove points from the convex hull by moving } return hull; } */ // creates and returns a zarray(float[2]). The resulting polygon is // CCW and implicitly closed. Unnecessary colinear points are omitted. zarray_t *g2d_convex_hull(const zarray_t *points) { zarray_t *hull = zarray_create(sizeof(float[2])); // gift-wrap algorithm. // step 1: find left most point. int insz = zarray_size(points); // must have at least 2 points. (XXX need 3?) assert(insz >= 2); float *pleft = NULL; for (int i = 0; i < insz; i++) { float *p; zarray_get_volatile(points, i, &p); if (pleft == NULL || p[0] < pleft[0]) pleft = p; } // cannot be NULL since there must be at least one point. assert(pleft != NULL); zarray_add(hull, pleft); // step 2. gift wrap. Keep searching for points that make the // smallest-angle left-hand turn. This implementation is carefully // written to use only addition/subtraction/multiply. No division // or sqrts. This guarantees exact results for integer-coordinate // polygons (no rounding/precision problems). float *p = pleft; while (1) { assert(p != NULL); float *q = NULL; float n0 = 0, n1 = 0; // the normal to the line (p, q) (not // necessarily unit length). // Search for the point q for which the line (p,q) is most "to // the right of" the other points. (i.e., every time we find a // point that is to the right of our current line, we change // lines.) for (int i = 0; i < insz; i++) { float *thisq; zarray_get_volatile(points, i, &thisq); if (thisq == p) continue; // the first time we find another point, we initialize our // value of q, forming the line (p,q) if (q == NULL) { q = thisq; n0 = q[1] - p[1]; n1 = -q[0] + p[0]; } else { // we already have a line (p,q). is point thisq RIGHT OF line (p, q)? float e0 = thisq[0] - p[0], e1 = thisq[1] - p[1]; float dot = e0*n0 + e1*n1; if (dot > 0) { // it is. change our line. q = thisq; n0 = q[1] - p[1]; n1 = -q[0] + p[0]; } } } // we must have elected *some* line, so long as there are at // least 2 points in the polygon. assert(q != NULL); // loop completed? if (q == pleft) break; int colinear = 0; // is this new point colinear with the last two? if (zarray_size(hull) > 1) { float *o; zarray_get_volatile(hull, zarray_size(hull) - 2, &o); float e0 = o[0] - p[0]; float e1 = o[1] - p[1]; if (n0*e0 + n1*e1 == 0) colinear = 1; } // if it is colinear, overwrite the last one. if (colinear) zarray_set(hull, zarray_size(hull)-1, q, NULL); else zarray_add(hull, q); p = q; } return hull; } // Find point p on the boundary of poly that is closest to q. void g2d_polygon_closest_boundary_point(const zarray_t *poly, const float q[2], float *p) { int psz = zarray_size(poly); float min_dist = HUGE_VALF; for (int i = 0; i < psz; i++) { float *p0, *p1; zarray_get_volatile(poly, i, &p0); zarray_get_volatile(poly, (i+1) % psz, &p1); g2d_line_segment_t seg; g2d_line_segment_init_from_points(&seg, p0, p1); float thisp[2]; g2d_line_segment_closest_point(&seg, q, thisp); float dist = g2d_distance(q, thisp); if (dist < min_dist) { memcpy(p, thisp, sizeof(float[2])); min_dist = dist; } } } int g2d_polygon_contains_point(const zarray_t *poly, float q[2]) { // use winding. If the point is inside the polygon, we'll wrap // around it (accumulating 6.28 radians). If we're outside the // polygon, we'll accumulate zero. int psz = zarray_size(poly); assert(psz > 0); int last_quadrant; int quad_acc = 0; for (int i = 0; i <= psz; i++) { float *p; zarray_get_volatile(poly, i % psz, &p); // p[0] < q[0] p[1] < q[1] quadrant // 0 0 0 // 0 1 3 // 1 0 1 // 1 1 2 // p[1] < q[1] p[0] < q[0] quadrant // 0 0 0 // 0 1 1 // 1 0 3 // 1 1 2 int quadrant; if (p[0] < q[0]) quadrant = (p[1] < q[1]) ? 2 : 1; else quadrant = (p[1] < q[1]) ? 3 : 0; if (i > 0) { int dquadrant = quadrant - last_quadrant; // encourage a jump table by mapping to small positive integers. switch (dquadrant) { case -3: case 1: quad_acc ++; break; case -1: case 3: quad_acc --; break; case 0: break; case -2: case 2: { // get the previous point. float *p0; zarray_get_volatile(poly, i-1, &p0); // Consider the points p0 and p (the points around the //polygon that we are tracing) and the query point q. // // If we've moved diagonally across quadrants, we want // to measure whether we have rotated +PI radians or // -PI radians. We can test this by computing the dot // product of vector (p0-q) with the vector // perpendicular to vector (p-q) float nx = p[1] - q[1]; float ny = -p[0] + q[0]; float dot = nx*(p0[0]-q[0]) + ny*(p0[1]-q[1]); if (dot < 0) quad_acc -= 2; else quad_acc += 2; break; } } } last_quadrant = quadrant; } int v = (quad_acc >= 2) || (quad_acc <= -2); if (0 && v != g2d_polygon_contains_point_ref(poly, q)) { printf("FAILURE %d %d\n", v, quad_acc); exit(-1); } return v; } void g2d_line_init_from_points(g2d_line_t *line, const float p0[2], const float p1[2]) { line->p[0] = p0[0]; line->p[1] = p0[1]; line->u[0] = p1[0]-p0[0]; line->u[1] = p1[1]-p0[1]; float mag = sqrtf(sq(line->u[0]) + sq(line->u[1])); line->u[0] /= mag; line->u[1] /= mag; } float g2d_line_get_coordinate(const g2d_line_t *line, const float q[2]) { return (q[0]-line->p[0])*line->u[0] + (q[1]-line->p[1])*line->u[1]; } // Compute intersection of two line segments. If they intersect, // result is stored in p and 1 is returned. Otherwise, zero is // returned. p may be NULL. int g2d_line_intersect_line(const g2d_line_t *linea, const g2d_line_t *lineb, float *p) { // this implementation is many times faster than the original, // mostly due to avoiding a general-purpose LU decomposition in // Matrix.inverse(). float m00, m01, m10, m11; float i00, i01; float b00, b10; m00 = linea->u[0]; m01= -lineb->u[0]; m10 = linea->u[1]; m11= -lineb->u[1]; // determinant of m float det = m00*m11-m01*m10; // parallel lines? if (fabs(det) < 0.00000001) return 0; // inverse of m i00 = m11/det; i01 = -m01/det; b00 = lineb->p[0] - linea->p[0]; b10 = lineb->p[1] - linea->p[1]; float x00; //, x10; x00 = i00*b00+i01*b10; if (p != NULL) { p[0] = linea->u[0]*x00 + linea->p[0]; p[1] = linea->u[1]*x00 + linea->p[1]; } return 1; } void g2d_line_segment_init_from_points(g2d_line_segment_t *seg, const float p0[2], const float p1[2]) { g2d_line_init_from_points(&seg->line, p0, p1); seg->p1[0] = p1[0]; seg->p1[1] = p1[1]; } // Find the point p on segment seg that is closest to point q. void g2d_line_segment_closest_point(const g2d_line_segment_t *seg, const float *q, float *p) { float a = g2d_line_get_coordinate(&seg->line, seg->line.p); float b = g2d_line_get_coordinate(&seg->line, seg->p1); float c = g2d_line_get_coordinate(&seg->line, q); if (a < b) c = dclamp(c, a, b); else c = dclamp(c, b, a); p[0] = seg->line.p[0] + c * seg->line.u[0]; p[1] = seg->line.p[1] + c * seg->line.u[1]; } // Compute intersection of two line segments. If they intersect, // result is stored in p and 1 is returned. Otherwise, zero is // returned. p may be NULL. int g2d_line_segment_intersect_segment(const g2d_line_segment_t *sega, const g2d_line_segment_t *segb, float *p) { float tmp[2]; if (!g2d_line_intersect_line(&sega->line, &segb->line, tmp)) return 0; float a = g2d_line_get_coordinate(&sega->line, sega->line.p); float b = g2d_line_get_coordinate(&sega->line, sega->p1); float c = g2d_line_get_coordinate(&sega->line, tmp); // does intersection lie on the first line? if ((ca && c>b)) return 0; a = g2d_line_get_coordinate(&segb->line, segb->line.p); b = g2d_line_get_coordinate(&segb->line, segb->p1); c = g2d_line_get_coordinate(&segb->line, tmp); // does intersection lie on second line? if ((ca && c>b)) return 0; if (p != NULL) { p[0] = tmp[0]; p[1] = tmp[1]; } return 1; } // Compute intersection of a line segment and a line. If they // intersect, result is stored in p and 1 is returned. Otherwise, zero // is returned. p may be NULL. int g2d_line_segment_intersect_line(const g2d_line_segment_t *seg, const g2d_line_t *line, float *p) { float tmp[2]; if (!g2d_line_intersect_line(&seg->line, line, tmp)) return 0; float a = g2d_line_get_coordinate(&seg->line, seg->line.p); float b = g2d_line_get_coordinate(&seg->line, seg->p1); float c = g2d_line_get_coordinate(&seg->line, tmp); // does intersection lie on the first line? if ((ca && c>b)) return 0; if (p != NULL) { p[0] = tmp[0]; p[1] = tmp[1]; } return 1; } // do the edges of polya and polyb collide? (Does NOT test for containment). int g2d_polygon_intersects_polygon(const zarray_t *polya, const zarray_t *polyb) { // do any of the line segments collide? If so, the answer is no. // dumb N^2 method. for (int ia = 0; ia < zarray_size(polya); ia++) { float pa0[2], pa1[2]; zarray_get(polya, ia, pa0); zarray_get(polya, (ia+1)%zarray_size(polya), pa1); g2d_line_segment_t sega; g2d_line_segment_init_from_points(&sega, pa0, pa1); for (int ib = 0; ib < zarray_size(polyb); ib++) { float pb0[2], pb1[2]; zarray_get(polyb, ib, pb0); zarray_get(polyb, (ib+1)%zarray_size(polyb), pb1); g2d_line_segment_t segb; g2d_line_segment_init_from_points(&segb, pb0, pb1); if (g2d_line_segment_intersect_segment(&sega, &segb, NULL)) return 1; } } return 0; } // does polya completely contain polyb? int g2d_polygon_contains_polygon(const zarray_t *polya, const zarray_t *polyb) { // do any of the line segments collide? If so, the answer is no. if (g2d_polygon_intersects_polygon(polya, polyb)) return 0; // if none of the edges cross, then the polygon is either fully // contained or fully outside. float p[2]; zarray_get(polyb, 0, p); return g2d_polygon_contains_point(polya, p); } // compute a point that is inside the polygon. (It may not be *far* inside though) void g2d_polygon_get_interior_point(const zarray_t *poly, float *p) { // take the first three points, which form a triangle. Find the middle point float a[2], b[2], c[2]; zarray_get(poly, 0, a); zarray_get(poly, 1, b); zarray_get(poly, 2, c); p[0] = (a[0]+b[0]+c[0])/3; p[1] = (a[1]+b[1]+c[1])/3; } int g2d_polygon_overlaps_polygon(const zarray_t *polya, const zarray_t *polyb) { // do any of the line segments collide? If so, the answer is yes. if (g2d_polygon_intersects_polygon(polya, polyb)) return 1; // if none of the edges cross, then the polygon is either fully // contained or fully outside. float p[2]; g2d_polygon_get_interior_point(polyb, p); if (g2d_polygon_contains_point(polya, p)) return 1; g2d_polygon_get_interior_point(polya, p); if (g2d_polygon_contains_point(polyb, p)) return 1; return 0; } static int double_sort_up(const void *_a, const void *_b) { float a = *((float*) _a); float b = *((float*) _b); if (a < b) return -1; if (a == b) return 0; return 1; } // Compute the crossings of the polygon along line y, storing them in // the array x. X must be allocated to be at least as long as // zarray_size(poly). X will be sorted, ready for // rasterization. Returns the number of intersections (and elements // written to x). /* To rasterize, do something like this: float res = 0.099; for (float y = y0; y < y1; y += res) { float xs[zarray_size(poly)]; int xsz = g2d_polygon_rasterize(poly, y, xs); int xpos = 0; int inout = 0; // start off "out" for (float x = x0; x < x1; x += res) { while (x > xs[xpos] && xpos < xsz) { xpos++; inout ^= 1; } if (inout) printf("y"); else printf(" "); } printf("\n"); */ // returns the number of x intercepts int g2d_polygon_rasterize(const zarray_t *poly, float y, float *x) { int sz = zarray_size(poly); g2d_line_t line; if (1) { float p0[2] = { 0, y }; float p1[2] = { 1, y }; g2d_line_init_from_points(&line, p0, p1); } int xpos = 0; for (int i = 0; i < sz; i++) { g2d_line_segment_t seg; float *p0, *p1; zarray_get_volatile(poly, i, &p0); zarray_get_volatile(poly, (i+1)%sz, &p1); g2d_line_segment_init_from_points(&seg, p0, p1); float q[2]; if (g2d_line_segment_intersect_line(&seg, &line, q)) x[xpos++] = q[0]; } qsort(x, xpos, sizeof(float), double_sort_up); return xpos; } /* /---(1,5) (-2,4)-/ | \ | \ (1,2)--(2,2)\ \ \ \ \ (0,0)------------------(4,0) */ #if 0 #include "timeprofile.h" int main(int argc, char *argv[]) { timeprofile_t *tp = timeprofile_create(); zarray_t *polya = g2d_polygon_create_data((float[][2]) { { 0, 0}, { 4, 0}, { 2, 2}, { 1, 2}, { 1, 5}, { -2,4} }, 6); zarray_t *polyb = g2d_polygon_create_data((float[][2]) { { .1, .1}, { .5, .1}, { .1, .5 } }, 3); zarray_t *polyc = g2d_polygon_create_data((float[][2]) { { 3, 0}, { 5, 0}, { 5, 1} }, 3); zarray_t *polyd = g2d_polygon_create_data((float[][2]) { { 5, 5}, { 6, 6}, { 5, 6} }, 3); /* 5 L---K 4 |I--J 3 |H-G 2 |E-F 1 |D--C 0 A---B 01234 */ zarray_t *polyE = g2d_polygon_create_data((float[][2]) { {0,0}, {4,0}, {4, 1}, {1,1}, {1,2}, {3,2}, {3,3}, {1,3}, {1,4}, {4,4}, {4,5}, {0,5}}, 12); srand(0); timeprofile_stamp(tp, "begin"); if (1) { int niters = 100000; for (int i = 0; i < niters; i++) { float q[2]; q[0] = 10.0f * random() / RAND_MAX - 2; q[1] = 10.0f * random() / RAND_MAX - 2; g2d_polygon_contains_point(polyE, q); } timeprofile_stamp(tp, "fast"); for (int i = 0; i < niters; i++) { float q[2]; q[0] = 10.0f * random() / RAND_MAX - 2; q[1] = 10.0f * random() / RAND_MAX - 2; g2d_polygon_contains_point_ref(polyE, q); } timeprofile_stamp(tp, "slow"); for (int i = 0; i < niters; i++) { float q[2]; q[0] = 10.0f * random() / RAND_MAX - 2; q[1] = 10.0f * random() / RAND_MAX - 2; int v0 = g2d_polygon_contains_point(polyE, q); int v1 = g2d_polygon_contains_point_ref(polyE, q); assert(v0 == v1); } timeprofile_stamp(tp, "both"); timeprofile_display(tp); } if (1) { zarray_t *poly = polyE; float res = 0.399; for (float y = 5.2; y >= -.5; y -= res) { float xs[zarray_size(poly)]; int xsz = g2d_polygon_rasterize(poly, y, xs); int xpos = 0; int inout = 0; // start off "out" for (float x = -3; x < 6; x += res) { while (x > xs[xpos] && xpos < xsz) { xpos++; inout ^= 1; } if (inout) printf("y"); else printf(" "); } printf("\n"); for (float x = -3; x < 6; x += res) { float q[2] = {x, y}; if (g2d_polygon_contains_point(poly, q)) printf("X"); else printf(" "); } printf("\n"); } } /* // CW order float p[][2] = { { 0, 0}, { -2, 4}, {1, 5}, {1, 2}, {2, 2}, {4, 0} }; */ float q[2] = { 10, 10 }; printf("0==%d\n", g2d_polygon_contains_point(polya, q)); q[0] = 1; q[1] = 1; printf("1==%d\n", g2d_polygon_contains_point(polya, q)); q[0] = 3; q[1] = .5; printf("1==%d\n", g2d_polygon_contains_point(polya, q)); q[0] = 1.2; q[1] = 2.1; printf("0==%d\n", g2d_polygon_contains_point(polya, q)); printf("0==%d\n", g2d_polygon_contains_polygon(polya, polyb)); printf("0==%d\n", g2d_polygon_contains_polygon(polya, polyc)); printf("0==%d\n", g2d_polygon_contains_polygon(polya, polyd)); //////////////////////////////////////////////////////// // Test convex hull if (1) { zarray_t *hull = g2d_convex_hull(polyE); for (int k = 0; k < zarray_size(hull); k++) { float *h; zarray_get_volatile(hull, k, &h); printf("%15f, %15f\n", h[0], h[1]); } } for (int i = 0; i < 100000; i++) { zarray_t *points = zarray_create(sizeof(float[2])); for (int j = 0; j < 100; j++) { float q[2]; q[0] = 10.0f * random() / RAND_MAX - 2; q[1] = 10.0f * random() / RAND_MAX - 2; zarray_add(points, q); } zarray_t *hull = g2d_convex_hull(points); for (int j = 0; j < zarray_size(points); j++) { float *q; zarray_get_volatile(points, j, &q); int on_edge; float p[2]; g2d_polygon_closest_boundary_point(hull, q, p); if (g2d_distance(q, p) < .00001) on_edge = 1; assert(on_edge || g2d_polygon_contains_point(hull, q)); } zarray_destroy(hull); zarray_destroy(points); } } #endif //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "image_types.h" //////////////////////////////////////////////////////////////////////////////////////////////////// // to support conversions between different types, we define all image // types at once. Type-specific implementations can then #include this // file, assured that the basic types of each image are known. typedef struct image_u8 image_u8_t; struct image_u8 { int32_t width; int32_t height; int32_t stride; uint8_t *buf; }; typedef struct image_u8x3 image_u8x3_t; struct image_u8x3 { const int32_t width; const int32_t height; const int32_t stride; // bytes per line uint8_t *buf; }; typedef struct image_u8x4 image_u8x4_t; struct image_u8x4 { const int32_t width; const int32_t height; const int32_t stride; // bytes per line uint8_t *buf; }; typedef struct image_f32 image_f32_t; struct image_f32 { const int32_t width; const int32_t height; const int32_t stride; // floats per line float *buf; // indexed as buf[y*stride + x] }; typedef struct image_u32 image_u32_t; struct image_u32 { const int32_t width; const int32_t height; const int32_t stride; // int32_ts per line uint32_t *buf; // indexed as buf[y*stride + x] }; //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "apriltag_math.h" //////////////////////////////////////////////////////////////////////////////////////////////////// // Computes the cholesky factorization of A, putting the lower // triangular matrix into R. static inline void mat33_chol(const float *A, float *R) { // A[0] = R[0]*R[0] R[0] = sqrt(A[0]); // A[1] = R[0]*R[3]; R[3] = A[1] / R[0]; // A[2] = R[0]*R[6]; R[6] = A[2] / R[0]; // A[4] = R[3]*R[3] + R[4]*R[4] R[4] = sqrt(A[4] - R[3]*R[3]); // A[5] = R[3]*R[6] + R[4]*R[7] R[7] = (A[5] - R[3]*R[6]) / R[4]; // A[8] = R[6]*R[6] + R[7]*R[7] + R[8]*R[8] R[8] = sqrt(A[8] - R[6]*R[6] - R[7]*R[7]); R[1] = 0; R[2] = 0; R[5] = 0; } static inline void mat33_lower_tri_inv(const float *A, float *R) { // A[0]*R[0] = 1 R[0] = 1 / A[0]; // A[3]*R[0] + A[4]*R[3] = 0 R[3] = -A[3]*R[0] / A[4]; // A[4]*R[4] = 1 R[4] = 1 / A[4]; // A[6]*R[0] + A[7]*R[3] + A[8]*R[6] = 0 R[6] = (-A[6]*R[0] - A[7]*R[3]) / A[8]; // A[7]*R[4] + A[8]*R[7] = 0 R[7] = -A[7]*R[4] / A[8]; // A[8]*R[8] = 1 R[8] = 1 / A[8]; } static inline void mat33_sym_solve(const float *A, const float *B, float *R) { float L[9]; mat33_chol(A, L); float M[9]; mat33_lower_tri_inv(L, M); float tmp[3]; tmp[0] = M[0]*B[0]; tmp[1] = M[3]*B[0] + M[4]*B[1]; tmp[2] = M[6]*B[0] + M[7]*B[1] + M[8]*B[2]; R[0] = M[0]*tmp[0] + M[3]*tmp[1] + M[6]*tmp[2]; R[1] = M[4]*tmp[1] + M[7]*tmp[2]; R[2] = M[8]*tmp[2]; } //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "apriltag.h" //////////////////////////////////////////////////////////////////////////////////////////////////// struct quad { float p[4][2]; // corners // H: tag coordinates ([-1,1] at the black corners) to pixels // Hinv: pixels to tag matd_t *H, *Hinv; }; // Represents a tag family. Every tag belongs to a tag family. Tag // families are generated by the Java tool // april.tag.TagFamilyGenerator and can be converted to C using // april.tag.TagToC. typedef struct apriltag_family apriltag_family_t; struct apriltag_family { // How many codes are there in this tag family? uint32_t ncodes; // how wide (in bit-sizes) is the black border? (usually 1) uint32_t black_border; // how many bits tall and wide is it? (e.g. 36bit tag ==> 6) uint32_t d; // minimum hamming distance between any two codes. (e.g. 36h11 => 11) uint32_t h; // The codes in the family. uint64_t codes[]; }; struct apriltag_quad_thresh_params { // reject quads containing too few pixels int min_cluster_pixels; // how many corner candidates to consider when segmenting a group // of pixels into a quad. int max_nmaxima; // Reject quads where pairs of edges have angles that are close to // straight or close to 180 degrees. Zero means that no quads are // rejected. (In radians). float critical_rad; // When fitting lines to the contours, what is the maximum mean // squared error allowed? This is useful in rejecting contours // that are far from being quad shaped; rejecting these quads "early" // saves expensive decoding processing. float max_line_fit_mse; // When we build our model of black & white pixels, we add an // extra check that the white model must be (overall) brighter // than the black model. How much brighter? (in pixel values, // [0,255]). . int min_white_black_diff; // should the thresholded image be deglitched? Only useful for // very noisy images int deglitch; }; // Represents a detector object. Upon creating a detector, all fields // are set to reasonable values, but can be overridden by accessing // these fields. typedef struct apriltag_detector apriltag_detector_t; struct apriltag_detector { /////////////////////////////////////////////////////////////// // User-configurable parameters. // When non-zero, the edges of the each quad are adjusted to "snap // to" strong gradients nearby. This is useful when decimation is // employed, as it can increase the quality of the initial quad // estimate substantially. Generally recommended to be on (1). // // Very computationally inexpensive. Option is ignored if // quad_decimate = 1. int refine_edges; // when non-zero, detections are refined in a way intended to // increase the number of detected tags. Especially effective for // very small tags near the resolution threshold (e.g. 10px on a // side). int refine_decode; // when non-zero, detections are refined in a way intended to // increase the accuracy of the extracted pose. This is done by // maximizing the contrast around the black and white border of // the tag. This generally increases the number of successfully // detected tags, though not as effectively (or quickly) as // refine_decode. // // This option must be enabled in order for "goodness" to be // computed. int refine_pose; struct apriltag_quad_thresh_params qtp; /////////////////////////////////////////////////////////////// // Statistics relating to last processed frame uint32_t nedges; uint32_t nsegments; uint32_t nquads; /////////////////////////////////////////////////////////////// // Internal variables below // Not freed on apriltag_destroy; a tag family can be shared // between multiple users. The user should ultimately destroy the // tag family passed into the constructor. zarray_t *tag_families; }; // Represents the detection of a tag. These are returned to the user // and must be individually destroyed by the user. typedef struct apriltag_detection apriltag_detection_t; struct apriltag_detection { // a pointer for convenience. not freed by apriltag_detection_destroy. apriltag_family_t *family; // The decoded ID of the tag int id; // How many error bits were corrected? Note: accepting large numbers of // corrected errors leads to greatly increased false positive rates. // NOTE: As of this implementation, the detector cannot detect tags with // a hamming distance greater than 2. int hamming; // A measure of the quality of tag localization: measures the // average contrast of the pixels around the border of the // tag. refine_pose must be enabled, or this field will be zero. float goodness; // A measure of the quality of the binary decoding process: the // average difference between the intensity of a data bit versus // the decision threshold. Higher numbers roughly indicate better // decodes. This is a reasonable measure of detection accuracy // only for very small tags-- not effective for larger tags (where // we could have sampled anywhere within a bit cell and still // gotten a good detection.) float decision_margin; // The 3x3 homography matrix describing the projection from an // "ideal" tag (with corners at (-1,-1), (1,-1), (1,1), and (-1, // 1)) to pixels in the image. This matrix will be freed by // apriltag_detection_destroy. matd_t *H; // The center of the detection in image pixel coordinates. float c[2]; // The corners of the tag in image pixel coordinates. These always // wrap counter-clock wise around the tag. float p[4][2]; }; // don't forget to add a family! apriltag_detector_t *apriltag_detector_create(); // add a family to the apriltag detector. caller still "owns" the family. // a single instance should only be provided to one apriltag detector instance. void apriltag_detector_add_family_bits(apriltag_detector_t *td, apriltag_family_t *fam, int bits_corrected); // Tunable, but really, 2 is a good choice. Values of >=3 // consume prohibitively large amounts of memory, and otherwise // you want the largest value possible. static inline void apriltag_detector_add_family(apriltag_detector_t *td, apriltag_family_t *fam) { apriltag_detector_add_family_bits(td, fam, 2); } // does not deallocate the family. void apriltag_detector_remove_family(apriltag_detector_t *td, apriltag_family_t *fam); // unregister all families, but does not deallocate the underlying tag family objects. void apriltag_detector_clear_families(apriltag_detector_t *td); // Destroy the april tag detector (but not the underlying // apriltag_family_t used to initialize it.) void apriltag_detector_destroy(apriltag_detector_t *td); // Detect tags from an image and return an array of // apriltag_detection_t*. You can use apriltag_detections_destroy to // free the array and the detections it contains, or call // _detection_destroy and zarray_destroy yourself. zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig); // Call this method on each of the tags returned by apriltag_detector_detect void apriltag_detection_destroy(apriltag_detection_t *det); // destroys the array AND the detections within it. void apriltag_detections_destroy(zarray_t *detections); //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "tag16h5" //////////////////////////////////////////////////////////////////////////////////////////////////// #ifdef IMLIB_ENABLE_APRILTAGS_TAG16H5 const apriltag_family_t tag16h5 = { .ncodes = 30, .black_border = 1, .d = 4, .h = 5, .codes = { 0x000000000000231bUL, 0x0000000000002ea5UL, 0x000000000000346aUL, 0x00000000000045b9UL, 0x00000000000079a6UL, 0x0000000000007f6bUL, 0x000000000000b358UL, 0x000000000000e745UL, 0x000000000000fe59UL, 0x000000000000156dUL, 0x000000000000380bUL, 0x000000000000f0abUL, 0x0000000000000d84UL, 0x0000000000004736UL, 0x0000000000008c72UL, 0x000000000000af10UL, 0x000000000000093cUL, 0x00000000000093b4UL, 0x000000000000a503UL, 0x000000000000468fUL, 0x000000000000e137UL, 0x0000000000005795UL, 0x000000000000df42UL, 0x0000000000001c1dUL, 0x000000000000e9dcUL, 0x00000000000073adUL, 0x000000000000ad5fUL, 0x000000000000d530UL, 0x00000000000007caUL, 0x000000000000af2eUL } }; #endif // IMLIB_ENABLE_APRILTAGS_TAG16H5 //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "tag25h7" //////////////////////////////////////////////////////////////////////////////////////////////////// #ifdef IMLIB_ENABLE_APRILTAGS_TAG25H7 const apriltag_family_t tag25h7 = { .ncodes = 242, .black_border = 1, .d = 5, .h = 7, .codes = { 0x00000000004b770dUL, 0x00000000011693e6UL, 0x0000000001a599abUL, 0x0000000000c3a535UL, 0x000000000152aafaUL, 0x0000000000accd98UL, 0x0000000001cad922UL, 0x00000000002c2fadUL, 0x0000000000bb3572UL, 0x00000000014a3b37UL, 0x000000000186524bUL, 0x0000000000c99d4cUL, 0x000000000023bfeaUL, 0x000000000141cb74UL, 0x0000000001d0d139UL, 0x0000000001670aebUL, 0x0000000000851675UL, 0x000000000150334eUL, 0x00000000006e3ed8UL, 0x0000000000fd449dUL, 0x0000000000aa55ecUL, 0x0000000001c86176UL, 0x00000000015e9b28UL, 0x00000000007ca6b2UL, 0x000000000147c38bUL, 0x0000000001d6c950UL, 0x00000000008b0e8cUL, 0x00000000011a1451UL, 0x0000000001562b65UL, 0x00000000013f53c8UL, 0x0000000000d58d7aUL, 0x0000000000829ec9UL, 0x0000000000faccf1UL, 0x000000000136e405UL, 0x00000000007a2f06UL, 0x00000000010934cbUL, 0x00000000016a8b56UL, 0x0000000001a6a26aUL, 0x0000000000f85545UL, 0x000000000195c2e4UL, 0x000000000024c8a9UL, 0x00000000012bfc96UL, 0x00000000016813aaUL, 0x0000000001a42abeUL, 0x0000000001573424UL, 0x0000000001044573UL, 0x0000000000b156c2UL, 0x00000000005e6811UL, 0x0000000001659bfeUL, 0x0000000001d55a63UL, 0x00000000005bf065UL, 0x0000000000e28667UL, 0x0000000001e9ba54UL, 0x00000000017d7c5aUL, 0x0000000001f5aa82UL, 0x0000000001a2bbd1UL, 0x00000000001ae9f9UL, 0x0000000001259e51UL, 0x000000000134062bUL, 0x0000000000e1177aUL, 0x0000000000ed07a8UL, 0x000000000162be24UL, 0x000000000059128bUL, 0x0000000001663e8fUL, 0x00000000001a83cbUL, 0x000000000045bb59UL, 0x000000000189065aUL, 0x00000000004bb370UL, 0x00000000016fb711UL, 0x000000000122c077UL, 0x0000000000eca17aUL, 0x0000000000dbc1f4UL, 0x000000000088d343UL, 0x000000000058ac5dUL, 0x0000000000ba02e8UL, 0x00000000001a1d9dUL, 0x0000000001c72eecUL, 0x0000000000924bc5UL, 0x0000000000dccab3UL, 0x0000000000886d15UL, 0x000000000178c965UL, 0x00000000005bc69aUL, 0x0000000001716261UL, 0x000000000174e2ccUL, 0x0000000001ed10f4UL, 0x0000000000156aa8UL, 0x00000000003e2a8aUL, 0x00000000002752edUL, 0x000000000153c651UL, 0x0000000001741670UL, 0x0000000000765b05UL, 0x000000000119c0bbUL, 0x000000000172a783UL, 0x00000000004faca1UL, 0x0000000000f31257UL, 0x00000000012441fcUL, 0x00000000000d3748UL, 0x0000000000c21f15UL, 0x0000000000ac5037UL, 0x000000000180e592UL, 0x00000000007d3210UL, 0x0000000000a27187UL, 0x00000000002beeafUL, 0x000000000026ff57UL, 0x0000000000690e82UL, 0x000000000077765cUL, 0x0000000001a9e1d7UL, 0x000000000140be1aUL, 0x0000000001aa1e3aUL, 0x0000000001944f5cUL, 0x00000000019b5032UL, 0x0000000000169897UL, 0x0000000001068eb9UL, 0x0000000000f30dbcUL, 0x000000000106a151UL, 0x0000000001d53e95UL, 0x0000000001348ceeUL, 0x0000000000cf4fcaUL, 0x0000000001728bb5UL, 0x0000000000dc1eecUL, 0x000000000069e8dbUL, 0x00000000016e1523UL, 0x000000000105fa25UL, 0x00000000018abb0cUL, 0x0000000000c4275dUL, 0x00000000006d8e76UL, 0x0000000000e8d6dbUL, 0x0000000000e16fd7UL, 0x0000000001ac2682UL, 0x000000000077435bUL, 0x0000000000a359ddUL, 0x00000000003a9c4eUL, 0x000000000123919aUL, 0x0000000001e25817UL, 0x000000000002a836UL, 0x00000000001545a4UL, 0x0000000001209c8dUL, 0x0000000000bb5f69UL, 0x0000000001dc1f02UL, 0x00000000005d5f7eUL, 0x00000000012d0581UL, 0x00000000013786c2UL, 0x0000000000e15409UL, 0x0000000001aa3599UL, 0x000000000139aad8UL, 0x0000000000b09d2aUL, 0x000000000054488fUL, 0x00000000013c351cUL, 0x0000000000976079UL, 0x0000000000b25b12UL, 0x0000000001addb34UL, 0x0000000001cb23aeUL, 0x0000000001175738UL, 0x0000000001303bb8UL, 0x0000000000d47716UL, 0x000000000188ceeaUL, 0x0000000000baf967UL, 0x0000000001226d39UL, 0x000000000135e99bUL, 0x000000000034adc5UL, 0x00000000002e384dUL, 0x000000000090d3faUL, 0x0000000000232713UL, 0x00000000017d49b1UL, 0x0000000000aa84d6UL, 0x0000000000c2ddf8UL, 0x0000000001665646UL, 0x00000000004f345fUL, 0x00000000002276b1UL, 0x0000000001255dd7UL, 0x00000000016f4cccUL, 0x00000000004aaffcUL, 0x0000000000c46da6UL, 0x000000000085c7b3UL, 0x0000000001311fcbUL, 0x00000000009c6c4fUL, 0x000000000187d947UL, 0x00000000008578e4UL, 0x0000000000e2bf0bUL, 0x0000000000a01b4cUL, 0x0000000000a1493bUL, 0x00000000007ad766UL, 0x0000000000ccfe82UL, 0x0000000001981b5bUL, 0x0000000001cacc85UL, 0x0000000000562cdbUL, 0x00000000015b0e78UL, 0x00000000008f66c5UL, 0x00000000003332bfUL, 0x00000000012ce754UL, 0x0000000000096a76UL, 0x0000000001d5e3baUL, 0x000000000027ea41UL, 0x00000000014412dfUL, 0x000000000067b9b4UL, 0x0000000000daa51aUL, 0x00000000001dcb17UL, 0x00000000004d4afdUL, 0x00000000006335d5UL, 0x0000000000ee2334UL, 0x00000000017d4e55UL, 0x0000000001b8b0f0UL, 0x00000000014999e3UL, 0x0000000001513dfaUL, 0x0000000000765cf2UL, 0x000000000056af90UL, 0x00000000012e16acUL, 0x0000000001d3d86cUL, 0x0000000000ff279bUL, 0x00000000018822ddUL, 0x000000000099d478UL, 0x00000000008dc0d2UL, 0x000000000034b666UL, 0x0000000000cf9526UL, 0x000000000186443dUL, 0x00000000007a8e29UL, 0x00000000019c6aa5UL, 0x0000000001f2a27dUL, 0x00000000012b2136UL, 0x0000000000d0cd0dUL, 0x00000000012cb320UL, 0x00000000017ddb0bUL, 0x000000000005353bUL, 0x00000000015b2cafUL, 0x0000000001e5a507UL, 0x000000000120f1e5UL, 0x000000000114605aUL, 0x00000000014efe4cUL, 0x0000000000568134UL, 0x00000000011b9f92UL, 0x000000000174d2a7UL, 0x0000000000692b1dUL, 0x000000000039e4feUL, 0x0000000000aaff3dUL, 0x000000000096224cUL, 0x00000000013c9f77UL, 0x000000000110ee8fUL, 0x0000000000f17beaUL, 0x000000000099fb5dUL, 0x0000000000337141UL, 0x000000000002b54dUL, 0x0000000001233a70UL } }; #endif // IMLIB_ENABLE_APRILTAGS_TAG25H7 //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "tag25h9" //////////////////////////////////////////////////////////////////////////////////////////////////// #ifdef IMLIB_ENABLE_APRILTAGS_TAG25H9 const apriltag_family_t tag25h9 = { .ncodes = 35, .black_border = 1, .d = 5, .h = 9, .codes = { 0x000000000155cbf1UL, 0x0000000001e4d1b6UL, 0x00000000017b0b68UL, 0x0000000001eac9cdUL, 0x00000000012e14ceUL, 0x00000000003548bbUL, 0x00000000007757e6UL, 0x0000000001065dabUL, 0x0000000001baa2e7UL, 0x0000000000dea688UL, 0x000000000081d927UL, 0x000000000051b241UL, 0x0000000000dbc8aeUL, 0x0000000001e50e19UL, 0x00000000015819d2UL, 0x00000000016d8282UL, 0x000000000163e035UL, 0x00000000009d9b81UL, 0x000000000173eec4UL, 0x0000000000ae3a09UL, 0x00000000005f7c51UL, 0x0000000001a137fcUL, 0x0000000000dc9562UL, 0x0000000001802e45UL, 0x0000000001c3542cUL, 0x0000000000870fa4UL, 0x0000000000914709UL, 0x00000000016684f0UL, 0x0000000000c8f2a5UL, 0x0000000000833ebbUL, 0x000000000059717fUL, 0x00000000013cd050UL, 0x0000000000fa0ad1UL, 0x0000000001b763b0UL, 0x0000000000b991ceUL } }; #endif // IMLIB_ENABLE_APRILTAGS_TAG25H9 //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "tag36h10" //////////////////////////////////////////////////////////////////////////////////////////////////// #ifdef IMLIB_ENABLE_APRILTAGS_TAG36H10 const apriltag_family_t tag36h10 = { .ncodes = 2320, .black_border = 1, .d = 6, .h = 10, .codes = { 0x00000001ca92a687UL, 0x000000020521ac4cUL, 0x000000027a3fb7d6UL, 0x00000002b4cebd9bUL, 0x00000003647bceeaUL, 0x000000039f0ad4afUL, 0x00000003d999da74UL, 0x000000044eb7e5feUL, 0x0000000538f3fd12UL, 0x00000005738302d7UL, 0x000000065dbf19ebUL, 0x000000070d6c2b3aUL, 0x00000007f7a8424eUL, 0x0000000832374813UL, 0x000000086cc64dd8UL, 0x00000008a755539dUL, 0x00000009570264ecUL, 0x0000000991916ab1UL, 0x0000000a06af763bUL, 0x0000000ab65c878aUL, 0x0000000b2b7a9314UL, 0x0000000b660998d9UL, 0x0000000bdb27a463UL, 0x0000000cc563bb77UL, 0x0000000e24bdde15UL, 0x0000000ed46aef64UL, 0x0000000f4988faeeUL, 0x000000006e5417c7UL, 0x0000000158902edbUL, 0x00000001cdae3a65UL, 0x0000000242cc45efUL, 0x000000027d5b4bb4UL, 0x00000002b7ea5179UL, 0x000000032d085d03UL, 0x00000003679762c8UL, 0x00000003a226688dUL, 0x00000003dcb56e52UL, 0x000000048c627fa1UL, 0x00000005769e96b5UL, 0x00000006264ba804UL, 0x0000000660daadc9UL, 0x00000006d5f8b953UL, 0x000000074b16c4ddUL, 0x00000007fac3d62cUL, 0x000000091f8ef305UL, 0x000000095a1df8caUL, 0x0000000994acfe8fUL, 0x0000000a09cb0a19UL, 0x0000000a445a0fdeUL, 0x0000000a7ee915a3UL, 0x0000000ab9781b68UL, 0x0000000af407212dUL, 0x0000000b69252cb7UL, 0x0000000c8df04990UL, 0x0000000d3d9d5adfUL, 0x0000000d782c60a4UL, 0x0000000f12158907UL, 0x00000001d0c9ce43UL, 0x000000020b58d408UL, 0x00000002f594eb1cUL, 0x00000003a541fc6bUL, 0x0000000454ef0dbaUL, 0x000000053f2b24ceUL, 0x0000000629673be2UL, 0x000000074e3258bbUL, 0x00000008ad8c7b59UL, 0x00000009d2579832UL, 0x0000000a8204a981UL, 0x0000000af722b50bUL, 0x0000000b6c40c095UL, 0x0000000ba6cfc65aUL, 0x0000000f15311ce5UL, 0x00000000748b3f83UL, 0x00000000af1a4548UL, 0x00000000e9a94b0dUL, 0x00000002be217935UL, 0x00000003e2ec960eUL, 0x00000004cd28ad22UL, 0x0000000507b7b2e7UL, 0x000000054246b8acUL, 0x000000057cd5be71UL, 0x00000006a1a0db4aUL, 0x00000006dc2fe10fUL, 0x0000000876190972UL, 0x000000099ae4264bUL, 0x0000000abfaf4324UL, 0x0000000c9427714cUL, 0x0000000d09457cd6UL, 0x0000000d43d4829bUL, 0x0000000ea32ea539UL, 0x0000000f52dbb688UL, 0x0000000161e2ea75UL, 0x0000000286ae074eUL, 0x000000066a2d6963UL, 0x00000008b3c3a315UL, 0x00000008ee52a8daUL, 0x0000000a131dc5b3UL, 0x0000000e6bbb3352UL, 0x0000000f55f74a66UL, 0x0000000005a45bb5UL, 0x000000007ac2673fUL, 0x00000001da1c89ddUL, 0x0000000289c99b2cUL, 0x00000003ae94b805UL, 0x000000050deedaa3UL, 0x00000005830ce62dUL, 0x00000005bd9bebf2UL, 0x0000000632b9f77cUL, 0x00000006e26708cbUL, 0x0000000841c12b69UL, 0x000000092bfd427dUL, 0x00000009668c4842UL, 0x00000009dbaa53ccUL, 0x0000000b007570a5UL, 0x0000000b3b04766aUL, 0x0000000c25408d7eUL, 0x0000000ea965ccf5UL, 0x0000000f93a1e409UL, 0x00000000434ef558UL, 0x00000001681a1231UL, 0x00000001dd381dbbUL, 0x0000000302033a94UL, 0x000000075aa0a833UL, 0x000000092f18d65bUL, 0x00000009a436e1e5UL, 0x0000000a1954ed6fUL, 0x0000000b78af100dUL, 0x0000000bb33e15d2UL, 0x0000000c62eb2721UL, 0x00000000466a8936UL, 0x00000000f6179a85UL, 0x000000016b35a60fUL, 0x0000000589440de9UL, 0x00000006738024fdUL, 0x0000000847f85325UL, 0x00000009e1e17b88UL, 0x0000000acc1d929cUL, 0x0000000b06ac9861UL, 0x0000000d5042d213UL, 0x0000000fd468118aUL, 0x00000000f9332e63UL, 0x0000000342c96815UL, 0x000000037d586ddaUL, 0x0000000551d09c02UL, 0x00000005c6eea78cUL, 0x00000006017dad51UL, 0x00000009354ffe17UL, 0x00000009aa6e09a1UL, 0x0000000a94aa20b5UL, 0x0000000acf39267aUL, 0x0000000bb9753d8eUL, 0x0000000bf4044353UL, 0x000000008730b6b7UL, 0x00000001716ccdcbUL, 0x000000022119df1aUL, 0x00000003f5920d42UL, 0x000000058f7b35a5UL, 0x00000006b446527eUL, 0x0000000972fa97baUL, 0x00000009e818a344UL, 0x0000000a5d36aeceUL, 0x0000000d1beaf40aUL, 0x0000000dcb980559UL, 0x0000000f65812dbcUL, 0x0000000139f95be4UL, 0x00000006b761e65cUL, 0x00000006f1f0ec21UL, 0x0000000a605242acUL, 0x0000000e43d1a4c1UL, 0x000000029c6f1260UL, 0x0000000386ab2974UL, 0x00000004e6054c12UL, 0x00000008c984ae27UL, 0x000000097931bf76UL, 0x00000009ee4fcb00UL, 0x0000000d22221bc6UL, 0x0000000e46ed389fUL, 0x0000000ebc0b4429UL, 0x00000006f82813ddUL, 0x0000000732b719a2UL, 0x00000009072f47caUL, 0x0000000941be4d8fUL, 0x000000097c4d5354UL, 0x00000009f16b5edeUL, 0x0000000b8b548741UL, 0x0000000d253dafa4UL, 0x0000000d5fccb569UL, 0x0000000d9a5bbb2eUL, 0x0000000e4a08cc7dUL, 0x00000003c77156f5UL, 0x00000007aaf0b90aUL, 0x0000000b53e1155aUL, 0x0000000b8e701b1fUL, 0x000000005c2b9448UL, 0x000000014667ab5cUL, 0x000000047a39fc22UL, 0x00000004ef5807acUL, 0x000000064eb22a4aUL, 0x0000000982847b10UL, 0x0000000aa74f97e9UL, 0x0000000ae1de9daeUL, 0x0000000b56fca938UL, 0x0000000f750b1112UL, 0x00000001bea14ac4UL, 0x00000001f9305089UL, 0x0000000233bf564eUL, 0x000000031dfb6d62UL, 0x00000003931978ecUL, 0x000000052d02a14fUL, 0x00000005a220acd9UL, 0x0000000b1f893751UL, 0x0000000fb2b5aab5UL, 0x000000040b531854UL, 0x00000005a53c40b7UL, 0x00000008d90e917dUL, 0x00000009139d9742UL, 0x000000094e2c9d07UL, 0x0000000d6c3b04e1UL, 0x0000000e910621baUL, 0x0000000f40b33309UL, 0x00000001152b6131UL, 0x000000032432951eUL, 0x00000003d3dfa66dUL, 0x000000065804e5e4UL, 0x0000000ab0a25383UL, 0x0000000b604f64d2UL, 0x0000000de474a449UL, 0x000000011846f50fUL, 0x00000006d03e854cUL, 0x00000007455c90d6UL, 0x0000000ab3bde761UL, 0x0000000dad013262UL, 0x0000000e973d4976UL, 0x00000002b54bb150UL, 0x00000009577f58a1UL, 0x00000009920e5e66UL, 0x0000000b66868c8eUL, 0x0000000f4a05eea3UL, 0x00000003dd326207UL, 0x00000005b1aa902fUL, 0x000000099529f244UL, 0x0000000b2f131aa7UL, 0x0000000d038b48cfUL, 0x0000000d3e1a4e94UL, 0x000000024664cd82UL, 0x000000036b2fea5bUL, 0x000000095db6805dUL, 0x0000000a0d6391acUL, 0x0000000abd10a2fbUL, 0x000000015f444a4cUL, 0x00000002be9e6ceaUL, 0x000000057d52b226UL, 0x00000005f270bdb0UL, 0x0000000b6fd94828UL, 0x0000000879b19105UL, 0x0000000d476d0a2eUL, 0x0000000e6c382707UL, 0x0000000dbfa6a996UL, 0x00000001689705e6UL, 0x00000003b22d3f98UL, 0x0000000636527f0fUL, 0x00000007d03ba772UL, 0x0000000ee78d5a4dUL, 0x0000000bf165a32aUL, 0x0000000c2bf4a8efUL, 0x0000000517be89f2UL, 0x000000067718ac90UL, 0x00000006b1a7b255UL, 0x0000000726c5bddfUL, 0x0000000bb9f23143UL, 0x00000001375abbbbUL, 0x0000000296b4de59UL, 0x00000008893b745bUL, 0x0000000a9842a848UL, 0x0000000b827ebf5cUL, 0x00000003840c894bUL, 0x00000006b7deda11UL, 0x0000000bc02958ffUL, 0x000000055ba04b51UL, 0x000000076aa77f3eUL, 0x00000009b43db8f0UL, 0x00000009eeccbeb5UL, 0x0000000a295bc47aUL, 0x0000000b4e26e153UL, 0x0000000e476a2c54UL, 0x00000006be1601cdUL, 0x00000006f8a50792UL, 0x000000097cca4709UL, 0x0000000bc66080bbUL, 0x00000001093a056eUL, 0x00000006fbc09b70UL, 0x0000000b8eed0ed4UL, 0x0000000cee473172UL, 0x000000023120b625UL, 0x00000005da111275UL, 0x0000000cf162c550UL, 0x0000000ec8f68756UL, 0x0000000b5db0c4a9UL, 0x00000002b2ad1127UL, 0x0000000536d2509eUL, 0x00000009c9fec402UL, 0x0000000c1394fdb4UL, 0x000000006c326b53UL, 0x00000005e99af5cbUL, 0x0000000af1e574b9UL, 0x0000000e6046cb44UL, 0x0000000661d49533UL, 0x00000008e5f9d4aaUL, 0x0000000db3b54dd3UL, 0x0000000e63625f22UL, 0x0000000e9df164e7UL, 0x0000000455e8f524UL, 0x00000005b54317c2UL, 0x0000000be258b389UL, 0x000000054340a016UL, 0x00000005b85eaba0UL, 0x00000001284dcc1aUL, 0x000000024d18e8f3UL, 0x00000004d13e286aUL, 0x00000008b4bd8a7fUL, 0x0000000215a5770cUL, 0x000000046572d87aUL, 0x0000000c2c719ca4UL, 0x00000004ddac77e2UL, 0x0000000d19c94796UL, 0x00000002d1c0d7d3UL, 0x00000009ae8384e9UL, 0x00000009e9128aaeUL, 0x0000000ca7c6cfeaUL, 0x0000000016282675UL, 0x0000000ad985c97eUL, 0x00000004af8bc195UL, 0x00000009f580da26UL, 0x0000000a6a9ee5b0UL, 0x0000000bc9f9084eUL, 0x0000000d63e230b1UL, 0x0000000c4232a7b6UL, 0x0000000d66fdc48fUL, 0x0000000ec657e72dUL, 0x0000000a364707a7UL, 0x0000000f79208c5aUL, 0x0000000de88a1f91UL, 0x0000000574f9ddf6UL, 0x000000065f35f50aUL, 0x000000069ce08eadUL, 0x0000000490f4ee9eUL, 0x0000000c9282b88dUL, 0x0000000752c4c7b8UL, 0x0000000b364429cdUL, 0x00000008b53a7e34UL, 0x0000000be90ccefaUL, 0x0000000b0507dfa2UL, 0x000000000d525e90UL, 0x00000005c549eecdUL, 0x0000000e3bf5c446UL, 0x0000000936c6d936UL, 0x00000009747172d9UL, 0x0000000ca843c39fUL, 0x0000000d57f0d4eeUL, 0x00000002d5595f66UL, 0x0000000bfbb2462eUL, 0x0000000266727b98UL, 0x00000007ac679429UL, 0x000000026fc53732UL, 0x0000000656602d25UL, 0x00000002eb1a6a78UL, 0x00000004850392dbUL, 0x0000000e5b098af2UL, 0x0000000ab534c280UL, 0x00000009ce143f4aUL, 0x0000000f4b7cc9c2UL, 0x0000000035b8e0d6UL, 0x0000000871d5b08aUL, 0x00000005b958930aUL, 0x00000000b429a7faUL, 0x000000054d8d431aUL, 0x00000007d1b28291UL, 0x0000000a1e645021UL, 0x0000000b80da069dUL, 0x0000000eef3b5d28UL, 0x0000000263d3db6fUL, 0x000000009592d503UL, 0x00000004b9d86499UL, 0x00000006c8df9886UL, 0x0000000a3740ef11UL, 0x0000000c4963b6dcUL, 0x000000006da94672UL, 0x000000053b64bf9bUL, 0x0000000b2deb559dUL, 0x0000000f116ab7b2UL, 0x00000008ace1aa04UL, 0x00000008ea8c43a7UL, 0x00000006a4119dd3UL, 0x000000099d54e8d4UL, 0x0000000c969833d5UL, 0x0000000f554c7911UL, 0x00000003ade9e6b0UL, 0x00000006e1bc3776UL, 0x00000007916948c5UL, 0x0000000dbe7ee48cUL, 0x000000079484dca3UL, 0x0000000f992e3a70UL, 0x0000000884f81b73UL, 0x0000000c68777d88UL, 0x0000000603ee6fdaUL, 0x0000000728b98cb3UL, 0x0000000b12701684UL, 0x0000000d5f21e414UL, 0x0000000058652f15UL, 0x00000002dc8a6e8cUL, 0x00000004767396efUL, 0x0000000b8dc549caUL, 0x0000000f36b5a61aUL, 0x00000000d09ece7dUL, 0x0000000dda77175aUL, 0x000000005e9c56d1UL, 0x000000073e7a97c5UL, 0x0000000b21f9f9daUL, 0x0000000de3c9d2f4UL, 0x000000069504ae32UL, 0x000000077f40c546UL, 0x0000000ed1217de6UL, 0x00000003a1f88aedUL, 0x0000000e623a9a18UL, 0x00000000aeec67a8UL, 0x0000000bea83aa19UL, 0x000000092eeaf8bbUL, 0x0000000a5d08d12eUL, 0x0000000819a9bf38UL, 0x0000000473d4f6c6UL, 0x0000000b192431f5UL, 0x0000000a6c92b484UL, 0x00000007046885b5UL, 0x0000000b9ab08cf7UL, 0x0000000782d94cd9UL, 0x0000000f158032faUL, 0x0000000077f5e976UL, 0x000000012dda2281UL, 0x0000000e72417123UL, 0x00000003056de487UL, 0x0000000e3de9931aUL, 0x0000000eb3079ea4UL, 0x0000000e4420bad6UL, 0x0000000439c2e4b6UL, 0x000000047da4a615UL, 0x00000000d7cfdda3UL, 0x000000056afc5107UL, 0x0000000e978c5f8bUL, 0x00000005aede1266UL, 0x0000000af1b79719UL, 0x0000000f8b1b3239UL, 0x000000075e8845dbUL, 0x0000000bf1b4b93fUL, 0x0000000fd5341b54UL, 0x0000000a2373b2d3UL, 0x00000005967e672bUL, 0x0000000a2cc66e6dUL, 0x0000000b17028581UL, 0x0000000b54ad1f24UL, 0x0000000e91d22b84UL, 0x0000000de85c41f1UL, 0x000000053d588e6fUL, 0x0000000e9e407afcUL, 0x0000000fc6272bb3UL, 0x0000000a8ca0629aUL, 0x0000000b86665d04UL, 0x000000005a58fde9UL, 0x00000001855b427eUL, 0x0000000aabb42946UL, 0x0000000e204ca78dUL, 0x000000032897267bUL, 0x00000000a78d7ae2UL, 0x000000096536a598UL, 0x0000000bf2aea0a9UL, 0x00000000c9bcd56cUL, 0x000000081eb921eaUL, 0x00000002732fe125UL, 0x00000002eb69808dUL, 0x000000061f3bd153UL, 0x00000008ddf0168fUL, 0x0000000921d1d7eeUL, 0x00000002bd48ca40UL, 0x000000083ab154b8UL, 0x00000005f436aee4UL, 0x000000093dca0abcUL, 0x000000026d75ad1eUL, 0x0000000872a1ba54UL, 0x0000000373a9f700UL, 0x000000050d931f63UL, 0x000000012d2f512cUL, 0x0000000c6efdbb59UL, 0x000000088e99ed22UL, 0x0000000903b7f8acUL, 0x0000000a9da1210fUL, 0x0000000a2eba3d41UL, 0x0000000fe9cd615cUL, 0x0000000fb8911731UL, 0x00000001cab3defcUL, 0x0000000e6289b02dUL, 0x000000066d6a35b6UL, 0x0000000b0096a91aUL, 0x0000000c9afcc532UL, 0x000000080aebe5acUL, 0x0000000d2f2e9768UL, 0x0000000cc67edb56UL, 0x0000000a51e37f35UL, 0x00000006ac0eb6c3UL, 0x00000006af2a4aa1UL, 0x0000000e76290ecbUL, 0x000000037e738db9UL, 0x000000072d9b11c5UL, 0x000000076e613f46UL, 0x00000004f073278bUL, 0x00000000e1eea307UL, 0x0000000e9e8f9111UL, 0x00000000793ee6f5UL, 0x000000017304e15fUL, 0x00000007a3361104UL, 0x0000000731339958UL, 0x00000008daa6a511UL, 0x0000000a4037ef6bUL, 0x0000000210896f2fUL, 0x0000000afc535032UL, 0x0000000e3025a0f8UL, 0x000000063e21ba5fUL, 0x00000003ebb5b8c8UL, 0x0000000f9c6b06c3UL, 0x0000000ca95ee37eUL, 0x000000081f852bb4UL, 0x0000000d6895d823UL, 0x00000007040cca75UL, 0x00000004d66ec391UL, 0x00000004a216e588UL, 0x000000051d6c18ceUL, 0x000000047711c319UL, 0x00000006ae7f794cUL, 0x00000004abe694d7UL, 0x0000000bc96f6f6eUL, 0x000000057aa76cd2UL, 0x0000000f948f2648UL, 0x000000031bcd1bc3UL, 0x000000094c7b3f1dUL, 0x000000032eef86acUL, 0x0000000668f8ff2eUL, 0x00000006de170ab8UL, 0x00000009341b93e2UL, 0x0000000974e1c163UL, 0x000000073182af6dUL, 0x00000005d85fb48bUL, 0x000000078b257bdeUL, 0x0000000173d0eb29UL, 0x00000005add785d1UL, 0x0000000af6e83240UL, 0x0000000dce79166cUL, 0x000000022716840bUL, 0x00000007b408f1d9UL, 0x0000000de43a217eUL, 0x000000036e10fb6eUL, 0x0000000ea9a83ddfUL, 0x00000004dcf50162UL, 0x00000009aab07a8bUL, 0x0000000281364431UL, 0x00000008392dd46eUL, 0x00000000945e6aceUL, 0x00000008d07b3a82UL, 0x0000000d012f1990UL, 0x0000000b7098acc7UL, 0x0000000c2fcfa16cUL, 0x00000001e7c731a9UL, 0x0000000f16ea68eeUL, 0x00000002fa69cb03UL, 0x00000004cee1f92bUL, 0x00000001e20cfda2UL, 0x00000009e9d1ef4dUL, 0x0000000e83358a6dUL, 0x000000059a873d48UL, 0x0000000a6842b671UL, 0x00000006885bdbefUL, 0x000000073e4014faUL, 0x00000007b9954840UL, 0x0000000548157ffdUL, 0x00000008853a8c5dUL, 0x0000000eb8874fe0UL, 0x0000000b25d4f257UL, 0x000000036b447da5UL, 0x000000071d87958fUL, 0x0000000eedd91553UL, 0x00000004af23612aUL, 0x0000000278329eacUL, 0x0000000191121b76UL, 0x00000006e691175dUL, 0x00000000bd140329UL, 0x00000006ed4532ceUL, 0x0000000d5e3c8ff4UL, 0x0000000e26c64033UL, 0x0000000494a2097bUL, 0x0000000f5e36d440UL, 0x0000000b7818d202UL, 0x0000000a63548c34UL, 0x0000000682f0bdfdUL, 0x00000003d3c65c17UL, 0x00000004c4399ae7UL, 0x00000006b18e67ffUL, 0x00000004778211a3UL, 0x00000004089b2dd5UL, 0x000000021edee850UL, 0x0000000739cede72UL, 0x0000000858dfc744UL, 0x00000004bc5dba6cUL, 0x000000021cbd3bdcUL, 0x0000000ac83de313UL, 0x00000006135f08daUL, 0x0000000d3a3a9f0bUL, 0x0000000eb2716099UL, 0x000000040b88e413UL, 0x0000000992442a25UL, 0x0000000c639de695UL, 0x0000000683bcc7c7UL, 0x00000006245fc74fUL, 0x0000000543766bd5UL, 0x0000000375356569UL, 0x0000000fa45b7a88UL, 0x00000009f29b1207UL, 0x0000000ba245457cUL, 0x00000005b98e5ec9UL, 0x0000000204aca6b6UL, 0x0000000d52e9605bUL, 0x0000000504a40d28UL, 0x0000000ab6e1695eUL, 0x0000000a26481ebbUL, 0x00000009455ec341UL, 0x00000002f05f98e9UL, 0x0000000ead83365cUL, 0x0000000b928d8486UL, 0x00000007b860de0bUL, 0x0000000964ef7da2UL, 0x00000002422962b9UL, 0x000000028f5ddfb2UL, 0x00000008c5c63713UL, 0x00000009068c6494UL, 0x000000050aad5744UL, 0x000000093e7cca30UL, 0x00000009825e8b8fUL, 0x00000003a8b4947dUL, 0x0000000fa06737b5UL, 0x0000000cb9c963e8UL, 0x000000091a2bc332UL, 0x0000000284666b59UL, 0x0000000ceb82a1c8UL, 0x0000000a2fe9f06aUL, 0x000000006fa50365UL, 0x000000019aa747faUL, 0x0000000a6e117dc2UL, 0x00000002b3810910UL, 0x0000000ff7e857b2UL, 0x0000000048b55c3eUL, 0x0000000975456ac2UL, 0x0000000ad200ed37UL, 0x0000000e4d4d86efUL, 0x0000000b3ec62491UL, 0x00000008cd465c4eUL, 0x00000008a2be2d94UL, 0x00000005a9f7d648UL, 0x000000059e067a85UL, 0x0000000b5c35327eUL, 0x0000000c4ca8714eUL, 0x00000000e927a04cUL, 0x0000000f71eac628UL, 0x0000000109354e62UL, 0x00000001037b1a5bUL, 0x000000026c27f893UL, 0x00000003597fa385UL, 0x0000000ee24b62efUL, 0x00000004b31f921cUL, 0x0000000650244e5dUL, 0x0000000cfec64526UL, 0x0000000cd4bb0a21UL, 0x0000000648c56197UL, 0x0000000bd95056f8UL, 0x0000000983c8c183UL, 0x0000000ddc662f22UL, 0x00000005631bb980UL, 0x00000001203f56f3UL, 0x00000006a0c37549UL, 0x0000000a59baa8a4UL, 0x00000006a23a5068UL, 0x0000000ad609c354UL, 0x0000000f6f6d5e74UL, 0x000000036bc95f79UL, 0x0000000424c92c62UL, 0x00000003692abf50UL, 0x00000000a4bc460dUL, 0x00000001b4434b89UL, 0x00000004eb31302dUL, 0x00000009a3a891f9UL, 0x000000093af8d5e7UL, 0x0000000ef60bfa02UL, 0x0000000607a378d6UL, 0x00000005a5a7d835UL, 0x0000000536c0f467UL, 0x000000011f66a7feUL, 0x000000074c7c43c5UL, 0x000000066eae7c29UL, 0x0000000f5a785d2cUL, 0x0000000d3948a5c0UL, 0x0000000ec1094aa4UL, 0x0000000fcad61c19UL, 0x000000049ca7108aUL, 0x000000077437f4b6UL, 0x0000000553083d4aUL, 0x00000005c26c14cdUL, 0x00000006eb4caceeUL, 0x0000000e8aded63cUL, 0x0000000132aa4b5cUL, 0x000000057603a19eUL, 0x0000000ee359dda3UL, 0x0000000d0f5ea330UL, 0x000000046b0f0b1fUL, 0x0000000d47cbfc81UL, 0x00000003ed1b37b0UL, 0x00000004c8c752d8UL, 0x0000000ac202044bUL, 0x0000000207f16128UL, 0x000000053f5c3981UL, 0x000000070e1a33a2UL, 0x0000000ed8348baaUL, 0x0000000798f94a3eUL, 0x00000008896c890eUL, 0x0000000521425a3fUL, 0x0000000c8329e9eaUL, 0x000000041f238ba5UL, 0x00000001093d3c81UL, 0x0000000cab628e90UL, 0x0000000a1b4bf356UL, 0x000000092eee2fceUL, 0x000000059d35b9afUL, 0x00000002176e9f53UL, 0x00000007c9abfb89UL, 0x0000000b06d107e9UL, 0x0000000e94c318d5UL, 0x00000002f397ae30UL, 0x0000000d7e5a1a48UL, 0x000000098c4abc47UL, 0x0000000574737c29UL, 0x0000000d7f5401b2UL, 0x0000000852b87bc6UL, 0x00000001180fa957UL, 0x0000000501c63328UL, 0x0000000fd28c0d13UL, 0x0000000f764aa079UL, 0x0000000c8a6f8c5aUL, 0x0000000975b10240UL, 0x00000006bd33e4c0UL, 0x0000000a1113e39cUL, 0x0000000abcfa8fb8UL, 0x0000000149ea1facUL, 0x0000000f6443556fUL, 0x0000000959da07e7UL, 0x00000004721a2ac4UL, 0x000000030bde0f15UL, 0x0000000c7c4fdef8UL, 0x0000000b7feb4465UL, 0x000000056675073cUL, 0x000000096f3f57b9UL, 0x0000000876f0386eUL, 0x0000000393f0a7e8UL, 0x000000032b40ebd6UL, 0x000000010a11346aUL, 0x0000000fb81f48aeUL, 0x00000000a892877eUL, 0x000000086f636e08UL, 0x00000004fbc4d72bUL, 0x0000000561d5f314UL, 0x0000000ca18e2835UL, 0x00000007e6f519f5UL, 0x00000008b94e7983UL, 0x0000000619adfaf3UL, 0x00000004c9ddbbabUL, 0x0000000cb6a461f2UL, 0x00000000f6e22456UL, 0x0000000858c9b401UL, 0x000000092dc1b3b8UL, 0x00000003a783615bUL, 0x0000000c74b66f67UL, 0x0000000ea90891bcUL, 0x000000031a829e4bUL, 0x00000007bd38f505UL, 0x0000000b4476ea80UL, 0x00000001af371feaUL, 0x000000084894ff56UL, 0x00000000537584dfUL, 0x0000000d4ebdd1d0UL, 0x0000000b43cc192bUL, 0x000000076700d287UL, 0x0000000aaad9fa58UL, 0x0000000fa511710fUL, 0x00000006e54699e5UL, 0x00000006d73391aeUL, 0x00000003c2f1fb49UL, 0x00000004fbd96a75UL, 0x0000000252e6304bUL, 0x0000000e72826214UL, 0x00000008fe6c9336UL, 0x0000000326397743UL, 0x00000000e03bc524UL, 0x0000000dedac9594UL, 0x000000004ff19096UL, 0x0000000409b4cdbbUL, 0x0000000b15921888UL, 0x0000000a259bcd6dUL, 0x000000043c67f305UL, 0x00000001dc65dcecUL, 0x00000001730b4f85UL, 0x0000000f8a48f16aUL, 0x000000057a30e743UL, 0x000000005afd9839UL, 0x0000000682d5f3aeUL, 0x00000000b694337eUL, 0x0000000758c7dacfUL, 0x000000018fa1ae7dUL, 0x00000005ba9b5984UL, 0x000000032e1d45ddUL, 0x0000000672f05518UL, 0x0000000382ffc5b1UL, 0x00000008f0b08f33UL, 0x0000000b4a4d9ff0UL, 0x000000053ba0f980UL, 0x00000002ac0751fbUL, 0x0000000ab005de73UL, 0x000000061ab7be9bUL, 0x000000063078c9adUL, 0x000000027659d148UL, 0x0000000c653c684fUL, 0x0000000ecee05017UL, 0x000000024378ce5eUL, 0x0000000d0f7e5bacUL, 0x00000008b4bf4199UL, 0x0000000b7aa495fbUL, 0x0000000f3d6b78a5UL, 0x000000098bab1024UL, 0x00000006b4971fadUL, 0x00000000723d6c89UL, 0x0000000a17071a75UL, 0x0000000d55a301f4UL, 0x0000000988de925bUL, 0x00000007ca276f45UL, 0x0000000321b6e484UL, 0x0000000316149ed6UL, 0x0000000b8dba5bb9UL, 0x0000000aad4df3f4UL, 0x0000000178e204f5UL, 0x000000095cd2e357UL, 0x000000073fb8a733UL, 0x000000084ca10c86UL, 0x000000089498492dUL, 0x00000005b9bdf383UL, 0x000000066c58bb10UL, 0x0000000f153ac21eUL, 0x00000007ca5d3b04UL, 0x0000000637a521c7UL, 0x0000000b5ed589c1UL, 0x0000000d0a3c644eUL, 0x00000003d5d0754fUL, 0x0000000c6b901174UL, 0x00000003e96fd3edUL, 0x000000079c2fdf8cUL, 0x00000006594ae371UL, 0x00000003504fd77aUL, 0x000000032b81dcc7UL, 0x000000057b4f3e35UL, 0x00000004c9808072UL, 0x0000000a06c10993UL, 0x0000000b059666afUL, 0x000000072b69c034UL, 0x0000000e33ae836eUL, 0x0000000ad6cadf0dUL, 0x000000019573ace1UL, 0x0000000d562fd1e7UL, 0x0000000847804d9dUL, 0x00000009e32f6734UL, 0x00000002355c580fUL, 0x00000002f177b8d6UL, 0x0000000d689ac650UL, 0x0000000071b70abcUL, 0x0000000254915730UL, 0x0000000c5934f949UL, 0x0000000134d59d09UL, 0x00000002c731fb06UL, 0x0000000d90c6c5cbUL, 0x0000000cc3f9bca4UL, 0x0000000bf078980cUL, 0x000000080838e95aUL, 0x00000005ccd6f054UL, 0x00000000378bae56UL, 0x00000008d1ddb978UL, 0x000000093b875cf4UL, 0x0000000e2ce90bc6UL, 0x0000000ec291b91bUL, 0x0000000d0a11bdc1UL, 0x0000000751be7244UL, 0x0000000579fcd29eUL, 0x0000000f76e5ccb1UL, 0x0000000eb33da184UL, 0x0000000c0ac75b0fUL, 0x000000015454fb33UL, 0x00000008b92a411cUL, 0x00000007da34b476UL, 0x00000004f413d45eUL, 0x000000010ec1dbeaUL, 0x0000000650739b93UL, 0x0000000315e08a31UL, 0x0000000d4fd5f1bdUL, 0x0000000b5058a126UL, 0x000000020d5cb63bUL, 0x0000000c1598dfe7UL, 0x0000000a0a2a338dUL, 0x0000000e29af7686UL, 0x000000083fd0cac9UL, 0x000000014a8094d8UL, 0x0000000816e0afa3UL, 0x0000000499ef5d2cUL, 0x0000000bddbd0d95UL, 0x0000000a30839ca9UL, 0x00000004fd33fb4cUL, 0x0000000ef63557b7UL, 0x0000000535f06ab2UL, 0x00000000d47d352eUL, 0x0000000a371e6dc4UL, 0x00000008ac914b17UL, 0x00000006ba9d69d7UL, 0x0000000096da89aaUL, 0x000000065bbd5d14UL, 0x0000000d41d2c5c4UL, 0x000000052a283583UL, 0x00000004c1f56d26UL, 0x000000086cd9984aUL, 0x000000026cbcedc6UL, 0x00000001aa0eaa03UL, 0x0000000b95d5ad2cUL, 0x0000000e2eb56b20UL, 0x0000000ff49d9d5cUL, 0x0000000cce338378UL, 0x0000000330e9fac7UL, 0x0000000e2f53974aUL, 0x0000000668d1c6d5UL, 0x0000000eca0ba751UL, 0x00000008d48ab5e6UL, 0x0000000d205e18cdUL, 0x00000001c391633cUL, 0x0000000ef5d02e5fUL, 0x0000000d12bb5f20UL, 0x0000000323215199UL, 0x000000088f5b3ffcUL, 0x0000000931445f29UL, 0x0000000b893cb727UL, 0x000000032851ecc0UL, 0x000000080b44d81bUL, 0x00000005aa48da98UL, 0x000000046d1e1284UL, 0x00000004c837ba14UL, 0x0000000eb22c26deUL, 0x0000000e51e9d246UL, 0x00000008d03deee6UL, 0x00000005af8e0909UL, 0x0000000bde9773a4UL, 0x0000000bf611cabfUL, 0x0000000d24ac96e7UL, 0x00000009fe919318UL, 0x000000050d0206a6UL, 0x0000000b43b9741cUL, 0x0000000ba48d4fb3UL, 0x00000006bccd7290UL, 0x00000008bc6bfb9cUL, 0x0000000e5a036c9fUL, 0x0000000a80a2cfeeUL, 0x0000000c193655a7UL, 0x00000007c8e5170dUL, 0x00000006141edbbbUL, 0x00000004d6b990dcUL, 0x0000000cc49b5702UL, 0x00000002343fef58UL, 0x0000000d50cb593cUL, 0x00000004248a60cdUL, 0x0000000901cfbd4cUL, 0x000000064a4c8736UL, 0x00000001b2dcbaeaUL, 0x0000000d691e5f4cUL, 0x0000000df352a493UL, 0x00000001991ac7daUL, 0x00000004c4879f45UL, 0x00000009b34aadeeUL, 0x000000052bb3db0dUL, 0x00000007b9a8c9d3UL, 0x0000000d7ce6e47eUL, 0x0000000ec0b922d8UL, 0x00000008079cab6bUL, 0x0000000abadc8899UL, 0x00000000f57b93b7UL, 0x000000005c4ef219UL, 0x0000000d7a438d49UL, 0x0000000f55ecca97UL, 0x0000000d07899f1dUL, 0x0000000260947d6cUL, 0x0000000ffbd21ab6UL, 0x0000000d04ff923eUL, 0x0000000964b72033UL, 0x000000031ac3fd7eUL, 0x0000000d2c52e2c4UL, 0x0000000799a640efUL, 0x000000098dd061edUL, 0x00000005cb2ab7b8UL, 0x000000072f3881c8UL, 0x0000000e65ed1164UL, 0x000000034fa0bd5bUL, 0x000000064f9823cdUL, 0x00000003797e1ac0UL, 0x00000002fb8a4751UL, 0x00000006f347342eUL, 0x000000022dd7ea0aUL, 0x0000000b19b65e57UL, 0x000000044fe83e8aUL, 0x000000007732732eUL, 0x000000064de20ed7UL, 0x000000006c9ea834UL, 0x00000008ce066650UL, 0x0000000c2a685ff0UL, 0x000000064f19b01fUL, 0x0000000491ab8a88UL, 0x000000041212fe5aUL, 0x00000006f9916f3bUL, 0x0000000694f72e71UL, 0x0000000ad7a5b35eUL, 0x0000000f62795292UL, 0x0000000c8cdc3d3aUL, 0x0000000fbc6b3518UL, 0x000000067b631901UL, 0x00000005b5ba79d5UL, 0x0000000f4fadebddUL, 0x0000000ac7c802e7UL, 0x0000000385712d9dUL, 0x000000064bd375b4UL, 0x0000000c9a11df70UL, 0x000000088355bf31UL, 0x0000000606ffbb0aUL, 0x0000000bda93c2d5UL, 0x00000007c5f94f0aUL, 0x000000076fe26501UL, 0x00000005d8b9153cUL, 0x0000000886bbb218UL, 0x0000000acee2fecaUL, 0x00000002ad19a925UL, 0x000000083b97855cUL, 0x0000000d36608312UL, 0x00000008ac60dbc7UL, 0x00000000885c8f58UL, 0x00000008abbdf891UL, 0x0000000ea1602271UL, 0x0000000ad654fee1UL, 0x00000006c461195eUL, 0x00000005eeb1a327UL, 0x000000018d743962UL, 0x00000001fc7c55a5UL, 0x0000000aba749670UL, 0x00000009c9a59c60UL, 0x00000006e5bafc06UL, 0x000000096977db12UL, 0x0000000a97b6ebfaUL, 0x000000063d2d9da6UL, 0x0000000fab00cd60UL, 0x0000000d7bdf4632UL, 0x0000000f83878d59UL, 0x0000000b1c2c462eUL, 0x000000014e5144a7UL, 0x0000000f4a909b28UL, 0x0000000e979a185bUL, 0x0000000908090a64UL, 0x000000099eccd798UL, 0x0000000348780a96UL, 0x0000000fdc7ad169UL, 0x0000000a600c2e5bUL, 0x0000000b0968cd98UL, 0x00000001a45ec098UL, 0x000000099118c1b4UL, 0x00000008afa5cd5aUL, 0x00000001db7e655eUL, 0x00000009f637e452UL, 0x00000009568504e3UL, 0x0000000045b2a662UL, 0x0000000f2a1455a2UL, 0x00000006c1ca9e75UL, 0x000000030a4a4639UL, 0x0000000c6c2c1a30UL, 0x000000087500b452UL, 0x00000005e338bb2eUL, 0x0000000d9dd11dffUL, 0x00000008c4b5d012UL, 0x00000008191194e0UL, 0x0000000dd11db867UL, 0x0000000c67c151ceUL, 0x00000005cb1a00e4UL, 0x0000000098b7a1c6UL, 0x0000000369f35cd4UL, 0x0000000ca2190bdbUL, 0x00000006e14bb3b9UL, 0x00000008d5692f8cUL, 0x0000000ca4b2f4f8UL, 0x0000000787f06877UL, 0x00000008acbb8550UL, 0x0000000535f4b56aUL, 0x0000000f4caf7ecbUL, 0x0000000d4615b258UL, 0x0000000347ca7070UL, 0x00000003c798c85dUL, 0x0000000460506465UL, 0x0000000870d0a5dcUL, 0x00000006510b2464UL, 0x0000000d1dba5544UL, 0x0000000d57789a33UL, 0x0000000e2417c5baUL, 0x0000000b5ff8628cUL, 0x0000000a3bb22787UL, 0x0000000a16b64f34UL, 0x0000000421e81d3dUL, 0x000000035b4596a7UL, 0x00000008d7a2dd7eUL, 0x000000050b2d83faUL, 0x00000009ea87e7c2UL, 0x0000000d5055e752UL, 0x0000000f96aa9da5UL, 0x0000000b096e2a07UL, 0x000000049970b44bUL, 0x0000000867fb1518UL, 0x00000005d0f5dba2UL, 0x00000001b191d11eUL, 0x00000008e839bb8fUL, 0x00000001cd4aca15UL, 0x0000000971ec5615UL, 0x00000007d72a7ebdUL, 0x00000008b1253bfbUL, 0x0000000e11de1d25UL, 0x00000000a7566839UL, 0x0000000f4f3542e0UL, 0x00000001ea791e32UL, 0x000000032a84f759UL, 0x0000000646f1844eUL, 0x000000042af26809UL, 0x00000001f4b464ffUL, 0x0000000da684d2d9UL, 0x0000000d854f5fb9UL, 0x00000004d4d3e91aUL, 0x00000005af3ef4e2UL, 0x00000008a1ef5ce7UL, 0x00000002354febf3UL, 0x0000000b3c5a8944UL, 0x000000098b62a144UL, 0x00000009bdba0b4eUL, 0x000000004aa99b42UL, 0x00000008099ea151UL, 0x00000002185463a3UL, 0x0000000b0a1ae997UL, 0x0000000e628d5770UL, 0x0000000b40b5ac89UL, 0x000000027213b17dUL, 0x00000004d21db5b5UL, 0x000000010d0748f7UL, 0x00000002276c7876UL, 0x0000000b98bee56dUL, 0x0000000bd1ca6ae8UL, 0x0000000824ab48faUL, 0x0000000c6f35ae62UL, 0x00000003547a563cUL, 0x0000000f1fc0d824UL, 0x000000058f55ed75UL, 0x0000000aa9d0de01UL, 0x00000004719dde60UL, 0x0000000d5386b3ddUL, 0x00000004d8d9f666UL, 0x0000000aee36013bUL, 0x0000000ba4ee322fUL, 0x0000000898d2db4eUL, 0x00000009fe364808UL, 0x0000000bb13e8045UL, 0x0000000be346d43aUL, 0x0000000b4c9f886fUL, 0x0000000c9a6f53b8UL, 0x00000000ed5a7b6fUL, 0x00000002a1fac740UL, 0x0000000b8c134a59UL, 0x0000000b1f773993UL, 0x0000000c4d9d0025UL, 0x0000000ca905bdcaUL, 0x00000003150a39a7UL, 0x0000000e8329fad5UL, 0x0000000bd4f98059UL, 0x00000003bc5cf6cdUL, 0x0000000c982fdd03UL, 0x00000000a372de28UL, 0x000000073fe2e35aUL, 0x00000000b9f684ecUL, 0x0000000c543ff680UL, 0x00000001bcf5f09aUL, 0x000000051b2a8099UL, 0x0000000ee53277c2UL, 0x00000000b3835a6cUL, 0x0000000aed6765c1UL, 0x000000092cfd64c8UL, 0x0000000d20c60ed2UL, 0x000000059dbd9f51UL, 0x0000000b6acb694bUL, 0x0000000427dcd5fdUL, 0x0000000646336a75UL, 0x00000008008dea4dUL, 0x00000000af2bdc7cUL, 0x0000000b8a46478aUL, 0x0000000b02c535b6UL, 0x0000000c645d8631UL, 0x0000000044b4af3dUL, 0x0000000c9edfe6cbUL, 0x000000032ac8ea2aUL, 0x000000079266a23fUL, 0x0000000c2d902e93UL, 0x00000006ae5cfbdbUL, 0x00000002c66c633eUL, 0x0000000eb7a8a4e3UL, 0x0000000cb17281cfUL, 0x00000007ca378680UL, 0x00000007ac81509dUL, 0x0000000a59a05073UL, 0x0000000c9cb9f18dUL, 0x0000000b78100d29UL, 0x0000000fab49420aUL, 0x0000000d0a4e69c4UL, 0x0000000d6c33f722UL, 0x000000068d21bff8UL, 0x00000001fdad8ca3UL, 0x00000002884d6968UL, 0x0000000b091ff264UL, 0x0000000eb5fb236fUL, 0x0000000a3d2a1839UL, 0x0000000527db0bc8UL, 0x00000002dc68cd9fUL, 0x0000000e3f4ea98aUL, 0x0000000a629fe44fUL, 0x0000000b73bd7d66UL, 0x00000002abfd7b6bUL, 0x00000001b4056054UL, 0x0000000d6efaac28UL, 0x00000000d13cc950UL, 0x0000000ef84ead94UL, 0x00000005b6ee0d50UL, 0x00000000f4bec692UL, 0x0000000de1b98881UL, 0x000000055ccccd31UL, 0x0000000086d9b84dUL, 0x00000005ab736e3dUL, 0x0000000167d2f005UL, 0x0000000118ed1522UL, 0x000000038bbdc903UL, 0x000000039cd31ac2UL, 0x000000031091bc51UL, 0x0000000d66a87d3fUL, 0x0000000afdade6d3UL, 0x00000002bd1fe097UL, 0x00000005cf545dd2UL, 0x00000005e0af578eUL, 0x00000006fe6dd4c9UL, 0x0000000862bc8fcaUL, 0x0000000cbce0b4c6UL, 0x000000008b7fa8ddUL, 0x00000003d108ae9fUL, 0x0000000fed2d914aUL, 0x0000000bab304bd8UL, 0x0000000debe74f8dUL, 0x00000001e857e3dcUL, 0x0000000570340581UL, 0x0000000114bbf4f5UL, 0x0000000a3cfc0566UL, 0x00000004026cd686UL, 0x0000000266fb76cdUL, 0x0000000b715773bbUL, 0x00000002fd2785fdUL, 0x0000000481b34cadUL, 0x000000011c58d2baUL, 0x00000003a5186f4dUL, 0x0000000da55ab71cUL, 0x0000000ac887db92UL, 0x00000009bd6d5592UL, 0x000000045857d12aUL, 0x00000008c862f0b9UL, 0x0000000870c88666UL, 0x00000004a4f4901fUL, 0x0000000774a993d0UL, 0x0000000c9f16c81dUL, 0x0000000eb415e9efUL, 0x0000000307aa6302UL, 0x0000000a246f21eeUL, 0x00000001a4f8a9c2UL, 0x00000000cf09f9b4UL, 0x0000000db30dbb49UL, 0x00000003581be36fUL, 0x00000006919a4318UL, 0x00000008ee677afdUL, 0x00000005944b9d59UL, 0x00000008d5fe61aaUL, 0x000000077c174b1dUL, 0x00000005cff8fa10UL, 0x0000000c1ce82f48UL, 0x00000007fbb18e65UL, 0x00000000b6737103UL, 0x0000000e2d30a9b6UL, 0x00000006481ff469UL, 0x00000005834b4d26UL, 0x00000003bba517d5UL, 0x0000000eee6e8080UL, 0x00000005fe4fea5eUL, 0x0000000e84e94c8cUL, 0x0000000ba2ad0a2aUL, 0x0000000a7f2aead0UL, 0x000000063cecb46dUL, 0x00000008943d7229UL, 0x00000001d3878b2bUL, 0x0000000f2b4efe94UL, 0x0000000d9af1949dUL, 0x0000000bb5824d39UL, 0x0000000b8d8f5090UL, 0x0000000ed5e19d08UL, 0x000000060287437eUL, 0x00000008fe6ae5c2UL, 0x00000006c85ac058UL, 0x0000000b906be1b8UL, 0x0000000f9d423f65UL, 0x00000006efed81d6UL, 0x0000000781b67fa2UL, 0x0000000e1dd437acUL, 0x00000007a9201a8cUL, 0x0000000fb444c819UL, 0x0000000ce75af959UL, 0x000000086df6e72bUL, 0x0000000756695aa7UL, 0x0000000b7b2bddf2UL, 0x0000000f19a1b99eUL, 0x00000009a5790e90UL, 0x00000001d3b3eac0UL, 0x0000000a5c5d9d2bUL, 0x0000000152850218UL, 0x0000000025c4ba6eUL, 0x0000000d4a5f4bebUL, 0x0000000709cec10eUL, 0x000000094ddbdb6cUL, 0x00000009d1218277UL, 0x00000006190ca34aUL, 0x0000000468ed6a3fUL, 0x0000000801bda52eUL, 0x0000000261b3f1a9UL, 0x00000000b3494d9bUL, 0x0000000583e2d7e5UL, 0x00000009407a80f2UL, 0x000000058e902456UL, 0x00000009108c2273UL, 0x000000059778ff8cUL, 0x0000000d6ce05028UL, 0x00000000286adc62UL, 0x00000007ed3060dcUL, 0x000000057b7e03edUL, 0x00000003e3dce5c1UL, 0x00000001bebc2295UL, 0x0000000014a17c9aUL, 0x0000000c7d90fbdaUL, 0x00000008158ae35aUL, 0x000000069d70a335UL, 0x0000000d3ef97931UL, 0x00000005793efb7aUL, 0x0000000e6989ef43UL, 0x0000000cd15f0116UL, 0x0000000f9dbc6e25UL, 0x0000000da4a91117UL, 0x0000000054d0917aUL, 0x000000060f2c3f15UL, 0x00000007393b0a66UL, 0x00000006630ed79bUL, 0x0000000ed8589c60UL, 0x00000007db37ab26UL, 0x0000000c4631e80aUL, 0x00000001badaf501UL, 0x00000009bdef764dUL, 0x0000000dd0949b4bUL, 0x000000086f116771UL, 0x0000000acd7ea109UL, 0x00000007cc9d2f6bUL, 0x00000003f5598822UL, 0x00000004ba5a8d0cUL, 0x000000066e7f9c42UL, 0x000000033127fb36UL, 0x00000000c85ff976UL, 0x00000009dbb32ddfUL, 0x00000003d06c7a56UL, 0x0000000ac07601ddUL, 0x00000005fda3d7e9UL, 0x000000040a47aef0UL, 0x0000000139928cd0UL, 0x0000000183ab75ebUL, 0x00000009dd6d1f4bUL, 0x0000000954afec44UL, 0x000000029953fe22UL, 0x0000000f947e49b1UL, 0x0000000a74266cb0UL, 0x00000003bbb7fdabUL, 0x00000008a72b63d1UL, 0x00000008763e2fbbUL, 0x00000008c9b4f9a2UL, 0x0000000a35f5a861UL, 0x000000099e54752cUL, 0x00000002fdb8e16fUL, 0x00000002d083ed68UL, 0x0000000a05d36c5eUL, 0x00000005460842feUL, 0x0000000173ae0ee6UL, 0x000000038b3c62e5UL, 0x0000000476c1ae99UL, 0x00000009a8cb898aUL, 0x000000019d4032acUL, 0x0000000a9c01d80bUL, 0x0000000ca7d5e4deUL, 0x0000000295d53115UL, 0x0000000b26740e51UL, 0x0000000bf21b0988UL, 0x0000000167391c15UL, 0x0000000d10af35c6UL, 0x0000000d94750799UL, 0x0000000cb986d117UL, 0x000000001dddf588UL, 0x000000071ed85f46UL, 0x0000000a5437d58fUL, 0x00000004029d1e25UL, 0x0000000c580ec972UL, 0x00000006847df8baUL, 0x0000000e294d997bUL, 0x0000000e2e8b10eeUL, 0x00000001593103ddUL, 0x0000000222103857UL, 0x00000001e035591dUL, 0x0000000b5c9ef2e9UL, 0x00000009f815ec3eUL, 0x0000000d1da2a021UL, 0x000000054f171191UL, 0x0000000e51f4a05eUL, 0x0000000c15e7d603UL, 0x0000000ba7f16b87UL, 0x000000080b7a83e1UL, 0x0000000720e2b18dUL, 0x00000005ec0c069dUL, 0x0000000a4f9f689cUL, 0x00000005871cafdaUL, 0x0000000c913140a2UL, 0x00000007a8f2efd1UL, 0x000000077064952cUL, 0x00000004ea2d857fUL, 0x0000000484523555UL, 0x000000054971a9e3UL, 0x0000000eb0694eb2UL, 0x0000000b513c8e63UL, 0x00000005c910db58UL, 0x0000000ca87a4dd7UL, 0x0000000b8ca63158UL, 0x0000000b4b09431dUL, 0x00000003dc9d50b7UL, 0x00000007d57f02acUL, 0x00000005c595b1b2UL, 0x00000009e0caf698UL, 0x0000000136b48555UL, 0x0000000687dbcc2bUL, 0x000000054bae2294UL, 0x00000006899bbd7bUL, 0x00000008108f46deUL, 0x00000001dbe8cf08UL, 0x0000000a02e1ae1dUL, 0x00000000f5f26d59UL, 0x0000000805cf202bUL, 0x0000000afede5687UL, 0x00000001583d5b30UL, 0x0000000da9ed0620UL, 0x0000000cf1237338UL, 0x00000003a5a77bc4UL, 0x0000000a17ffa0c6UL, 0x000000029de4c387UL, 0x000000007825d431UL, 0x000000002d7b9b38UL, 0x00000008ed0f26aaUL, 0x000000056e54e30dUL, 0x00000009620ab0e7UL, 0x0000000c7e3ea94cUL, 0x0000000d288a41e2UL, 0x0000000f68884f1eUL, 0x00000005ee02df09UL, 0x0000000c02dbf645UL, 0x0000000eac4c2424UL, 0x0000000cab2d51e1UL, 0x0000000037439577UL, 0x00000005618ada43UL, 0x00000002683b5859UL, 0x00000008a607c1ceUL, 0x0000000795fd9198UL, 0x0000000b3edb11b8UL, 0x0000000846939c5cUL, 0x00000008b1f6fa23UL, 0x0000000b1a2f2bfeUL, 0x0000000b63a07ad7UL, 0x00000005f8ea7b00UL, 0x00000004ee9c6d0cUL, 0x0000000990f2889bUL, 0x0000000b7f7251d0UL, 0x0000000ac3291369UL, 0x00000009d8f36a7bUL, 0x0000000d57342897UL, 0x0000000efca98365UL, 0x0000000dacc69f0eUL, 0x00000003a70e4b3cUL, 0x00000001e95c34c2UL, 0x00000004caab6c06UL, 0x00000007231f6ee1UL, 0x000000037909aa04UL, 0x0000000048c9a9ccUL, 0x000000059cd081bcUL, 0x00000004dd78c2e4UL, 0x00000004979da10fUL, 0x000000004749d0c5UL, 0x0000000a17a4283bUL, 0x0000000de7e1d52dUL, 0x00000000e47cedf1UL, 0x00000004fa48cbffUL, 0x0000000545a932a0UL, 0x00000006c2bd9eb8UL, 0x0000000dd9bd3b8cUL, 0x000000043332c1baUL, 0x0000000501fa761dUL, 0x00000007ec40adbbUL, 0x00000004049f2b33UL, 0x0000000cde28f57bUL, 0x0000000f68c804b9UL, 0x00000008f50fbd3eUL, 0x000000054e1bc344UL, 0x000000036b26e3a2UL, 0x000000002e5ac9b1UL, 0x000000010837858dUL, 0x00000006ccac9e0bUL, 0x0000000625ba8a52UL, 0x0000000ac4c8b45cUL, 0x0000000868678237UL, 0x00000004187235feUL, 0x0000000bd62663ceUL, 0x0000000ea832dfb2UL, 0x0000000d5a72f0a7UL, 0x00000000659c855eUL, 0x0000000bea7f5e48UL, 0x0000000ff9566715UL, 0x00000001bd06d99aUL, 0x00000009666c578cUL, 0x0000000c6527d3ecUL, 0x0000000b541f3c61UL, 0x0000000678a9ad70UL, 0x000000036eaadfa3UL, 0x0000000af74b01deUL, 0x000000054cc3cdc3UL, 0x0000000d2e587ce6UL, 0x00000008694b9349UL, 0x0000000d309898feUL, 0x00000005c3250e09UL, 0x000000084dcac28eUL, 0x0000000f72add2dfUL, 0x00000001901681a3UL, 0x000000009e6a8fd4UL, 0x000000012f614cd1UL, 0x00000006d7801ac4UL, 0x000000014cf1ca54UL, 0x000000012a7eb608UL, 0x00000005e7a3bf62UL, 0x00000000ba5056a2UL, 0x00000005bee44c9bUL, 0x0000000819d7dc86UL, 0x0000000062adc8fdUL, 0x0000000bd3155d41UL, 0x0000000cd8c6b38aUL, 0x0000000e320fd50eUL, 0x0000000e189d6655UL, 0x00000006863c2831UL, 0x00000000d2b9058fUL, 0x000000023bfad8faUL, 0x0000000199bd1216UL, 0x000000056138afd7UL, 0x0000000face83a93UL, 0x00000009554da725UL, 0x00000009b614dd91UL, 0x000000098acbca3fUL, 0x0000000d5f0d5f21UL, 0x0000000eb59039e1UL, 0x000000051d1ec82aUL, 0x0000000a366ef3baUL, 0x00000001ad0e01f0UL, 0x00000007f038ad0bUL, 0x00000003ee055321UL, 0x00000003bf2dcbb7UL, 0x0000000210e9856cUL, 0x0000000e4fea8231UL, 0x0000000b89444937UL, 0x000000058852cc34UL, 0x00000001ee29eea9UL, 0x0000000b919c79f2UL, 0x0000000ddc44d3adUL, 0x0000000ddcbd4777UL, 0x00000003c3982ba1UL, 0x0000000dc8ebc45dUL, 0x00000008b97712b1UL, 0x00000009702ea21eUL, 0x00000001f457e726UL, 0x000000027c6f6e26UL, 0x00000000a9797770UL, 0x0000000d7615f53bUL, 0x000000074f1cb6e1UL, 0x0000000a32e4d7dcUL, 0x00000002e89afd1dUL, 0x00000000b03704d5UL, 0x0000000cca58aab0UL, 0x00000001e5749225UL, 0x00000006e63a36baUL, 0x0000000562992099UL, 0x000000064701b950UL, 0x0000000f94ed6196UL, 0x0000000b3441b5f1UL, 0x0000000c64fac247UL, 0x0000000d72ebd98bUL, 0x0000000fa1985b23UL, 0x00000002df788358UL, 0x000000088838b488UL, 0x00000006091032b4UL, 0x000000025ff2d736UL, 0x0000000dce63d3d5UL, 0x0000000bb5970414UL, 0x000000044d8b5ffeUL, 0x0000000e1a5666d8UL, 0x0000000e34129125UL, 0x00000000e23854b1UL, 0x000000001b2a6dbeUL, 0x0000000d11507bcdUL, 0x0000000844531e6bUL, 0x0000000d864a8611UL, 0x0000000e2a5a7700UL, 0x00000002d178962aUL, 0x0000000156b07f01UL, 0x000000048b59fec3UL, 0x00000003d3d9d79cUL, 0x00000001846fb339UL, 0x0000000ddf1d03caUL, 0x00000000998abaf9UL, 0x0000000c9d76190bUL, 0x000000067354a1a8UL, 0x0000000cc89e2b09UL, 0x0000000353356834UL, 0x00000007ad97470eUL, 0x0000000f4d560524UL, 0x0000000534b7804eUL, 0x000000014290c632UL, 0x0000000b67d39d60UL, 0x000000035b166febUL, 0x000000088e6fb681UL, 0x0000000a0f82ae1aUL, 0x000000008460ce52UL, 0x00000008b06a9012UL, 0x0000000daf1299dcUL, 0x0000000629ab696cUL, 0x00000003113b448aUL, 0x00000000db5ca215UL, 0x00000003e00b1e2dUL, 0x000000085a87f5abUL, 0x0000000b3995ff20UL, 0x000000085661554dUL, 0x0000000e709c5384UL, 0x00000000111ca99bUL, 0x000000049e614279UL, 0x0000000f14677ec4UL, 0x00000008f6439bfbUL, 0x0000000749faa461UL, 0x00000001c4f9189aUL, 0x0000000e8e9015caUL, 0x0000000f6e68d510UL, 0x0000000b3819319fUL, 0x0000000da9f7119fUL, 0x00000007787f40f8UL, 0x0000000bc57f5716UL, 0x000000060ff2897eUL, 0x0000000b3a28a934UL, 0x000000010b34c97cUL, 0x0000000c14f53aedUL, 0x0000000d3c4eaf5dUL, 0x0000000b3148d39eUL, 0x000000007874ea02UL, 0x0000000f86692b4aUL, 0x00000005b03a0e8dUL, 0x0000000ce6db8cc6UL, 0x00000008233d5908UL, 0x0000000f163e3c06UL, 0x0000000dff854cceUL, 0x000000026706f1bcUL, 0x000000094c358653UL, 0x00000007384c9821UL, 0x0000000e51b8e5d5UL, 0x0000000eda32963bUL, 0x0000000a073f392fUL, 0x0000000c3ccfa213UL, 0x000000034adf5216UL, 0x0000000cb8da286bUL, 0x00000003b5fbbf08UL, 0x000000012812d1f8UL, 0x0000000c97c54c39UL, 0x0000000e1c3e36b9UL, 0x0000000abb8dc0edUL, 0x0000000019dcbbf6UL, 0x000000025b0d7c4dUL, 0x0000000045e6b5ceUL, 0x000000017dc086caUL, 0x0000000c3f425e6bUL, 0x00000006fdee14f8UL, 0x000000039155e6b4UL, 0x00000000a191ec15UL, 0x0000000398fcd7f4UL, 0x0000000a6e2b0594UL, 0x0000000fe5678d82UL, 0x0000000e317eba1fUL, 0x00000002c4f10ca1UL, 0x0000000ae239c19eUL, 0x000000018e663ed2UL, 0x00000004a040b7e7UL, 0x0000000bbca0849cUL, 0x0000000ce05b3a74UL, 0x00000007cee982fdUL, 0x000000078ee54fa7UL, 0x00000007b47bb0bdUL, 0x00000007e8f19216UL, 0x0000000d67d91cedUL, 0x0000000ef5effe94UL, 0x0000000ec1d1938dUL, 0x00000004c05ef70eUL, 0x00000000324442d9UL, 0x0000000fb0183bb4UL, 0x0000000fb7a0bd50UL, 0x000000089aa17d87UL, 0x0000000e4e6aed89UL, 0x0000000dbecf68b4UL, 0x0000000683770de4UL, 0x0000000b9f41a136UL, 0x0000000c7614caceUL, 0x000000089c298386UL, 0x0000000959cf09deUL, 0x0000000ab30b19e3UL, 0x0000000db2e4b614UL, 0x000000026d30d39bUL, 0x00000006ccefe452UL, 0x0000000587c5035cUL, 0x0000000ea73bbbe0UL, 0x0000000dd9d91a11UL, 0x0000000dd8c5e851UL, 0x0000000e8b4aa077UL, 0x00000008ccf8faddUL, 0x000000047ddd3c0bUL, 0x0000000635a92f19UL, 0x0000000f0edfd1a3UL, 0x00000001f760bf5eUL, 0x0000000a83feb68aUL, 0x00000004f74da9ddUL, 0x000000052f759252UL, 0x000000098bee689eUL, 0x0000000c5fc8c3d5UL, 0x00000008373d1286UL, 0x0000000f5f1cdabdUL, 0x0000000ada68d3e5UL, 0x00000003bbb9eb5eUL, 0x000000050cde8478UL, 0x0000000f01f956e0UL, 0x0000000a922f2842UL, 0x0000000233a8b25aUL, 0x000000071118b754UL, 0x0000000b7f874552UL, 0x000000044d757121UL, 0x0000000b873b14ccUL, 0x00000005bcc1db5cUL, 0x0000000bf9b895ceUL, 0x00000005e65bb620UL, 0x0000000bbd1ed35cUL, 0x0000000358e79973UL, 0x000000062aa5a4a5UL, 0x000000081715fc0fUL, 0x00000008df03a76eUL, 0x0000000376b7c6c7UL, 0x0000000a07a49f2eUL, 0x000000045e159b63UL, 0x0000000dae5706b0UL, 0x0000000b5e52c7ccUL, 0x0000000206935e8eUL, 0x000000039f0c5119UL, 0x00000003cd58c574UL, 0x0000000571986d35UL, 0x0000000ad66da60fUL, 0x000000002b1a6315UL, 0x0000000d0131b533UL, 0x0000000741a195c5UL, 0x00000000b8663437UL, 0x00000001cde52798UL, 0x00000006b8e658b1UL, 0x0000000b43c0d44dUL, 0x000000045481d697UL, 0x000000029de93df5UL, 0x000000010549b874UL, 0x0000000c056b5828UL, 0x000000003fa830adUL, 0x00000009496d14faUL, 0x0000000f540592a0UL, 0x0000000f31c8b855UL, 0x000000064f2ba36bUL, 0x0000000fe7c6e4f5UL, 0x00000005e42a78b0UL, 0x00000009c2b8b096UL, 0x0000000dcb4a6e71UL, 0x0000000d63b0e7edUL, 0x0000000de1bcbcdaUL, 0x000000068e7161f2UL, 0x00000003e5ddf88dUL, 0x0000000419a37501UL, 0x0000000fad63e7abUL, 0x0000000c6e81b4baUL, 0x00000008329315d3UL, 0x0000000c88d267e6UL, 0x000000073a0ac25fUL, 0x0000000e7b75690fUL, 0x0000000dcbb95be2UL, 0x00000007a1d2a059UL, 0x0000000d8fac361eUL, 0x00000006312ff5c9UL, 0x0000000d2cf50d54UL, 0x00000008c65fd00fUL, 0x0000000aa1636532UL, 0x0000000870c7285dUL, 0x00000001894f0b84UL, 0x00000004260cc5c3UL, 0x0000000e9997b9ecUL, 0x000000087a052144UL, 0x00000008706babf6UL, 0x0000000bd5f62ad3UL, 0x00000001a7895439UL, 0x0000000f7e294bbcUL, 0x0000000bcc27ca26UL, 0x00000003186a63d4UL, 0x00000007f3ede4a4UL, 0x0000000b64e32468UL, 0x000000071f250d53UL, 0x00000007c6513783UL, 0x0000000b1778714aUL, 0x000000094bf2c57fUL, 0x000000064a9f893aUL, 0x00000001305be654UL, 0x0000000493e0c9f6UL, 0x000000005ba6fed8UL, 0x0000000c4a0c7a06UL, 0x00000000cc2ec0ddUL, 0x0000000d9a6769afUL, 0x0000000724c78a49UL, 0x0000000c85c981a4UL, 0x000000012553c4cdUL, 0x000000083cb892b1UL, 0x0000000bc324ccc7UL, 0x0000000ef43f6c1dUL, 0x00000002d6748bb7UL, 0x00000005efdce2d7UL, 0x000000094af64f28UL, 0x0000000f9d58feb3UL, 0x0000000cf547ac63UL, 0x0000000ceb309febUL, 0x000000030beba8caUL, 0x00000008ab2e486aUL, 0x00000004a95d58adUL, 0x000000025ce07c46UL, 0x0000000712b93fd7UL, 0x00000007f46acc81UL, 0x000000064049d4beUL, 0x000000065303aa09UL, 0x0000000f3aad21b3UL, 0x00000002903a6cd0UL, 0x00000005a0e0467dUL, 0x00000003c4fa64e4UL, 0x00000005c6655126UL, 0x0000000b40a2a67fUL, 0x0000000b0c22c6e5UL, 0x00000001507e039bUL, 0x0000000b282b16b8UL, 0x0000000c0e14a3d3UL, 0x000000093d381427UL, 0x00000006bb55bb87UL, 0x0000000b675af72fUL, 0x0000000fceb4f95eUL, 0x000000066af6ebbdUL, 0x000000020a44d1f2UL, 0x00000006bc873916UL, 0x0000000b8947bee8UL, 0x00000004b6bed8a6UL, 0x00000007012f7867UL, 0x00000007eda3c150UL, 0x0000000ab3ef1b8eUL, 0x00000006d71466eeUL, 0x0000000408c4e225UL, 0x0000000e117838b1UL, 0x00000000aef3a075UL, 0x00000005a0779d4fUL, 0x000000070a3b1d69UL, 0x000000026ccd31fdUL, 0x0000000ed64dd1b2UL, 0x0000000981d4f60cUL, 0x00000006a6e4fb61UL, 0x000000052f15fc93UL, 0x0000000032b3a64dUL, 0x0000000ecb17d667UL, 0x0000000a983fb935UL, 0x000000037d23c88dUL, 0x0000000b8590fbcbUL, 0x0000000ec2f1a277UL, 0x000000090d3053e6UL, 0x0000000a36fa8ccdUL, 0x000000044bd08eccUL, 0x000000061dd197d9UL, 0x0000000a307cfd82UL, 0x00000001d09c2de4UL, 0x00000005f6d74368UL, 0x00000001327d1b2dUL, 0x0000000594cc36b9UL, 0x0000000fea1cba7cUL, 0x000000050c31262dUL, 0x0000000d99b1a6baUL, 0x00000001bf789cd2UL, 0x0000000e2f6f66f9UL, 0x000000013d5edfc6UL, 0x0000000bc3a9ab0cUL, 0x00000001da5b2734UL, 0x000000025ef4f2deUL, 0x0000000dcb55a50aUL, 0x00000009c6dbc6acUL, 0x000000089a838853UL, 0x0000000168f099eeUL, 0x0000000d51601760UL, 0x000000089f324f1aUL, 0x00000002cb1ec1eaUL, 0x00000006306de366UL, 0x0000000012a2f11eUL, 0x0000000b5c0bf797UL, 0x00000005c5f02be4UL, 0x00000005019f54beUL, 0x00000006ae4a096aUL, 0x00000004bce78778UL, 0x000000094b65b97fUL, 0x0000000d3f6e7bd2UL, 0x00000001fbd2a84cUL, 0x00000006d0127ab1UL, 0x00000003e82799aaUL, 0x00000004c1264dfeUL, 0x0000000cf69c9360UL, 0x00000004b43e5342UL, 0x000000035d1f0372UL, 0x0000000d78c18eb4UL, 0x0000000262574101UL, 0x0000000c2c5c7335UL, 0x0000000bad04051aUL, 0x00000001c481f94eUL, 0x00000003285aa0deUL, 0x00000008973e1f69UL, 0x00000005d238c694UL, 0x00000007b71847b9UL, 0x0000000242f5675cUL, 0x0000000cc5751c2dUL, 0x0000000e09bc620bUL, 0x00000000e4e904ddUL, 0x000000007ca4f1a7UL, 0x00000002ac79ae43UL, 0x0000000e213d4250UL, 0x0000000d4137c2b5UL, 0x0000000ddfce11bcUL, 0x0000000d1d658566UL, 0x0000000213f5b1bbUL, 0x0000000cd35be0a8UL, 0x0000000cc67d7f91UL, 0x0000000509bde098UL, 0x000000074d3d8f46UL, 0x000000051309c970UL, 0x000000053e2bdf66UL, 0x0000000a5dd3fed3UL, 0x0000000a4e69b212UL, 0x0000000b1d39936dUL, 0x00000006b6c8926bUL, 0x000000046540a7b0UL, 0x00000002eebc599fUL, 0x00000002e54a283eUL, 0x0000000f9a328a9cUL, 0x00000007ea9cfc53UL, 0x00000005cffa2bdbUL, 0x0000000464d16f8eUL, 0x0000000eb09444bcUL, 0x00000003f341b259UL, 0x00000004d112b108UL, 0x000000070cb94242UL, 0x0000000974ed4ffdUL, 0x00000001084da291UL, 0x000000085673ca39UL, 0x0000000d4d74766fUL, 0x000000064a68e1deUL, 0x0000000e35630caeUL, 0x00000002073229dbUL, 0x000000063d3a3902UL, 0x000000031598ee06UL, 0x0000000808d61126UL, 0x0000000029957984UL, 0x0000000d4f5f2649UL, 0x00000009ec8a706bUL, 0x0000000349981760UL, 0x0000000c93ab23a6UL, 0x00000002c7aa80daUL, 0x0000000866f102baUL, 0x0000000b15cff7bcUL, 0x000000066a13a4caUL, 0x000000054a755048UL, 0x0000000d13fdb8d9UL, 0x000000016ad5edf3UL, 0x0000000e043bb154UL, 0x0000000cc8755671UL, 0x0000000cf9b2bfd5UL, 0x00000003608890b4UL, 0x0000000330fef315UL, 0x0000000e3299ca65UL, 0x00000000b60765e1UL, 0x00000000e9bb17dcUL, 0x000000095f474d8bUL, 0x0000000e721d3d00UL, 0x0000000d4679e565UL, 0x0000000c80da6113UL, 0x000000098deeff30UL, 0x0000000c293bb871UL, 0x0000000e79132f48UL, 0x0000000b152dafbbUL, 0x000000055f6a4386UL, 0x0000000a1b8a4044UL, 0x00000004f4187b05UL, 0x00000000b17c2ed3UL, 0x000000095d75ba04UL, 0x0000000bbf12e96dUL, 0x00000006abd1a52fUL, 0x0000000f300bc991UL, 0x0000000f0a7385d4UL, 0x000000052964f82aUL, 0x0000000a9962925fUL, 0x0000000613b2eef1UL, 0x00000005fd2c92a8UL, 0x000000009ebecd05UL, 0x000000036002b87aUL, 0x0000000902c79eefUL, 0x0000000394e63c7eUL, 0x0000000133285064UL, 0x0000000f7cfe2d4bUL, 0x00000004f068522cUL, 0x000000096fea1a0fUL, 0x0000000c5a927b13UL, 0x0000000e9a2c1994UL, 0x00000005c53b3803UL, 0x0000000f636b6188UL, 0x0000000007c656e3UL, 0x000000026af1fc5fUL, 0x0000000ec2f40b78UL, 0x0000000faa1921e5UL, 0x00000006137a8b30UL, 0x0000000028674f7bUL, 0x00000003de184e35UL, 0x0000000eeef093e6UL, 0x0000000d44b3dae0UL, 0x0000000bb7ab7d93UL, 0x00000002ae18c956UL, 0x0000000cde492bd6UL, 0x00000001cee0216eUL, 0x0000000f1e5830adUL, 0x000000076f6c3299UL, 0x0000000dea24af84UL, 0x0000000277e75586UL, 0x0000000a17318024UL, 0x00000005c4739486UL, 0x00000005e3de4725UL, 0x00000006f67c9f6dUL, 0x000000025f42791dUL, 0x00000003c54d15b3UL, 0x0000000ef98d9c32UL, 0x000000042f64819dUL, 0x000000016d5fd070UL, 0x000000063cb98d4fUL, 0x000000045a3ad27cUL, 0x00000001b496b0acUL, 0x0000000aa471c42dUL, 0x00000000599346a2UL, 0x00000000dc8d1c2dUL, 0x00000007498928c1UL, 0x0000000ea06e90ffUL, 0x0000000b683baa32UL, 0x0000000f93014e16UL, 0x000000020575d56eUL, 0x0000000794325589UL, 0x00000001533e9935UL, 0x000000086b8bcb70UL, 0x0000000ce11faf5dUL, 0x000000036c0bd318UL, 0x0000000e5e8c1167UL, 0x0000000e1831ba64UL, 0x0000000e088dbfa4UL, 0x0000000984479674UL, 0x0000000afef02b29UL, 0x000000048518c716UL, 0x00000004301564ceUL, 0x000000021cc88710UL, 0x0000000d5c995278UL, 0x0000000d8367de1cUL, 0x00000004a51125e8UL, 0x0000000113e1c226UL, 0x0000000ef141e076UL, 0x000000044097011dUL, 0x00000004ca9d707cUL, 0x000000040d8831f1UL, 0x0000000bd9c3b1d8UL, 0x0000000978364177UL, 0x000000010f7606a9UL, 0x000000046a64270aUL, 0x000000042df1b22bUL, 0x0000000e906cf2a0UL, 0x0000000997da6fa5UL, 0x0000000a5722c26fUL, 0x0000000b14f58aaaUL, 0x0000000afc167ad8UL, 0x000000037be56e60UL, 0x0000000de7f80d62UL, 0x00000000c3fb0a64UL, 0x0000000ce8ca802cUL, 0x000000035032ed9dUL, 0x0000000aa8ba3ee6UL, 0x000000094b2e707cUL, 0x00000002debbdae1UL, 0x0000000f53e25fcfUL, 0x0000000e935543ebUL, 0x00000001462f0e90UL, 0x000000054ce7d18cUL, 0x00000002ddafdc5fUL, 0x0000000700565deeUL, 0x0000000fd408e0afUL, 0x000000017d089decUL, 0x0000000833ea2459UL, 0x00000003c8d3776aUL, 0x00000002e5eebac8UL, 0x000000020cbf49b0UL, 0x0000000c44675eb7UL, 0x00000003a4b6beb1UL, 0x0000000ce6f37c1eUL, 0x000000063fba2e7cUL, 0x00000005a05b553dUL, 0x00000001286445b0UL, 0x00000005e07a9b61UL, 0x00000007d8397ea4UL, 0x00000008084b7bbbUL, 0x0000000b05b38097UL, 0x000000029c3019eeUL, 0x0000000ed1d2708bUL, 0x00000009df8a4d47UL, 0x0000000e4891e436UL, 0x00000002a762ab72UL, 0x000000092f70600fUL, 0x000000092329a2cdUL, 0x00000003e200c6edUL, 0x00000008c0a7233eUL, 0x000000060866806aUL, 0x0000000f4fddd24aUL, 0x0000000f78464c71UL, 0x00000009c3d22242UL, 0x00000003877ea6d1UL, 0x0000000e2a6d54acUL, 0x0000000497d2a5e7UL, 0x0000000ca82f781eUL, 0x0000000481524f4cUL, 0x0000000dee088814UL, 0x0000000b2a82d3a4UL, 0x00000008e6afe6e5UL, 0x0000000d6279a5daUL, 0x00000004567cbc1aUL, 0x00000005bec2b2fdUL, 0x00000004ef452505UL, 0x000000061d992cbaUL, 0x0000000ab96be0cbUL, 0x0000000708ef35d9UL, 0x0000000b3f6f3623UL, 0x000000036eb1801dUL, 0x0000000badfee917UL, 0x0000000a3db13cd0UL, 0x00000001d1a12828UL, 0x00000002500816ceUL, 0x0000000cf7612148UL, 0x00000000be6a3f4bUL, 0x000000074142f3daUL, 0x0000000ce5deed92UL, 0x0000000f9530a786UL, 0x0000000047c8bb38UL, 0x0000000fcabfe88fUL, 0x0000000bc83accb1UL, 0x000000020cd9fb1fUL, 0x0000000023dcceb3UL, 0x00000009e969b8c4UL, 0x00000006e28de934UL, 0x000000080a399667UL, 0x000000076a0b85adUL, 0x000000021a84be3cUL, 0x0000000a28d028b5UL, 0x0000000c4e7690dfUL, 0x0000000bfd9621e8UL, 0x00000006f4bc0c24UL, 0x0000000aa8e76bd7UL, 0x0000000deb55dac9UL, 0x0000000bb344fa8bUL, 0x0000000fcaab4decUL, 0x0000000146aba6cbUL, 0x0000000f49ed6eb8UL, 0x0000000dd57e9deaUL, 0x0000000225d5d090UL, 0x0000000d6e86c1c5UL, 0x0000000639be5f39UL, 0x0000000f5e7a6132UL, 0x0000000d2968b09fUL, 0x000000082b30ba1eUL, 0x0000000803fa46ccUL, 0x0000000c290fab00UL, 0x000000010df59de5UL, 0x000000051ae9dcfbUL, 0x000000049af8516dUL, 0x000000002b564ce6UL, 0x0000000c615a1de0UL, 0x0000000fef9864a4UL, 0x0000000c16e27341UL, 0x000000039e846736UL, 0x00000001ecbb6746UL, 0x0000000588d03a7cUL, 0x000000010a0eaf9cUL, 0x0000000671ccea6bUL, 0x000000033a154603UL, 0x0000000a7b003bc1UL, 0x0000000c5fc3848dUL, 0x000000078e50a9c7UL, 0x000000017dbfb88eUL, 0x00000004fd0ed541UL, 0x000000084221debaUL, 0x00000003132cf7e6UL, 0x0000000b67e7ac53UL, 0x0000000df6b28024UL, 0x0000000785b9f7edUL, 0x0000000e3d35320dUL, 0x0000000159c06583UL, 0x00000005c54a80a3UL, 0x0000000ed4d4533bUL, 0x0000000cf16c601aUL, 0x00000005e94efbd1UL, 0x00000005d587126eUL, 0x0000000eef2f2807UL, 0x000000009f3c558eUL, 0x0000000736cfd539UL, 0x0000000f5a922ae1UL, 0x00000004e2ab9959UL, 0x00000006a2dd34e7UL, 0x00000008c9d30d23UL, 0x0000000eba20b791UL, 0x0000000d5c5095e3UL, 0x0000000423d75a82UL, 0x000000040cebaafeUL, 0x000000065e08d288UL, 0x00000002e4f6d767UL, 0x0000000fe10d2f21UL, 0x0000000110347bdaUL, 0x0000000e43a9bfb3UL, 0x0000000cdea483ccUL, 0x0000000fb1e2d8c6UL, 0x0000000d8a0af7a7UL, 0x000000037d05b182UL, 0x00000008d1241d83UL, 0x0000000da1ea7b6eUL, 0x000000065bea93dbUL, 0x00000002a02f8753UL, 0x0000000454243289UL, 0x00000004150bc5a2UL, 0x0000000bbabe5911UL, 0x00000004cbcdbc59UL, 0x0000000f0e61340bUL, 0x000000030a2cdea8UL, 0x00000005daecb091UL, 0x00000005dc93d891UL, 0x0000000c501b4051UL, 0x0000000782cfba78UL, 0x00000004c191b61eUL, 0x0000000b7e27ef35UL, 0x000000005a476838UL, 0x00000009b0209574UL, 0x0000000a775164cfUL, 0x0000000d33d21701UL, 0x00000003afcb7d45UL, 0x00000004df2035cdUL, 0x0000000498819a21UL, 0x0000000293f9e506UL, 0x00000009a35ff1c8UL, 0x0000000c090ebe6bUL, 0x0000000a4f0551d4UL, 0x00000005dc0dc194UL, 0x00000001388aeb31UL, 0x0000000340b27bf4UL, 0x00000003a0f320abUL, 0x00000000996be75dUL, 0x0000000b257ecf39UL, 0x000000078d86f2f1UL, 0x0000000673f5ff91UL, 0x00000004538d7e3eUL, 0x0000000de5bc4369UL } }; #endif // IMLIB_ENABLE_APRILTAGS_TAG36H10 //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "tag36h11" //////////////////////////////////////////////////////////////////////////////////////////////////// #ifdef IMLIB_ENABLE_APRILTAGS_TAG36H11 const apriltag_family_t tag36h11 = { .ncodes = 587, .black_border = 1, .d = 6, .h = 11, .codes = { 0x0000000d5d628584UL, 0x0000000d97f18b49UL, 0x0000000dd280910eUL, 0x0000000e479e9c98UL, 0x0000000ebcbca822UL, 0x0000000f31dab3acUL, 0x0000000056a5d085UL, 0x000000010652e1d4UL, 0x000000022b1dfeadUL, 0x0000000265ad0472UL, 0x000000034fe91b86UL, 0x00000003ff962cd5UL, 0x000000043a25329aUL, 0x0000000474b4385fUL, 0x00000004e9d243e9UL, 0x00000005246149aeUL, 0x00000005997f5538UL, 0x0000000683bb6c4cUL, 0x00000006be4a7211UL, 0x00000007e3158eeaUL, 0x000000081da494afUL, 0x0000000858339a74UL, 0x00000008cd51a5feUL, 0x00000009f21cc2d7UL, 0x0000000a2cabc89cUL, 0x0000000adc58d9ebUL, 0x0000000b16e7dfb0UL, 0x0000000b8c05eb3aUL, 0x0000000d25ef139dUL, 0x0000000d607e1962UL, 0x0000000e4aba3076UL, 0x00000002dde6a3daUL, 0x000000043d40c678UL, 0x00000005620be351UL, 0x000000064c47fa65UL, 0x0000000686d7002aUL, 0x00000006c16605efUL, 0x00000006fbf50bb4UL, 0x00000008d06d39dcUL, 0x00000009f53856b5UL, 0x0000000adf746dc9UL, 0x0000000bc9b084ddUL, 0x0000000d290aa77bUL, 0x0000000d9e28b305UL, 0x0000000e4dd5c454UL, 0x0000000fad2fe6f2UL, 0x0000000181a8151aUL, 0x000000026be42c2eUL, 0x00000002e10237b8UL, 0x0000000405cd5491UL, 0x00000007742eab1cUL, 0x000000085e6ac230UL, 0x00000008d388cdbaUL, 0x00000009f853ea93UL, 0x0000000c41ea2445UL, 0x0000000cf1973594UL, 0x000000014a34a333UL, 0x000000031eacd15bUL, 0x00000006c79d2dabUL, 0x000000073cbb3935UL, 0x000000089c155bd3UL, 0x00000008d6a46198UL, 0x000000091133675dUL, 0x0000000a708d89fbUL, 0x0000000ae5ab9585UL, 0x0000000b9558a6d4UL, 0x0000000b98743ab2UL, 0x0000000d6cec68daUL, 0x00000001506bcaefUL, 0x00000004becd217aUL, 0x00000004f95c273fUL, 0x0000000658b649ddUL, 0x0000000a76c4b1b7UL, 0x0000000ecf621f56UL, 0x00000001c8a56a57UL, 0x00000003628e92baUL, 0x000000053706c0e2UL, 0x00000005e6b3d231UL, 0x00000007809cfa94UL, 0x0000000e97eead6fUL, 0x00000005af40604aUL, 0x00000007492988adUL, 0x0000000ed5994712UL, 0x00000005eceaf9edUL, 0x00000007c1632815UL, 0x0000000c1a0095b4UL, 0x0000000e9e25d52bUL, 0x00000003a6705419UL, 0x0000000a8333012fUL, 0x00000004ce5704d0UL, 0x0000000508e60a95UL, 0x0000000877476120UL, 0x0000000a864e950dUL, 0x0000000ea45cfce7UL, 0x000000019da047e8UL, 0x000000024d4d5937UL, 0x00000006e079cc9bUL, 0x000000099f2e11d7UL, 0x000000033aa50429UL, 0x0000000499ff26c7UL, 0x000000050f1d3251UL, 0x000000066e7754efUL, 0x000000096ad633ceUL, 0x00000009a5653993UL, 0x0000000aca30566cUL, 0x0000000c298a790aUL, 0x00000008be44b65dUL, 0x0000000dc68f354bUL, 0x000000016f7f919bUL, 0x00000004dde0e826UL, 0x0000000d548cbd9fUL, 0x0000000e0439ceeeUL, 0x0000000fd8b1fd16UL, 0x000000076521bb7bUL, 0x0000000d92375742UL, 0x0000000cab16d40cUL, 0x0000000730c9dd72UL, 0x0000000ad9ba39c2UL, 0x0000000b14493f87UL, 0x000000052b15651fUL, 0x0000000185409cadUL, 0x000000077ae2c68dUL, 0x000000094f5af4b5UL, 0x00000000a13bad55UL, 0x000000061ea437cdUL, 0x0000000a022399e2UL, 0x0000000203b163d1UL, 0x00000007bba8f40eUL, 0x000000095bc9442dUL, 0x000000041c0b5358UL, 0x00000008e9c6cc81UL, 0x00000000eb549670UL, 0x00000009da3a0b51UL, 0x0000000d832a67a1UL, 0x0000000dcd4350bcUL, 0x00000004aa05fdd2UL, 0x000000060c7bb44eUL, 0x00000004b358b96cUL, 0x0000000067299b45UL, 0x0000000b9c89b5faUL, 0x00000006975acaeaUL, 0x000000062b8f7afaUL, 0x000000033567c3d7UL, 0x0000000bac139950UL, 0x0000000a5927c62aUL, 0x00000005c916e6a4UL, 0x0000000260ecb7d5UL, 0x000000029b7bbd9aUL, 0x0000000903205f26UL, 0x0000000ae72270a4UL, 0x00000003d2ec51a7UL, 0x000000082ea55324UL, 0x000000011a6f3427UL, 0x00000001ca1c4576UL, 0x0000000a40c81aefUL, 0x0000000bddccd730UL, 0x00000000e617561eUL, 0x0000000969317b0fUL, 0x000000067f781364UL, 0x0000000610912f96UL, 0x0000000b2549fdfcUL, 0x000000006e5aaa6bUL, 0x0000000b6c475339UL, 0x0000000c56836a4dUL, 0x0000000844e351ebUL, 0x00000004647f83b4UL, 0x00000000908a04f5UL, 0x00000007f51034c9UL, 0x0000000aee537fcaUL, 0x00000005e92494baUL, 0x0000000d445808f4UL, 0x000000028d68b563UL, 0x000000004d25374bUL, 0x00000002bc065f65UL, 0x000000096dc3ea0cUL, 0x00000004b2ade817UL, 0x000000007c3fd502UL, 0x0000000e768b5cafUL, 0x000000017605cf6cUL, 0x0000000182741ee4UL, 0x000000062846097cUL, 0x000000072b5ebf80UL, 0x0000000263da6e13UL, 0x0000000fa841bcb5UL, 0x00000007e45e8c69UL, 0x0000000653c81fa0UL, 0x00000007443b5e70UL, 0x00000000a5234afdUL, 0x000000074756f24eUL, 0x0000000157ebf02aUL, 0x000000082ef46939UL, 0x000000080d420264UL, 0x00000002aeed3e98UL, 0x0000000b0a1dd4f8UL, 0x0000000b5436be13UL, 0x00000007b7b4b13bUL, 0x00000001ce80d6d3UL, 0x000000016c08427dUL, 0x0000000ee54462ddUL, 0x00000001f7644cceUL, 0x00000009c7b5cc92UL, 0x0000000e369138f8UL, 0x00000005d5a66e91UL, 0x0000000485d62f49UL, 0x0000000e6e819e94UL, 0x0000000b1f340eb5UL, 0x000000009d198ce2UL, 0x0000000d60717437UL, 0x00000000196b856cUL, 0x0000000f0a6173a5UL, 0x000000012c0e1ec6UL, 0x000000062b82d5cfUL, 0x0000000ad154c067UL, 0x0000000ce3778832UL, 0x00000006b0a7b864UL, 0x00000004c7686694UL, 0x00000005058ff3ecUL, 0x0000000d5e21ea23UL, 0x00000009ff4a76eeUL, 0x00000009dd981019UL, 0x00000001bad4d30aUL, 0x0000000c601896d1UL, 0x0000000973439b48UL, 0x00000001ce7431a8UL, 0x000000057a8021d6UL, 0x0000000f9dba96e6UL, 0x000000083a2e4e7cUL, 0x00000008ea585380UL, 0x0000000af6c0e744UL, 0x0000000875b73babUL, 0x0000000da34ca901UL, 0x00000002ab9727efUL, 0x0000000d39f21b9aUL, 0x00000008a10b742fUL, 0x00000005f8952dbaUL, 0x0000000f8da71ab0UL, 0x0000000c25f9df96UL, 0x000000006f8a5d94UL, 0x0000000e42e63e1aUL, 0x0000000b78409d1bUL, 0x0000000792229addUL, 0x00000005acf8c455UL, 0x00000002fc29a9b0UL, 0x0000000ea486237bUL, 0x0000000b0c9685a0UL, 0x00000001ad748a47UL, 0x000000003b4712d5UL, 0x0000000f29216d30UL, 0x00000008dad65e49UL, 0x00000000a2cf09ddUL, 0x00000000b5f174c6UL, 0x0000000e54f57743UL, 0x0000000b9cf54d78UL, 0x00000004a312a88aUL, 0x000000027babc962UL, 0x0000000b86897111UL, 0x0000000f2ff6c116UL, 0x000000082274bd8aUL, 0x000000097023505eUL, 0x000000052d46edd1UL, 0x0000000585c1f538UL, 0x0000000bddd00e43UL, 0x00000005590b74dfUL, 0x0000000729404a1fUL, 0x000000065320855eUL, 0x0000000d3d4b6956UL, 0x00000007ae374f14UL, 0x00000002d7a60e06UL, 0x0000000315cd9b5eUL, 0x0000000fd36b4eacUL, 0x0000000f1df7642bUL, 0x000000055db27726UL, 0x00000008f15ebc19UL, 0x0000000992f8c531UL, 0x000000062dea2a40UL, 0x0000000928275cabUL, 0x000000069c263cb9UL, 0x0000000a774cca9eUL, 0x0000000266b2110eUL, 0x00000001b14acbb8UL, 0x0000000624b8a71bUL, 0x00000001c539406bUL, 0x00000003086d529bUL, 0x00000000111dd66eUL, 0x000000098cd630bfUL, 0x00000008b9d1ffdcUL, 0x000000072b2f61e7UL, 0x00000009ed9d672bUL, 0x000000096cdd15f3UL, 0x00000006366c2504UL, 0x00000006ca9df73aUL, 0x0000000a066d60f0UL, 0x0000000e7a4b8addUL, 0x00000008264647efUL, 0x0000000aa195bf81UL, 0x00000009a3db8244UL, 0x0000000014d2df6aUL, 0x00000000b63265b7UL, 0x00000002f010de73UL, 0x000000097e774986UL, 0x0000000248affc29UL, 0x0000000fb57dcd11UL, 0x00000000b1a7e4d9UL, 0x00000004bfa2d07dUL, 0x000000054e5cdf96UL, 0x00000004c15c1c86UL, 0x0000000cd9c61166UL, 0x0000000499380b2aUL, 0x0000000540308d09UL, 0x00000008b63fe66fUL, 0x0000000c81aeb35eUL, 0x000000086fe0bd5cUL, 0x0000000ce2480c2aUL, 0x00000001ab29ee60UL, 0x00000008048daa15UL, 0x0000000dbfeb2d39UL, 0x0000000567c9858cUL, 0x00000002b6edc5bcUL, 0x00000002078fca82UL, 0x0000000adacc22aaUL, 0x0000000b92486f49UL, 0x000000051fac5964UL, 0x0000000691ee6420UL, 0x0000000f63b3e129UL, 0x000000039be7e572UL, 0x0000000da2ce6c74UL, 0x000000020cf17a5cUL, 0x0000000ee55f9b6eUL, 0x0000000fb8572726UL, 0x0000000b2c2de548UL, 0x0000000caa9bce92UL, 0x0000000ae9182db3UL, 0x000000074b6e5bd1UL, 0x0000000137b252afUL, 0x000000051f686881UL, 0x0000000d672f6c02UL, 0x0000000654146ce4UL, 0x0000000f944bc825UL, 0x0000000e8327f809UL, 0x000000076a73fd59UL, 0x0000000f79da4cb4UL, 0x0000000956f8099bUL, 0x00000007b5f2655cUL, 0x0000000d06b114a6UL, 0x0000000d0697ca50UL, 0x000000027c390797UL, 0x0000000bc61ed9b2UL, 0x0000000cc12dd19bUL, 0x0000000eb7818d2cUL, 0x0000000092fcecdaUL, 0x000000089ded4ea1UL, 0x0000000256a0ba34UL, 0x0000000b6948e627UL, 0x00000001ef6b1054UL, 0x00000008639294a2UL, 0x0000000eda3780a4UL, 0x000000039ee2af1dUL, 0x0000000cd257edc5UL, 0x00000002d9d6bc22UL, 0x0000000121d3b47dUL, 0x000000037e23f8adUL, 0x0000000119f31cf6UL, 0x00000002c97f4f09UL, 0x0000000d502abfe0UL, 0x000000010bc3ca77UL, 0x000000053d7190efUL, 0x000000090c3e62a6UL, 0x00000007e9ebf675UL, 0x0000000979ce23d1UL, 0x000000027f0c98e9UL, 0x0000000eafb4ae59UL, 0x00000007ca7fe2bdUL, 0x00000001490ca8f6UL, 0x00000009123387baUL, 0x0000000b3bc73888UL, 0x00000003ea87e325UL, 0x00000004888964aaUL, 0x0000000a0188a6b9UL, 0x0000000cd383c666UL, 0x000000040029a3fdUL, 0x0000000e1c00ac5cUL, 0x000000039e6f2b6eUL, 0x0000000de664f622UL, 0x0000000e979a75e8UL, 0x00000007c6b4c86cUL, 0x0000000fd492e071UL, 0x00000008fbb35118UL, 0x000000040b4a09b7UL, 0x0000000af80bd6daUL, 0x000000070e0b2521UL, 0x00000002f5c54d93UL, 0x00000003f4a118d5UL, 0x000000009c1897b9UL, 0x0000000079776eacUL, 0x0000000084b00b17UL, 0x00000003a95ad90eUL, 0x000000028c544095UL, 0x000000039d457c05UL, 0x00000007a3791a78UL, 0x0000000bb770e22eUL, 0x00000009a822bd6cUL, 0x000000068a4b1fedUL, 0x0000000a5fd27b3bUL, 0x00000000c3995b79UL, 0x0000000d1519dff1UL, 0x00000008e7eee359UL, 0x0000000cd3ca50b1UL, 0x0000000b73b8b793UL, 0x000000057aca1c43UL, 0x0000000ec2655277UL, 0x0000000785a2c1b3UL, 0x000000075a07985aUL, 0x0000000a4b01eb69UL, 0x0000000a18a11347UL, 0x0000000db1f28ca3UL, 0x0000000877ec3e25UL, 0x000000031f6341b8UL, 0x00000001363a3a4cUL, 0x0000000075d8b9baUL, 0x00000007ae0792a9UL, 0x0000000a83a21651UL, 0x00000007f08f9fb5UL, 0x00000000d0cf73a9UL, 0x0000000b04dcc98eUL, 0x0000000f65c7b0f8UL, 0x000000065ddaf69aUL, 0x00000002cf9b86b3UL, 0x000000014cb51e25UL, 0x0000000f48027b5bUL, 0x00000000ec26ea8bUL, 0x000000044bafd45cUL, 0x0000000b12c7c0c4UL, 0x0000000959fd9d82UL, 0x0000000c77c9725aUL, 0x000000048a22d462UL, 0x00000008398e8072UL, 0x0000000ec89b05ceUL, 0x0000000bb682d4c9UL, 0x0000000e5a86d2ffUL, 0x0000000358f01134UL, 0x00000008556ddcf6UL, 0x000000067584b6e2UL, 0x000000011609439fUL, 0x000000008488816eUL, 0x0000000aaf1a2c46UL, 0x0000000f879898cfUL, 0x00000008bbe5e2f7UL, 0x0000000101eee363UL, 0x0000000690f69377UL, 0x0000000f5bd93cd9UL, 0x0000000cea4c2bf6UL, 0x00000009550be706UL, 0x00000002c5b38a60UL, 0x0000000e72033547UL, 0x00000004458b0629UL, 0x0000000ee8d9ed41UL, 0x0000000d2f918d72UL, 0x000000078dc39fd3UL, 0x00000008212636f6UL, 0x00000007450a72a7UL, 0x0000000c4f0cf4c6UL, 0x0000000367bcddcdUL, 0x0000000c1caf8cc6UL, 0x0000000a7f5b853dUL, 0x00000009d536818bUL, 0x0000000535e021b0UL, 0x0000000a7eb8729eUL, 0x0000000422a67b49UL, 0x0000000929e928a6UL, 0x000000048e8aefccUL, 0x0000000a9897393cUL, 0x00000005eb81d37eUL, 0x00000001e80287b7UL, 0x000000034770d903UL, 0x00000002eef86728UL, 0x000000059266ccb6UL, 0x00000000110bba61UL, 0x00000001dfd284efUL, 0x0000000447439d1bUL, 0x0000000fece0e599UL, 0x00000009309f3703UL, 0x000000080764d1ddUL, 0x0000000353f1e6a0UL, 0x00000002c1c12dccUL, 0x0000000c1d21b9d7UL, 0x0000000457ee453eUL, 0x0000000d66faf540UL, 0x000000044831e652UL, 0x0000000cfd49a848UL, 0x00000009312d4133UL, 0x00000003f097d3eeUL, 0x00000008c9ebef7aUL, 0x0000000a99e29e88UL, 0x00000000e9fab22cUL, 0x00000004e748f4fbUL, 0x0000000ecdee4288UL, 0x0000000abce5f1d0UL, 0x0000000c42f6876cUL, 0x00000007ed402ea0UL, 0x0000000e5c4242c3UL, 0x0000000d5b2c31aeUL, 0x0000000286863be6UL, 0x0000000160444d94UL, 0x00000005f0f5808eUL, 0x0000000ae3d44b2aUL, 0x00000009f5c5d109UL, 0x00000008ad9316d7UL, 0x00000003422ba064UL, 0x00000002fed11d56UL, 0x0000000bea6e3e04UL, 0x000000004b029eecUL, 0x00000006deed7435UL, 0x00000003718ce17cUL, 0x000000055857f5e2UL, 0x00000002edac7b62UL, 0x0000000085d6c512UL, 0x0000000d6ca88e0fUL, 0x00000002b7e1fc69UL, 0x0000000a699d5c1bUL, 0x0000000f05ad74deUL, 0x00000004cf5fb56dUL, 0x00000005725e07e1UL, 0x000000072f18a2deUL, 0x00000001cec52609UL, 0x000000048534243cUL, 0x00000002523a4d69UL, 0x000000035c1b80d1UL, 0x0000000a4d7338a7UL, 0x00000000db1af012UL, 0x0000000e61a9475dUL, 0x000000005df03f91UL, 0x000000097ae260bbUL, 0x000000032d627fefUL, 0x0000000b640f73c2UL, 0x000000045a1ac9c6UL, 0x00000006a2202de1UL, 0x000000057d3e25f2UL, 0x00000005aa9f986eUL, 0x00000000cc859d8aUL, 0x0000000e3ec6cca8UL, 0x000000054e95e1aeUL, 0x0000000446887b06UL, 0x00000007516732beUL, 0x00000003817ac8f5UL, 0x00000003e26d938cUL, 0x0000000aa81bc235UL, 0x0000000df387ca1bUL, 0x00000000f3a3b3f2UL, 0x0000000b4bf69677UL, 0x0000000ae21868edUL, 0x000000081e1d2d9dUL, 0x0000000a0a9ea14cUL, 0x00000008eee297a9UL, 0x00000004740c0559UL, 0x0000000e8b141837UL, 0x0000000ac69e0a3dUL, 0x00000009ed83a1e1UL, 0x00000005edb55ecbUL, 0x000000007340fe81UL, 0x000000050dfbc6bfUL, 0x00000004f583508aUL, 0x0000000cb1fb78bcUL, 0x00000004025ced2fUL, 0x000000039791ebecUL, 0x000000053ee388f1UL, 0x00000007d6c0bd23UL, 0x000000093a995fbeUL, 0x00000008a41728deUL, 0x00000002fe70e053UL, 0x0000000ab3db443aUL, 0x00000001364edb05UL, 0x000000047b6eeed6UL, 0x000000012e71af01UL, 0x000000052ff83587UL, 0x00000003a1575dd8UL, 0x00000003feaa3564UL, 0x0000000eacf78ba7UL, 0x00000000872b94f8UL, 0x0000000da8ddf9a2UL, 0x00000009aa920d2bUL, 0x00000001f350ed36UL, 0x000000018a5e861fUL, 0x00000002c35b89c3UL, 0x00000003347ac48aUL, 0x00000007f23e022eUL, 0x00000002459068fbUL, 0x0000000e83be4b73UL } }; #endif // IMLIB_ENABLE_APRILTAGS_TAG36H11 //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "artoolkit" //////////////////////////////////////////////////////////////////////////////////////////////////// #ifdef IMLIB_ENABLE_APRILTAGS_ARTOOLKIT const apriltag_family_t artoolkit = { .ncodes = 512, .black_border = 1, .d = 6, .h = 7, .codes = { 0x0006dc269c27UL, 0x0006d4229e26UL, 0x0006cc2e9825UL, 0x0006c42a9a24UL, 0x0006fc369423UL, 0x0006f4329622UL, 0x0006ec3e9021UL, 0x0006e43a9220UL, 0x00069c068c2fUL, 0x000694028e2eUL, 0x00068c0e882dUL, 0x0006840a8a2cUL, 0x0006bc16842bUL, 0x0006b412862aUL, 0x0006ac1e8029UL, 0x0006a41a8228UL, 0x00065c66bc37UL, 0x00065462be36UL, 0x00064c6eb835UL, 0x0006446aba34UL, 0x00067c76b433UL, 0x00067472b632UL, 0x00066c7eb031UL, 0x0006647ab230UL, 0x00061c46ac3fUL, 0x00061442ae3eUL, 0x00060c4ea83dUL, 0x0006044aaa3cUL, 0x00063c56a43bUL, 0x00063452a63aUL, 0x00062c5ea039UL, 0x0006245aa238UL, 0x0007dca6dc07UL, 0x0007d4a2de06UL, 0x0007ccaed805UL, 0x0007c4aada04UL, 0x0007fcb6d403UL, 0x0007f4b2d602UL, 0x0007ecbed001UL, 0x0007e4bad200UL, 0x00079c86cc0fUL, 0x00079482ce0eUL, 0x00078c8ec80dUL, 0x0007848aca0cUL, 0x0007bc96c40bUL, 0x0007b492c60aUL, 0x0007ac9ec009UL, 0x0007a49ac208UL, 0x00075ce6fc17UL, 0x000754e2fe16UL, 0x00074ceef815UL, 0x000744eafa14UL, 0x00077cf6f413UL, 0x000774f2f612UL, 0x00076cfef011UL, 0x000764faf210UL, 0x00071cc6ec1fUL, 0x000714c2ee1eUL, 0x00070ccee81dUL, 0x000704caea1cUL, 0x00073cd6e41bUL, 0x000734d2e61aUL, 0x00072cdee019UL, 0x000724dae218UL, 0x0004dd261c67UL, 0x0004d5221e66UL, 0x0004cd2e1865UL, 0x0004c52a1a64UL, 0x0004fd361463UL, 0x0004f5321662UL, 0x0004ed3e1061UL, 0x0004e53a1260UL, 0x00049d060c6fUL, 0x000495020e6eUL, 0x00048d0e086dUL, 0x0004850a0a6cUL, 0x0004bd16046bUL, 0x0004b512066aUL, 0x0004ad1e0069UL, 0x0004a51a0268UL, 0x00045d663c77UL, 0x000455623e76UL, 0x00044d6e3875UL, 0x0004456a3a74UL, 0x00047d763473UL, 0x000475723672UL, 0x00046d7e3071UL, 0x0004657a3270UL, 0x00041d462c7fUL, 0x000415422e7eUL, 0x00040d4e287dUL, 0x0004054a2a7cUL, 0x00043d56247bUL, 0x00043552267aUL, 0x00042d5e2079UL, 0x0004255a2278UL, 0x0005dda65c47UL, 0x0005d5a25e46UL, 0x0005cdae5845UL, 0x0005c5aa5a44UL, 0x0005fdb65443UL, 0x0005f5b25642UL, 0x0005edbe5041UL, 0x0005e5ba5240UL, 0x00059d864c4fUL, 0x000595824e4eUL, 0x00058d8e484dUL, 0x0005858a4a4cUL, 0x0005bd96444bUL, 0x0005b592464aUL, 0x0005ad9e4049UL, 0x0005a59a4248UL, 0x00055de67c57UL, 0x000555e27e56UL, 0x00054dee7855UL, 0x000545ea7a54UL, 0x00057df67453UL, 0x000575f27652UL, 0x00056dfe7051UL, 0x000565fa7250UL, 0x00051dc66c5fUL, 0x000515c26e5eUL, 0x00050dce685dUL, 0x000505ca6a5cUL, 0x00053dd6645bUL, 0x000535d2665aUL, 0x00052dde6059UL, 0x000525da6258UL, 0x0002de279ca7UL, 0x0002d6239ea6UL, 0x0002ce2f98a5UL, 0x0002c62b9aa4UL, 0x0002fe3794a3UL, 0x0002f63396a2UL, 0x0002ee3f90a1UL, 0x0002e63b92a0UL, 0x00029e078cafUL, 0x000296038eaeUL, 0x00028e0f88adUL, 0x0002860b8aacUL, 0x0002be1784abUL, 0x0002b61386aaUL, 0x0002ae1f80a9UL, 0x0002a61b82a8UL, 0x00025e67bcb7UL, 0x00025663beb6UL, 0x00024e6fb8b5UL, 0x0002466bbab4UL, 0x00027e77b4b3UL, 0x00027673b6b2UL, 0x00026e7fb0b1UL, 0x0002667bb2b0UL, 0x00021e47acbfUL, 0x00021643aebeUL, 0x00020e4fa8bdUL, 0x0002064baabcUL, 0x00023e57a4bbUL, 0x00023653a6baUL, 0x00022e5fa0b9UL, 0x0002265ba2b8UL, 0x0003dea7dc87UL, 0x0003d6a3de86UL, 0x0003ceafd885UL, 0x0003c6abda84UL, 0x0003feb7d483UL, 0x0003f6b3d682UL, 0x0003eebfd081UL, 0x0003e6bbd280UL, 0x00039e87cc8fUL, 0x00039683ce8eUL, 0x00038e8fc88dUL, 0x0003868bca8cUL, 0x0003be97c48bUL, 0x0003b693c68aUL, 0x0003ae9fc089UL, 0x0003a69bc288UL, 0x00035ee7fc97UL, 0x000356e3fe96UL, 0x00034eeff895UL, 0x000346ebfa94UL, 0x00037ef7f493UL, 0x000376f3f692UL, 0x00036efff091UL, 0x000366fbf290UL, 0x00031ec7ec9fUL, 0x000316c3ee9eUL, 0x00030ecfe89dUL, 0x000306cbea9cUL, 0x00033ed7e49bUL, 0x000336d3e69aUL, 0x00032edfe099UL, 0x000326dbe298UL, 0x0000df271ce7UL, 0x0000d7231ee6UL, 0x0000cf2f18e5UL, 0x0000c72b1ae4UL, 0x0000ff3714e3UL, 0x0000f73316e2UL, 0x0000ef3f10e1UL, 0x0000e73b12e0UL, 0x00009f070cefUL, 0x000097030eeeUL, 0x00008f0f08edUL, 0x0000870b0aecUL, 0x0000bf1704ebUL, 0x0000b71306eaUL, 0x0000af1f00e9UL, 0x0000a71b02e8UL, 0x00005f673cf7UL, 0x000057633ef6UL, 0x00004f6f38f5UL, 0x0000476b3af4UL, 0x00007f7734f3UL, 0x0000777336f2UL, 0x00006f7f30f1UL, 0x0000677b32f0UL, 0x00001f472cffUL, 0x000017432efeUL, 0x00000f4f28fdUL, 0x0000074b2afcUL, 0x00003f5724fbUL, 0x0000375326faUL, 0x00002f5f20f9UL, 0x0000275b22f8UL, 0x0001dfa75cc7UL, 0x0001d7a35ec6UL, 0x0001cfaf58c5UL, 0x0001c7ab5ac4UL, 0x0001ffb754c3UL, 0x0001f7b356c2UL, 0x0001efbf50c1UL, 0x0001e7bb52c0UL, 0x00019f874ccfUL, 0x000197834eceUL, 0x00018f8f48cdUL, 0x0001878b4accUL, 0x0001bf9744cbUL, 0x0001b79346caUL, 0x0001af9f40c9UL, 0x0001a79b42c8UL, 0x00015fe77cd7UL, 0x000157e37ed6UL, 0x00014fef78d5UL, 0x000147eb7ad4UL, 0x00017ff774d3UL, 0x000177f376d2UL, 0x00016fff70d1UL, 0x000167fb72d0UL, 0x00011fc76cdfUL, 0x000117c36edeUL, 0x00010fcf68ddUL, 0x000107cb6adcUL, 0x00013fd764dbUL, 0x000137d366daUL, 0x00012fdf60d9UL, 0x000127db62d8UL, 0x000ed8249d27UL, 0x000ed0209f26UL, 0x000ec82c9925UL, 0x000ec0289b24UL, 0x000ef8349523UL, 0x000ef0309722UL, 0x000ee83c9121UL, 0x000ee0389320UL, 0x000e98048d2fUL, 0x000e90008f2eUL, 0x000e880c892dUL, 0x000e80088b2cUL, 0x000eb814852bUL, 0x000eb010872aUL, 0x000ea81c8129UL, 0x000ea0188328UL, 0x000e5864bd37UL, 0x000e5060bf36UL, 0x000e486cb935UL, 0x000e4068bb34UL, 0x000e7874b533UL, 0x000e7070b732UL, 0x000e687cb131UL, 0x000e6078b330UL, 0x000e1844ad3fUL, 0x000e1040af3eUL, 0x000e084ca93dUL, 0x000e0048ab3cUL, 0x000e3854a53bUL, 0x000e3050a73aUL, 0x000e285ca139UL, 0x000e2058a338UL, 0x000fd8a4dd07UL, 0x000fd0a0df06UL, 0x000fc8acd905UL, 0x000fc0a8db04UL, 0x000ff8b4d503UL, 0x000ff0b0d702UL, 0x000fe8bcd101UL, 0x000fe0b8d300UL, 0x000f9884cd0fUL, 0x000f9080cf0eUL, 0x000f888cc90dUL, 0x000f8088cb0cUL, 0x000fb894c50bUL, 0x000fb090c70aUL, 0x000fa89cc109UL, 0x000fa098c308UL, 0x000f58e4fd17UL, 0x000f50e0ff16UL, 0x000f48ecf915UL, 0x000f40e8fb14UL, 0x000f78f4f513UL, 0x000f70f0f712UL, 0x000f68fcf111UL, 0x000f60f8f310UL, 0x000f18c4ed1fUL, 0x000f10c0ef1eUL, 0x000f08cce91dUL, 0x000f00c8eb1cUL, 0x000f38d4e51bUL, 0x000f30d0e71aUL, 0x000f28dce119UL, 0x000f20d8e318UL, 0x000cd9241d67UL, 0x000cd1201f66UL, 0x000cc92c1965UL, 0x000cc1281b64UL, 0x000cf9341563UL, 0x000cf1301762UL, 0x000ce93c1161UL, 0x000ce1381360UL, 0x000c99040d6fUL, 0x000c91000f6eUL, 0x000c890c096dUL, 0x000c81080b6cUL, 0x000cb914056bUL, 0x000cb110076aUL, 0x000ca91c0169UL, 0x000ca1180368UL, 0x000c59643d77UL, 0x000c51603f76UL, 0x000c496c3975UL, 0x000c41683b74UL, 0x000c79743573UL, 0x000c71703772UL, 0x000c697c3171UL, 0x000c61783370UL, 0x000c19442d7fUL, 0x000c11402f7eUL, 0x000c094c297dUL, 0x000c01482b7cUL, 0x000c3954257bUL, 0x000c3150277aUL, 0x000c295c2179UL, 0x000c21582378UL, 0x000dd9a45d47UL, 0x000dd1a05f46UL, 0x000dc9ac5945UL, 0x000dc1a85b44UL, 0x000df9b45543UL, 0x000df1b05742UL, 0x000de9bc5141UL, 0x000de1b85340UL, 0x000d99844d4fUL, 0x000d91804f4eUL, 0x000d898c494dUL, 0x000d81884b4cUL, 0x000db994454bUL, 0x000db190474aUL, 0x000da99c4149UL, 0x000da1984348UL, 0x000d59e47d57UL, 0x000d51e07f56UL, 0x000d49ec7955UL, 0x000d41e87b54UL, 0x000d79f47553UL, 0x000d71f07752UL, 0x000d69fc7151UL, 0x000d61f87350UL, 0x000d19c46d5fUL, 0x000d11c06f5eUL, 0x000d09cc695dUL, 0x000d01c86b5cUL, 0x000d39d4655bUL, 0x000d31d0675aUL, 0x000d29dc6159UL, 0x000d21d86358UL, 0x000ada259da7UL, 0x000ad2219fa6UL, 0x000aca2d99a5UL, 0x000ac2299ba4UL, 0x000afa3595a3UL, 0x000af23197a2UL, 0x000aea3d91a1UL, 0x000ae23993a0UL, 0x000a9a058dafUL, 0x000a92018faeUL, 0x000a8a0d89adUL, 0x000a82098bacUL, 0x000aba1585abUL, 0x000ab21187aaUL, 0x000aaa1d81a9UL, 0x000aa21983a8UL, 0x000a5a65bdb7UL, 0x000a5261bfb6UL, 0x000a4a6db9b5UL, 0x000a4269bbb4UL, 0x000a7a75b5b3UL, 0x000a7271b7b2UL, 0x000a6a7db1b1UL, 0x000a6279b3b0UL, 0x000a1a45adbfUL, 0x000a1241afbeUL, 0x000a0a4da9bdUL, 0x000a0249abbcUL, 0x000a3a55a5bbUL, 0x000a3251a7baUL, 0x000a2a5da1b9UL, 0x000a2259a3b8UL, 0x000bdaa5dd87UL, 0x000bd2a1df86UL, 0x000bcaadd985UL, 0x000bc2a9db84UL, 0x000bfab5d583UL, 0x000bf2b1d782UL, 0x000beabdd181UL, 0x000be2b9d380UL, 0x000b9a85cd8fUL, 0x000b9281cf8eUL, 0x000b8a8dc98dUL, 0x000b8289cb8cUL, 0x000bba95c58bUL, 0x000bb291c78aUL, 0x000baa9dc189UL, 0x000ba299c388UL, 0x000b5ae5fd97UL, 0x000b52e1ff96UL, 0x000b4aedf995UL, 0x000b42e9fb94UL, 0x000b7af5f593UL, 0x000b72f1f792UL, 0x000b6afdf191UL, 0x000b62f9f390UL, 0x000b1ac5ed9fUL, 0x000b12c1ef9eUL, 0x000b0acde99dUL, 0x000b02c9eb9cUL, 0x000b3ad5e59bUL, 0x000b32d1e79aUL, 0x000b2adde199UL, 0x000b22d9e398UL, 0x0008db251de7UL, 0x0008d3211fe6UL, 0x0008cb2d19e5UL, 0x0008c3291be4UL, 0x0008fb3515e3UL, 0x0008f33117e2UL, 0x0008eb3d11e1UL, 0x0008e33913e0UL, 0x00089b050defUL, 0x000893010feeUL, 0x00088b0d09edUL, 0x000883090becUL, 0x0008bb1505ebUL, 0x0008b31107eaUL, 0x0008ab1d01e9UL, 0x0008a31903e8UL, 0x00085b653df7UL, 0x000853613ff6UL, 0x00084b6d39f5UL, 0x000843693bf4UL, 0x00087b7535f3UL, 0x0008737137f2UL, 0x00086b7d31f1UL, 0x0008637933f0UL, 0x00081b452dffUL, 0x000813412ffeUL, 0x00080b4d29fdUL, 0x000803492bfcUL, 0x00083b5525fbUL, 0x0008335127faUL, 0x00082b5d21f9UL, 0x0008235923f8UL, 0x0009dba55dc7UL, 0x0009d3a15fc6UL, 0x0009cbad59c5UL, 0x0009c3a95bc4UL, 0x0009fbb555c3UL, 0x0009f3b157c2UL, 0x0009ebbd51c1UL, 0x0009e3b953c0UL, 0x00099b854dcfUL, 0x000993814fceUL, 0x00098b8d49cdUL, 0x000983894bccUL, 0x0009bb9545cbUL, 0x0009b39147caUL, 0x0009ab9d41c9UL, 0x0009a39943c8UL, 0x00095be57dd7UL, 0x000953e17fd6UL, 0x00094bed79d5UL, 0x000943e97bd4UL, 0x00097bf575d3UL, 0x000973f177d2UL, 0x00096bfd71d1UL, 0x000963f973d0UL, 0x00091bc56ddfUL, 0x000913c16fdeUL, 0x00090bcd69ddUL, 0x000903c96bdcUL, 0x00093bd565dbUL, 0x000933d167daUL, 0x00092bdd61d9UL, 0x000923d963d8UL } }; #endif // IMLIB_ENABLE_APRILTAGS_ARTOOLKIT //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "union_find.h" //////////////////////////////////////////////////////////////////////////////////////////////////// typedef struct unionfind unionfind_t; struct unionfind { struct ufrec *data; }; struct ufrec { // the parent of this node. If a node's parent is its own index, // then it is a root. #ifdef IMLIB_ENABLE_HIGH_RES_APRILTAGS uint32_t parent; #else uint16_t parent; #endif }; static inline unionfind_t *unionfind_create(uint32_t maxid) { unionfind_t *uf = (unionfind_t*) fb_alloc(sizeof(unionfind_t), FB_ALLOC_NO_HINT); uf->data = (struct ufrec*) fb_alloc((maxid+1) * sizeof(struct ufrec), FB_ALLOC_NO_HINT); for (int i = 0; i <= maxid; i++) { uf->data[i].parent = i; } return uf; } static inline void unionfind_destroy() { fb_free(); fb_free(); } /* static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id) { // base case: a node is its own parent if (uf->data[id].parent == id) return id; // otherwise, recurse uint32_t root = unionfind_get_representative(uf, uf->data[id].parent); // short circuit the path. [XXX This write prevents tail recursion] uf->data[id].parent = root; return root; } */ // this one seems to be every-so-slightly faster than the recursive // version above. static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id) { uint32_t root = id; // chase down the root while (uf->data[root].parent != root) { root = uf->data[root].parent; } // go back and collapse the tree. // // XXX: on some of our workloads that have very shallow trees // (e.g. image segmentation), we are actually faster not doing // this... while (uf->data[id].parent != root) { uint32_t tmp = uf->data[id].parent; uf->data[id].parent = root; id = tmp; } return root; } static inline uint32_t unionfind_connect(unionfind_t *uf, uint32_t aid, uint32_t bid) { uint32_t aroot = unionfind_get_representative(uf, aid); uint32_t broot = unionfind_get_representative(uf, bid); if (aroot != broot) uf->data[broot].parent = aroot; return aroot; } //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "union_find.c" //////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "apriltag_quad_thresh.c" //////////////////////////////////////////////////////////////////////////////////////////////////// // limitation: image size must be <32768 in width and height. This is // because we use a fixed-point 16 bit integer representation with one // fractional bit. static inline uint32_t u64hash_2(uint64_t x) { return (2654435761 * x) >> 32; return (uint32_t) x; } struct uint32_zarray_entry { uint32_t id; zarray_t *cluster; struct uint32_zarray_entry *next; }; #ifndef M_PI # define M_PI 3.141592653589793238462643383279502884196 #endif struct pt { // Note: these represent 2*actual value. uint16_t x, y; float theta; int16_t gx, gy; }; struct remove_vertex { int i; // which vertex to remove? int left, right; // left vertex, right vertex float err; }; struct segment { int is_vertex; // always greater than zero, but right can be > size, which denotes // a wrap around back to the beginning of the points. and left < right. int left, right; }; struct line_fit_pt { float Mx, My; float Mxx, Myy, Mxy; float W; // total weight }; static inline void ptsort(struct pt *pts, int sz) { #define MAYBE_SWAP(arr,apos,bpos) \ if (arr[apos].theta > arr[bpos].theta) { \ tmp = arr[apos]; arr[apos] = arr[bpos]; arr[bpos] = tmp; \ }; if (sz <= 1) return; if (sz == 2) { struct pt tmp; MAYBE_SWAP(pts, 0, 1); return; } // NB: Using less-branch-intensive sorting networks here on the // hunch that it's better for performance. if (sz == 3) { // 3 element bubble sort is optimal struct pt tmp; MAYBE_SWAP(pts, 0, 1); MAYBE_SWAP(pts, 1, 2); MAYBE_SWAP(pts, 0, 1); return; } if (sz == 4) { // 4 element optimal sorting network. struct pt tmp; MAYBE_SWAP(pts, 0, 1); // sort each half, like a merge sort MAYBE_SWAP(pts, 2, 3); MAYBE_SWAP(pts, 0, 2); // minimum value is now at 0. MAYBE_SWAP(pts, 1, 3); // maximum value is now at end. MAYBE_SWAP(pts, 1, 2); // that only leaves the middle two. return; } if (sz == 5) { // this 9-step swap is optimal for a sorting network, but two // steps slower than a generic sort. struct pt tmp; MAYBE_SWAP(pts, 0, 1); // sort each half (3+2), like a merge sort MAYBE_SWAP(pts, 3, 4); MAYBE_SWAP(pts, 1, 2); MAYBE_SWAP(pts, 0, 1); MAYBE_SWAP(pts, 0, 3); // minimum element now at 0 MAYBE_SWAP(pts, 2, 4); // maximum element now at end MAYBE_SWAP(pts, 1, 2); // now resort the three elements 1-3. MAYBE_SWAP(pts, 2, 3); MAYBE_SWAP(pts, 1, 2); return; } #undef MAYBE_SWAP // a merge sort with temp storage. struct pt *tmp = fb_alloc(sizeof(struct pt) * sz, FB_ALLOC_NO_HINT); memcpy(tmp, pts, sizeof(struct pt) * sz); int asz = sz/2; int bsz = sz - asz; struct pt *as = &tmp[0]; struct pt *bs = &tmp[asz]; ptsort(as, asz); ptsort(bs, bsz); #define MERGE(apos,bpos) \ if (as[apos].theta < bs[bpos].theta) \ pts[outpos++] = as[apos++]; \ else \ pts[outpos++] = bs[bpos++]; int apos = 0, bpos = 0, outpos = 0; while (apos + 8 < asz && bpos + 8 < bsz) { MERGE(apos,bpos); MERGE(apos,bpos); MERGE(apos,bpos); MERGE(apos,bpos); MERGE(apos,bpos); MERGE(apos,bpos); MERGE(apos,bpos); MERGE(apos,bpos); } while (apos < asz && bpos < bsz) { MERGE(apos,bpos); } if (apos < asz) memcpy(&pts[outpos], &as[apos], (asz-apos)*sizeof(struct pt)); if (bpos < bsz) memcpy(&pts[outpos], &bs[bpos], (bsz-bpos)*sizeof(struct pt)); fb_free(); // tmp #undef MERGE } // lfps contains *cumulative* moments for N points, with // index j reflecting points [0,j] (inclusive). // // fit a line to the points [i0, i1] (inclusive). i0, i1 are both [0, // sz) if i1 < i0, we treat this as a wrap around. void fit_line(struct line_fit_pt *lfps, int sz, int i0, int i1, float *lineparm, float *err, float *mse) { assert(i0 != i1); assert(i0 >= 0 && i1 >= 0 && i0 < sz && i1 < sz); float Mx, My, Mxx, Myy, Mxy, W; int N; // how many points are included in the set? if (i0 < i1) { N = i1 - i0 + 1; Mx = lfps[i1].Mx; My = lfps[i1].My; Mxx = lfps[i1].Mxx; Mxy = lfps[i1].Mxy; Myy = lfps[i1].Myy; W = lfps[i1].W; if (i0 > 0) { Mx -= lfps[i0-1].Mx; My -= lfps[i0-1].My; Mxx -= lfps[i0-1].Mxx; Mxy -= lfps[i0-1].Mxy; Myy -= lfps[i0-1].Myy; W -= lfps[i0-1].W; } } else { // i0 > i1, e.g. [15, 2]. Wrap around. assert(i0 > 0); Mx = lfps[sz-1].Mx - lfps[i0-1].Mx; My = lfps[sz-1].My - lfps[i0-1].My; Mxx = lfps[sz-1].Mxx - lfps[i0-1].Mxx; Mxy = lfps[sz-1].Mxy - lfps[i0-1].Mxy; Myy = lfps[sz-1].Myy - lfps[i0-1].Myy; W = lfps[sz-1].W - lfps[i0-1].W; Mx += lfps[i1].Mx; My += lfps[i1].My; Mxx += lfps[i1].Mxx; Mxy += lfps[i1].Mxy; Myy += lfps[i1].Myy; W += lfps[i1].W; N = sz - i0 + i1 + 1; } assert(N >= 2); float Ex = Mx / W; float Ey = My / W; float Cxx = Mxx / W - Ex*Ex; float Cxy = Mxy / W - Ex*Ey; float Cyy = Myy / W - Ey*Ey; float nx, ny; if (1) { // on iOS about 5% of total CPU spent in these trig functions. // 85 ms per frame on 5S, example.pnm // // XXX this was using the float-precision atan2. Was there a case where // we needed that precision? Seems doubtful. float normal_theta = .5 * atan2f(-2*Cxy, (Cyy - Cxx)); nx = cosf(normal_theta); ny = sinf(normal_theta); } else { // 73.5 ms per frame on 5S, example.pnm float ty = -2*Cxy; float tx = (Cyy - Cxx); float mag = ty*ty + tx*tx; if (mag == 0) { nx = 1; ny = 0; } else { float norm = sqrtf(ty*ty + tx*tx); tx /= norm; // ty is now sin(2theta) // tx is now cos(2theta). We want sin(theta) and cos(theta) // due to precision err, tx could still have slightly too large magnitude. if (tx > 1) { ny = 0; nx = 1; } else if (tx < -1) { ny = 1; nx = 0; } else { // half angle formula ny = sqrtf((1 - tx)/2); nx = sqrtf((1 + tx)/2); // pick a consistent branch cut if (ty < 0) ny = - ny; } } } if (lineparm) { lineparm[0] = Ex; lineparm[1] = Ey; lineparm[2] = nx; lineparm[3] = ny; } // sum of squared errors = // // SUM_i ((p_x - ux)*nx + (p_y - uy)*ny)^2 // SUM_i nx*nx*(p_x - ux)^2 + 2nx*ny(p_x -ux)(p_y-uy) + ny*ny*(p_y-uy)*(p_y-uy) // nx*nx*SUM_i((p_x -ux)^2) + 2nx*ny*SUM_i((p_x-ux)(p_y-uy)) + ny*ny*SUM_i((p_y-uy)^2) // // nx*nx*N*Cxx + 2nx*ny*N*Cxy + ny*ny*N*Cyy // sum of squared errors if (err) *err = nx*nx*N*Cxx + 2*nx*ny*N*Cxy + ny*ny*N*Cyy; // mean squared error if (mse) *mse = nx*nx*Cxx + 2*nx*ny*Cxy + ny*ny*Cyy; } int pt_compare_theta(const void *_a, const void *_b) { struct pt *a = (struct pt*) _a; struct pt *b = (struct pt*) _b; return (a->theta < b->theta) ? -1 : 1; } int err_compare_descending(const void *_a, const void *_b) { const float *a = _a; const float *b = _b; return ((*a) < (*b)) ? 1 : -1; } /* 1. Identify A) white points near a black point and B) black points near a white point. 2. Find the connected components within each of the classes above, yielding clusters of "white-near-black" and "black-near-white". (These two classes are kept separate). Each segment has a unique id. 3. For every pair of "white-near-black" and "black-near-white" clusters, find the set of points that are in one and adjacent to the other. In other words, a "boundary" layer between the two clusters. (This is actually performed by iterating over the pixels, rather than pairs of clusters.) Critically, this helps keep nearby edges from becoming connected. */ int quad_segment_maxima(apriltag_detector_t *td, zarray_t *cluster, struct line_fit_pt *lfps, int indices[4]) { int sz = zarray_size(cluster); // ksz: when fitting points, how many points on either side do we consider? // (actual "kernel" width is 2ksz). // // This value should be about: 0.5 * (points along shortest edge). // // If all edges were equally-sized, that would give a value of // sz/8. We make it somewhat smaller to account for tags at high // aspects. // XXX Tunable. Maybe make a multiple of JPEG block size to increase robustness // to JPEG compression artifacts? int ksz = imin(20, sz / 12); // can't fit a quad if there are too few points. if (ksz < 2) return 0; // printf("sz %5d, ksz %3d\n", sz, ksz); float *errs = fb_alloc(sz * sizeof(float), FB_ALLOC_NO_HINT); for (int i = 0; i < sz; i++) { fit_line(lfps, sz, (i + sz - ksz) % sz, (i + ksz) % sz, NULL, &errs[i], NULL); } // apply a low-pass filter to errs if (1) { float *y = fb_alloc(sz * sizeof(float), FB_ALLOC_NO_HINT); // how much filter to apply? // XXX Tunable float sigma = 1; // was 3 // cutoff = exp(-j*j/(2*sigma*sigma)); // log(cutoff) = -j*j / (2*sigma*sigma) // log(cutoff)*2*sigma*sigma = -j*j; // how big a filter should we use? We make our kernel big // enough such that we represent any values larger than // 'cutoff'. // XXX Tunable (though not super useful to change) float cutoff = 0.05; int fsz = sqrt(-log(cutoff)*2*sigma*sigma) + 1; fsz = 2*fsz + 1; // For default values of cutoff = 0.05, sigma = 3, // we have fsz = 17. float *f = fb_alloc(fsz * sizeof(float), FB_ALLOC_NO_HINT); for (int i = 0; i < fsz; i++) { int j = i - fsz / 2; f[i] = exp(-j*j/(2*sigma*sigma)); } for (int iy = 0; iy < sz; iy++) { float acc = 0; #ifdef OPTIMIZED int index = (iy - fsz/2 + sz) % sz; for (int i = 0; i < fsz; i++) { acc += errs[index] * f[i]; index++; if (index >= sz) // faster to compare than divide (%) index -= sz; } #else for (int i = 0; i < fsz; i++) { acc += errs[(iy + i - fsz / 2 + sz) % sz] * f[i]; } #endif y[iy] = acc; } fb_free(); // f memcpy(errs, y, sz * sizeof(float)); fb_free(); // y } int *maxima = fb_alloc(sz * sizeof(int), FB_ALLOC_NO_HINT); float *maxima_errs = fb_alloc(sz * sizeof(float), FB_ALLOC_NO_HINT); int nmaxima = 0; for (int i = 0; i < sz; i++) { if (errs[i] > errs[(i+1)%sz] && errs[i] > errs[(i+sz-1)%sz]) { maxima[nmaxima] = i; maxima_errs[nmaxima] = errs[i]; nmaxima++; } } // if we didn't get at least 4 maxima, we can't fit a quad. if (nmaxima < 4){ fb_free(); // maxima_errs fb_free(); // maxima fb_free(); // errs return 0; } // select only the best maxima if we have too many int max_nmaxima = td->qtp.max_nmaxima; if (nmaxima > max_nmaxima) { float *maxima_errs_copy = fb_alloc(nmaxima * sizeof(float), FB_ALLOC_NO_HINT); memcpy(maxima_errs_copy, maxima_errs, nmaxima * sizeof(float)); // throw out all but the best handful of maxima. Sorts descending. qsort(maxima_errs_copy, nmaxima, sizeof(float), err_compare_descending); float maxima_thresh = maxima_errs_copy[max_nmaxima]; int out = 0; for (int in = 0; in < nmaxima; in++) { if (maxima_errs[in] <= maxima_thresh) continue; maxima[out++] = maxima[in]; } nmaxima = out; fb_free(); // maxima_errs_copy } fb_free(); // maxima_errs fb_free(); // maxima fb_free(); // errs int best_indices[4]; float best_error = HUGE_VALF; float err01, err12, err23, err30; float mse01, mse12, mse23, mse30; float params01[4], params12[4], params23[4], params30[4]; // disallow quads where the angle is less than a critical value. float max_dot = cos(td->qtp.critical_rad); //25*M_PI/180); for (int m0 = 0; m0 < nmaxima - 3; m0++) { int i0 = maxima[m0]; for (int m1 = m0+1; m1 < nmaxima - 2; m1++) { int i1 = maxima[m1]; fit_line(lfps, sz, i0, i1, params01, &err01, &mse01); if (mse01 > td->qtp.max_line_fit_mse) continue; for (int m2 = m1+1; m2 < nmaxima - 1; m2++) { int i2 = maxima[m2]; fit_line(lfps, sz, i1, i2, params12, &err12, &mse12); if (mse12 > td->qtp.max_line_fit_mse) continue; float dot = params01[2]*params12[2] + params01[3]*params12[3]; if (fabs(dot) > max_dot) continue; for (int m3 = m2+1; m3 < nmaxima; m3++) { int i3 = maxima[m3]; fit_line(lfps, sz, i2, i3, params23, &err23, &mse23); if (mse23 > td->qtp.max_line_fit_mse) continue; fit_line(lfps, sz, i3, i0, params30, &err30, &mse30); if (mse30 > td->qtp.max_line_fit_mse) continue; float err = err01 + err12 + err23 + err30; if (err < best_error) { best_error = err; best_indices[0] = i0; best_indices[1] = i1; best_indices[2] = i2; best_indices[3] = i3; } } } } } if (best_error == HUGE_VALF) return 0; for (int i = 0; i < 4; i++) indices[i] = best_indices[i]; if (best_error / sz < td->qtp.max_line_fit_mse) return 1; return 0; } // return 1 if the quad looks okay, 0 if it should be discarded int fit_quad(apriltag_detector_t *td, image_u8_t *im, zarray_t *cluster, struct quad *quad, bool overrideMode) { int res = 0; int sz = zarray_size(cluster); if (sz < 4) // can't fit a quad to less than 4 points return 0; ///////////////////////////////////////////////////////////// // Step 1. Sort points so they wrap around the center of the // quad. We will constrain our quad fit to simply partition this // ordered set into 4 groups. // compute a bounding box so that we can order the points // according to their angle WRT the center. int32_t xmax = 0, xmin = INT32_MAX, ymax = 0, ymin = INT32_MAX; for (int pidx = 0; pidx < zarray_size(cluster); pidx++) { struct pt *p; zarray_get_volatile(cluster, pidx, &p); xmax = imax(xmax, p->x); xmin = imin(xmin, p->x); ymax = imax(ymax, p->y); ymin = imin(ymin, p->y); } // add some noise to (cx,cy) so that pixels get a more diverse set // of theta estimates. This will help us remove more points. // (Only helps a small amount. The actual noise values here don't // matter much at all, but we want them [-1, 1]. (XXX with // fixed-point, should range be bigger?) float cx = (xmin + xmax) * 0.5 + 0.05118; float cy = (ymin + ymax) * 0.5 + -0.028581; float dot = 0; for (int pidx = 0; pidx < zarray_size(cluster); pidx++) { struct pt *p; zarray_get_volatile(cluster, pidx, &p); float dx = p->x - cx; float dy = p->y - cy; p->theta = atan2f(dy, dx); dot += dx*p->gx + dy*p->gy; // p->theta = terrible_atan2(dy, dx); } // Ensure that the black border is inside the white border. if ((!overrideMode) && (dot < 0)) return 0; // we now sort the points according to theta. This is a preparatory // step for segmenting them into four lines. if (1) { // zarray_sort(cluster, pt_compare_theta); ptsort((struct pt*) cluster->data, zarray_size(cluster)); // remove duplicate points. (A byproduct of our segmentation system.) if (1) { int outpos = 1; struct pt *last; zarray_get_volatile(cluster, 0, &last); for (int i = 1; i < sz; i++) { struct pt *p; zarray_get_volatile(cluster, i, &p); if (p->x != last->x || p->y != last->y) { if (i != outpos) { struct pt *out; zarray_get_volatile(cluster, outpos, &out); memcpy(out, p, sizeof(struct pt)); } outpos++; } last = p; } cluster->size = outpos; sz = outpos; } } else { // This is a counting sort in which we retain at most one // point for every bucket; the bucket index is computed from // theta. Since a good quad completes a complete revolution, // there's reason to think that we should get a good // distribution of thetas. We might "lose" a few points due // to collisions, but this shouldn't affect quality very much. // XXX tunable. Increase to reduce the likelihood of "losing" // points due to collisions. int nbuckets = 4*sz; #define ASSOC 2 struct pt v[nbuckets][ASSOC]; memset(v, 0, sizeof(v)); // put each point into a bucket. for (int i = 0; i < sz; i++) { struct pt *p; zarray_get_volatile(cluster, i, &p); assert(p->theta >= -M_PI && p->theta <= M_PI); int bucket = (nbuckets - 1) * (p->theta + M_PI) / (2*M_PI); assert(bucket >= 0 && bucket < nbuckets); for (int i = 0; i < ASSOC; i++) { if (v[bucket][i].theta == 0) { v[bucket][i] = *p; break; } } } // collect the points from the buckets and put them back into the array. int outsz = 0; for (int i = 0; i < nbuckets; i++) { for (int j = 0; j < ASSOC; j++) { if (v[i][j].theta != 0) { zarray_set(cluster, outsz, &v[i][j], NULL); outsz++; } } } zarray_truncate(cluster, outsz); sz = outsz; } if (sz < 4) return 0; ///////////////////////////////////////////////////////////// // Step 2. Precompute statistics that allow line fit queries to be // efficiently computed for any contiguous range of indices. struct line_fit_pt *lfps = fb_alloc0(sz * sizeof(struct line_fit_pt), FB_ALLOC_NO_HINT); for (int i = 0; i < sz; i++) { struct pt *p; zarray_get_volatile(cluster, i, &p); if (i > 0) { memcpy(&lfps[i], &lfps[i-1], sizeof(struct line_fit_pt)); } if (0) { // we now undo our fixed-point arithmetic. float delta = 0.5; float x = p->x * .5 + delta; float y = p->y * .5 + delta; float W; for (int dy = -1; dy <= 1; dy++) { int iy = y + dy; if (iy < 0 || iy + 1 >= im->height) continue; for (int dx = -1; dx <= 1; dx++) { int ix = x + dx; if (ix < 0 || ix + 1 >= im->width) continue; int grad_x = im->buf[iy * im->stride + ix + 1] - im->buf[iy * im->stride + ix - 1]; int grad_y = im->buf[(iy+1) * im->stride + ix] - im->buf[(iy-1) * im->stride + ix]; W = sqrtf(grad_x*grad_x + grad_y*grad_y) + 1; // float fx = x + dx, fy = y + dy; float fx = ix + .5, fy = iy + .5; lfps[i].Mx += W * fx; lfps[i].My += W * fy; lfps[i].Mxx += W * fx * fx; lfps[i].Mxy += W * fx * fy; lfps[i].Myy += W * fy * fy; lfps[i].W += W; } } } else { // we now undo our fixed-point arithmetic. float delta = 0.5; // adjust for pixel center bias float x = p->x * .5 + delta; float y = p->y * .5 + delta; int ix = x, iy = y; float W = 1; if (ix > 0 && ix+1 < im->width && iy > 0 && iy+1 < im->height) { int grad_x = im->buf[iy * im->stride + ix + 1] - im->buf[iy * im->stride + ix - 1]; int grad_y = im->buf[(iy+1) * im->stride + ix] - im->buf[(iy-1) * im->stride + ix]; // XXX Tunable. How to shape the gradient magnitude? W = sqrt(grad_x*grad_x + grad_y*grad_y) + 1; } float fx = x, fy = y; lfps[i].Mx += W * fx; lfps[i].My += W * fy; lfps[i].Mxx += W * fx * fx; lfps[i].Mxy += W * fx * fy; lfps[i].Myy += W * fy * fy; lfps[i].W += W; } } int indices[4]; if (1) { if (!quad_segment_maxima(td, cluster, lfps, indices)) goto finish; } // printf("%d %d %d %d\n", indices[0], indices[1], indices[2], indices[3]); if (0) { // no refitting here; just use those points as the vertices. // Note, this is useful for debugging, but pretty bad in // practice since this code path also omits several // plausibility checks that save us tons of time in quad // decoding. for (int i = 0; i < 4; i++) { struct pt *p; zarray_get_volatile(cluster, indices[i], &p); quad->p[i][0] = .5*p->x; // undo fixed-point arith. quad->p[i][1] = .5*p->y; } res = 1; } else { float lines[4][4]; for (int i = 0; i < 4; i++) { int i0 = indices[i]; int i1 = indices[(i+1)&3]; if (0) { // if there are enough points, skip the points near the corners // (because those tend not to be very good.) if (i1-i0 > 8) { int t = (i1-i0)/6; if (t < 0) t = -t; i0 = (i0 + t) % sz; i1 = (i1 + sz - t) % sz; } } float err; fit_line(lfps, sz, i0, i1, lines[i], NULL, &err); if (err > td->qtp.max_line_fit_mse) { res = 0; goto finish; } } for (int i = 0; i < 4; i++) { // solve for the intersection of lines (i) and (i+1)&3. // p0 + lambda0*u0 = p1 + lambda1*u1, where u0 and u1 // are the line directions. // // lambda0*u0 - lambda1*u1 = (p1 - p0) // // rearrange (solve for lambdas) // // [u0_x -u1_x ] [lambda0] = [ p1_x - p0_x ] // [u0_y -u1_y ] [lambda1] [ p1_y - p0_y ] // // remember that lines[i][0,1] = p, lines[i][2,3] = NORMAL vector. // We want the unit vector, so we need the perpendiculars. Thus, below // we have swapped the x and y components and flipped the y components. float A00 = lines[i][3], A01 = -lines[(i+1)&3][3]; float A10 = -lines[i][2], A11 = lines[(i+1)&3][2]; float B0 = -lines[i][0] + lines[(i+1)&3][0]; float B1 = -lines[i][1] + lines[(i+1)&3][1]; float det = A00 * A11 - A10 * A01; // inverse. float W00 = A11 / det, W01 = -A01 / det; if (fabs(det) < 0.001) { res = 0; goto finish; } // solve float L0 = W00*B0 + W01*B1; // compute intersection quad->p[i][0] = lines[i][0] + L0*A00; quad->p[i][1] = lines[i][1] + L0*A10; if (0) { // we should get the same intersection starting // from point p1 and moving L1*u1. float W10 = -A10 / det, W11 = A00 / det; float L1 = W10*B0 + W11*B1; float x = lines[(i+1)&3][0] - L1*A10; float y = lines[(i+1)&3][1] - L1*A11; assert(fabs(x - quad->p[i][0]) < 0.001 && fabs(y - quad->p[i][1]) < 0.001); } res = 1; } } // reject quads that are too small if (1) { float area = 0; // get area of triangle formed by points 0, 1, 2, 0 float length[3], p; for (int i = 0; i < 3; i++) { int idxa = i; // 0, 1, 2, int idxb = (i+1) % 3; // 1, 2, 0 length[i] = sqrt(sq(quad->p[idxb][0] - quad->p[idxa][0]) + sq(quad->p[idxb][1] - quad->p[idxa][1])); } p = (length[0] + length[1] + length[2]) / 2; area += sqrt(p*(p-length[0])*(p-length[1])*(p-length[2])); // get area of triangle formed by points 2, 3, 0, 2 for (int i = 0; i < 3; i++) { int idxs[] = { 2, 3, 0, 2 }; int idxa = idxs[i]; int idxb = idxs[i+1]; length[i] = sqrt(sq(quad->p[idxb][0] - quad->p[idxa][0]) + sq(quad->p[idxb][1] - quad->p[idxa][1])); } p = (length[0] + length[1] + length[2]) / 2; area += sqrt(p*(p-length[0])*(p-length[1])*(p-length[2])); // we don't actually know the family yet (quad detection is generic.) // This threshold is based on a 6x6 tag (which is actually 8x8) // int d = fam->d + fam->black_border*2; int d = 8; if (area < d*d) { res = 0; goto finish; } } // reject quads whose cumulative angle change isn't equal to 2PI if (1) { float total = 0; for (int i = 0; i < 4; i++) { int i0 = i, i1 = (i+1)&3, i2 = (i+2)&3; float theta0 = atan2f(quad->p[i0][1] - quad->p[i1][1], quad->p[i0][0] - quad->p[i1][0]); float theta1 = atan2f(quad->p[i2][1] - quad->p[i1][1], quad->p[i2][0] - quad->p[i1][0]); float dtheta = theta0 - theta1; if (dtheta < 0) dtheta += 2*M_PI; if (dtheta < td->qtp.critical_rad || dtheta > (M_PI - td->qtp.critical_rad)) res = 0; total += dtheta; } // looking for 2PI if (total < 6.2 || total > 6.4) { res = 0; goto finish; } } // adjust pixel coordinates; all math up 'til now uses pixel // coordinates in which (0,0) is the lower left corner. But each // pixel actually spans from to [x, x+1), [y, y+1) the mean value of which // is +.5 higher than x & y. /* float delta = .5; for (int i = 0; i < 4; i++) { quad->p[i][0] += delta; quad->p[i][1] += delta; } */ finish: fb_free(); // lfps 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); 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); for (int x = 1; x < w - 1; x++) { uint8_t v = im->buf[y*s + x]; if (v == 127) continue; // (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); } } } #undef DO_UNIONFIND #endif // OPTIMIZED image_u8_t *threshold(apriltag_detector_t *td, image_u8_t *im) { int w = im->width, h = im->height, s = im->stride; assert(w < 32768); assert(h < 32768); image_u8_t *threshim = fb_alloc(sizeof(image_u8_t), FB_ALLOC_NO_HINT); threshim->width = w; threshim->height = h; threshim->stride = s; threshim->buf = fb_alloc(w * h, FB_ALLOC_NO_HINT); assert(threshim->stride == s); // The idea is to find the maximum and minimum values in a // window around each pixel. If it's a contrast-free region // (max-min is small), don't try to binarize. Otherwise, // threshold according to (max+min)/2. // // Mark low-contrast regions with value 127 so that we can skip // future work on these areas too. // however, computing max/min around every pixel is needlessly // expensive. We compute max/min for tiles. To avoid artifacts // that arise when high-contrast features appear near a tile // edge (and thus moving from one tile to another results in a // large change in max/min value), the max/min values used for // any pixel are computed from all 3x3 surrounding tiles. Thus, // the max/min sampling area for nearby pixels overlap by at least // one tile. // // The important thing is that the windows be large enough to // capture edge transitions; the tag does not need to fit into // a tile. // XXX Tunable. Generally, small tile sizes--- so long as they're // large enough to span a single tag edge--- seem to be a winner. const int tilesz = 4; // the last (possibly partial) tiles along each row and column will // just use the min/max value from the last full tile. int tw = w / tilesz; int th = h / tilesz; uint8_t *im_max = fb_alloc(tw*th*sizeof(uint8_t), FB_ALLOC_NO_HINT); uint8_t *im_min = fb_alloc(tw*th*sizeof(uint8_t), FB_ALLOC_NO_HINT); // first, collect min/max statistics for each tile for (int ty = 0; ty < th; ty++) { for (int tx = 0; tx < tw; tx++) { #if defined( OPTIMIZED ) && defined(ARM_MATH_DSP) 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; for (int dy = 0; dy < tilesz; dy++) { for (int dx = 0; dx < tilesz; dx++) { uint8_t v = im->buf[(ty*tilesz+dy)*s + tx*tilesz + dx]; if (v < min) min = v; if (v > max) max = v; } } im_max[ty*tw+tx] = max; im_min[ty*tw+tx] = min; #endif } } // second, apply 3x3 max/min convolution to "blur" these values // over larger areas. This reduces artifacts due to abrupt changes // in the threshold value. if (1) { uint8_t *im_max_tmp = fb_alloc(tw*th*sizeof(uint8_t), FB_ALLOC_NO_HINT); uint8_t *im_min_tmp = fb_alloc(tw*th*sizeof(uint8_t), FB_ALLOC_NO_HINT); #ifdef OPTIMIZED // Checking boundaries on every pixel wastes significant time; just break it into 5 pieces // (center, top, bottom, left right) // First pass does the entire center area int ty, tx, dy, dx; for (ty = 1; ty < th-1; ty++) { for (tx = 1; tx < tw-1; tx++) { uint8_t max = 0, min = 255; for (dy = -1; dy <= 1; dy++) { for (dx = -1; dx <= 1; dx++) { uint8_t m = im_max[(ty+dy)*tw+tx+dx]; if (m > max) max = m; m = im_min[(ty+dy)*tw+tx+dx]; if (m < min) min = m; } } im_max_tmp[ty*tw + tx] = max; im_min_tmp[ty*tw + tx] = min; } } // top edge ty = 0; for (tx = 1; tx < tw-1; tx++) { uint8_t max = 0, min = 255; for (dy = 0; dy <= 1; dy++) { for (dx = -1; dx <= 1; dx++) { uint8_t m = im_max[(ty+dy)*tw+tx+dx]; if (m > max) max = m; m = im_min[(ty+dy)*tw+tx+dx]; if (m < min) min = m; } } im_max_tmp[ty*tw + tx] = max; im_min_tmp[ty*tw + tx] = min; } // bottom edge ty = th-1; for (tx = 1; tx < tw-1; tx++) { uint8_t max = 0, min = 255; for (dy = -1; dy <= 0; dy++) { for (dx = -1; dx <= 1; dx++) { uint8_t m = im_max[(ty+dy)*tw+tx+dx]; if (m > max) max = m; m = im_min[(ty+dy)*tw+tx+dx]; if (m < min) min = m; } } im_max_tmp[ty*tw + tx] = max; im_min_tmp[ty*tw + tx] = min; } // left edge tx = 0; for (ty = 1; ty < th-1; ty++) { uint8_t max = 0, min = 255; for (dy = -1; dy <= 1; dy++) { for (dx = 0; dx <= 1; dx++) { uint8_t m = im_max[(ty+dy)*tw+tx+dx]; if (m > max) max = m; m = im_min[(ty+dy)*tw+tx+dx]; if (m < min) min = m; } } im_max_tmp[ty*tw + tx] = max; im_min_tmp[ty*tw + tx] = min; } // right edge tx = tw-1; for (ty = 1; ty < th-1; ty++) { uint8_t max = 0, min = 255; for (dy = -1; dy <= 1; dy++) { for (dx = -1; dx <= 0; dx++) { uint8_t m = im_max[(ty+dy)*tw+tx+dx]; if (m > max) max = m; m = im_min[(ty+dy)*tw+tx+dx]; if (m < min) min = m; } } im_max_tmp[ty*tw + tx] = max; im_min_tmp[ty*tw + tx] = min; } #else for (int ty = 0; ty < th; ty++) { for (int tx = 0; tx < tw; tx++) { uint8_t max = 0, min = 255; for (int dy = -1; dy <= 1; dy++) { if (ty+dy < 0 || ty+dy >= th) continue; for (int dx = -1; dx <= 1; dx++) { if (tx+dx < 0 || tx+dx >= tw) continue; uint8_t m = im_max[(ty+dy)*tw+tx+dx]; if (m > max) max = m; m = im_min[(ty+dy)*tw+tx+dx]; if (m < min) min = m; } } im_max_tmp[ty*tw + tx] = max; im_min_tmp[ty*tw + tx] = min; } } #endif memcpy(im_max, im_max_tmp, tw*th*sizeof(uint8_t)); memcpy(im_min, im_min_tmp, tw*th*sizeof(uint8_t)); fb_free(); // im_min_tmp fb_free(); // im_max_tmp } #if defined( OPTIMIZED ) && defined(ARM_MATH_DSP) 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 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 < td->qtp.min_white_black_diff) { for (int dy = 0; dy < tilesz; dy++) { int y = ty*tilesz + dy; for (int dx = 0; dx < tilesz; dx++) { int x = tx*tilesz + dx; threshim->buf[y*s+x] = 127; } } continue; } // otherwise, actually threshold this tile. // argument for biasing towards dark; specular highlights // can be substantially brighter than white tag parts uint8_t thresh = min + (max - min) / 2; for (int dy = 0; dy < tilesz; dy++) { int y = ty*tilesz + dy; for (int dx = 0; dx < tilesz; dx++) { int x = tx*tilesz + dx; uint8_t v = im->buf[y*s+x]; if (v > thresh) threshim->buf[y*s+x] = 255; else threshim->buf[y*s+x] = 0; } } } } } // we skipped over the non-full-sized tiles above. Fix those now. if (1) { for (int y = 0; y < h; y++) { // what is the first x coordinate we need to process in this row? int x0; if (y >= th*tilesz) { x0 = 0; // we're at the bottom; do the whole row. } else { x0 = tw*tilesz; // we only need to do the right most part. } // compute tile coordinates and clamp. int ty = y / tilesz; if (ty >= th) ty = th - 1; for (int x = x0; x < w; x++) { int tx = x / tilesz; if (tx >= tw) tx = tw - 1; int max = im_max[ty*tw + tx]; int min = im_min[ty*tw + tx]; int thresh = min + (max - min) / 2; uint8_t v = im->buf[y*s+x]; if (v > thresh) threshim->buf[y*s+x] = 255; else threshim->buf[y*s+x] = 0; } } } fb_free(); // im_min fb_free(); // im_max // this is a dilate/erode deglitching scheme that does not improve // anything as far as I can tell. if (0 || td->qtp.deglitch) { image_u8_t *tmp = fb_alloc(sizeof(image_u8_t), FB_ALLOC_NO_HINT); tmp->width = w; tmp->height = h; tmp->stride = s; tmp->buf = fb_alloc(w * h, FB_ALLOC_NO_HINT); for (int y = 1; y + 1 < h; y++) { for (int x = 1; x + 1 < w; x++) { uint8_t max = 0; for (int dy = -1; dy <= 1; dy++) { for (int dx = -1; dx <= 1; dx++) { uint8_t v = threshim->buf[(y+dy)*s + x + dx]; if (v > max) max = v; } } tmp->buf[y*s+x] = max; } } for (int y = 1; y + 1 < h; y++) { for (int x = 1; x + 1 < w; x++) { uint8_t min = 255; for (int dy = -1; dy <= 1; dy++) { for (int dx = -1; dx <= 1; dx++) { uint8_t v = tmp->buf[(y+dy)*s + x + dx]; if (v < min) min = v; } } threshim->buf[y*s+x] = min; } } fb_free(); // tmp->buf fb_free(); // tmp } return threshim; } zarray_t *apriltag_quad_thresh(apriltag_detector_t *td, image_u8_t *im, bool overrideMode) { //////////////////////////////////////////////////////// // step 1. threshold the image, creating the edge image. int w = im->width, h = im->height; image_u8_t *threshim = threshold(td, im); int ts = threshim->stride; //////////////////////////////////////////////////////// // step 2. find connected components. unionfind_t *uf = unionfind_create(w * h); for (int y = 0; y < h - 1; y++) { do_unionfind_line(uf, threshim, h, w, ts, y); } uint32_t nclustermap; struct uint32_zarray_entry **clustermap = fb_alloc0_all(&nclustermap, FB_ALLOC_PREFER_SPEED); nclustermap /= sizeof(struct uint32_zarray_entry*); if (!nclustermap) fb_alloc_fail(); for (int y = 1; y < h-1; y++) { for (int x = 1; x < w-1; x++) { uint8_t v0 = threshim->buf[y*ts + x]; if (v0 == 127) continue; // XXX don't query this until we know we need it? uint32_t rep0 = unionfind_get_representative(uf, y*w + x); // whenever we find two adjacent pixels such that one is // white and the other black, we add the point half-way // between them to a cluster associated with the unique // ids of the white and black regions. // // We additionally compute the gradient direction (i.e., which // direction was the white pixel?) Note: if (v1-v0) == 255, then // (dx,dy) points towards the white pixel. if (v1-v0) == -255, then // (dx,dy) points towards the black pixel. p.gx and p.gy will thus // be -255, 0, or 255. // // Note that any given pixel might be added to multiple // different clusters. But in the common case, a given // pixel will be added multiple times to the same cluster, // which increases the size of the cluster and thus the // computational costs. // // A possible optimization would be to combine entries // within the same cluster. #define DO_CONN(dx, dy) \ if (1) { \ uint8_t v1 = threshim->buf[y*ts + dy*ts + x + dx]; \ \ while (v0 + v1 == 255) { \ uint32_t rep1 = unionfind_get_representative(uf, y*w + dy*w + x + dx); \ uint32_t clusterid; \ if (rep0 < rep1) \ clusterid = (rep1 << 16) + rep0; \ else \ clusterid = (rep0 << 16) + rep1; \ \ /* XXX lousy hash function */ \ uint32_t clustermap_bucket = u64hash_2(clusterid) % nclustermap; \ struct uint32_zarray_entry *entry = clustermap[clustermap_bucket]; \ while (entry && entry->id != clusterid) { \ entry = entry->next; \ } \ \ if (!entry) { \ entry = umm_calloc(1, sizeof(struct uint32_zarray_entry)); \ if (!entry) break; \ entry->id = clusterid; \ entry->cluster = zarray_create_fail_ok(sizeof(struct pt)); \ if (!entry->cluster) { \ free(entry); \ break; \ } \ entry->next = clustermap[clustermap_bucket]; \ clustermap[clustermap_bucket] = entry; \ } \ \ struct pt p = { .x = 2*x + dx, .y = 2*y + dy, .gx = dx*((int) v1-v0), .gy = dy*((int) v1-v0)}; \ zarray_add_fail_ok(entry->cluster, &p); \ break; \ } \ } // do 4 connectivity. NB: Arguments must be [-1, 1] or we'll overflow .gx, .gy DO_CONN(1, 0); DO_CONN(0, 1); #ifdef IMLIB_ENABLE_FINE_APRILTAGS // do 8 connectivity DO_CONN(-1, 1); DO_CONN(1, 1); #endif } } #undef DO_CONN //////////////////////////////////////////////////////// // step 3. process each connected component. zarray_t *clusters = zarray_create_fail_ok(sizeof(zarray_t*)); //, uint32_zarray_hash_size(clustermap)); if (clusters) { for (int i = 0; i < nclustermap; i++) { for (struct uint32_zarray_entry *entry = clustermap[i]; entry; entry = entry->next) { // XXX reject clusters here? zarray_add_fail_ok(clusters, &entry->cluster); } } } int sz = clusters ? zarray_size(clusters) : 0; if (1) { for (int i = 0; i < nclustermap; i++) { struct uint32_zarray_entry *entry = clustermap[i]; while (entry) { // free any leaked cluster (zarray_add_fail_ok) bool leaked = true; for (int j = 0; j < sz && leaked; j++) { zarray_t *cluster; zarray_get(clusters, j, &cluster); leaked &= entry->cluster != cluster; } if (leaked) free(entry->cluster); struct uint32_zarray_entry *tmp = entry->next; free(entry); entry = tmp; } } fb_free(); // clustermap } unionfind_destroy(); fb_free(); // threshim->buf fb_free(); // threshim zarray_t *quads = zarray_create_fail_ok(sizeof(struct quad)); if (quads) { for (int i = 0; i < sz; i++) { zarray_t *cluster; zarray_get(clusters, i, &cluster); if (zarray_size(cluster) < td->qtp.min_cluster_pixels) continue; // a cluster should contain only boundary points around the // tag. it cannot be bigger than the whole screen. (Reject // large connected blobs that will be prohibitively slow to // fit quads to.) A typical point along an edge is added three // times (because it has 3 neighbors). The maximum perimeter // is 2w+2h. if (zarray_size(cluster) > 3*(2*w+2*h)) { continue; } struct quad quad; memset(&quad, 0, sizeof(struct quad)); if (fit_quad(td, im, cluster, &quad, overrideMode)) { zarray_add_fail_ok(quads, &quad); } } } // printf(" %d %d %d %d\n", indices[0], indices[1], indices[2], indices[3]); for (int i = 0; i < sz; i++) { zarray_t *cluster; zarray_get(clusters, i, &cluster); zarray_destroy(cluster); } if (clusters) zarray_destroy(clusters); if (!quads) { // we should have enough memory now quads = zarray_create(sizeof(struct quad)); } return quads; } //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "apriltag.c" //////////////////////////////////////////////////////////////////////////////////////////////////// #ifndef M_PI # define M_PI 3.141592653589793238462643383279502884196 #endif // Regresses a model of the form: // intensity(x,y) = C0*x + C1*y + CC2 // The J matrix is the: // J = [ x1 y1 1 ] // [ x2 y2 1 ] // [ ... ] // The A matrix is J'J struct graymodel { float A[3][3]; float B[3]; float C[3]; }; void graymodel_init(struct graymodel *gm) { memset(gm, 0, sizeof(struct graymodel)); } void graymodel_add(struct graymodel *gm, float x, float y, float gray) { // update upper right entries of A = J'J gm->A[0][0] += x*x; gm->A[0][1] += x*y; gm->A[0][2] += x; gm->A[1][1] += y*y; gm->A[1][2] += y; gm->A[2][2] += 1; // update B = J'gray gm->B[0] += x * gray; gm->B[1] += y * gray; gm->B[2] += gray; } void graymodel_solve(struct graymodel *gm) { mat33_sym_solve((float*) gm->A, gm->B, gm->C); } float graymodel_interpolate(struct graymodel *gm, float x, float y) { return gm->C[0]*x + gm->C[1]*y + gm->C[2]; } struct quick_decode_entry { uint64_t rcode; // the queried code 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 { int nentries; struct quick_decode_entry *entries; }; /** if the bits in w were arranged in a d*d grid and that grid was * rotated, what would the new bits in w be? * The bits are organized like this (for d = 3): * * 8 7 6 2 5 8 0 1 2 * 5 4 3 ==> 1 4 7 ==> 3 4 5 (rotate90 applied twice) * 2 1 0 0 3 6 6 7 8 **/ static uint64_t rotate90(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 = r + d*c; wr = wr << 1; if ((w & (((uint64_t) 1) << b))!=0) wr |= 1; } } 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) return; matd_destroy(quad->H); matd_destroy(quad->Hinv); free(quad); } struct quad *quad_copy(struct quad *quad) { struct quad *q = calloc(1, sizeof(struct quad)); memcpy(q, quad, sizeof(struct quad)); if (quad->H) q->H = matd_copy(quad->H); if (quad->Hinv) q->Hinv = matd_copy(quad->Hinv); return q; } // http://en.wikipedia.org/wiki/Hamming_weight //types and constants used in the functions below //uint64_t is an unsigned 64-bit integer variable type (defined in C99 version of C language) const uint64_t m1 = 0x5555555555555555; //binary: 0101... const uint64_t m2 = 0x3333333333333333; //binary: 00110011.. const uint64_t m4 = 0x0f0f0f0f0f0f0f0f; //binary: 4 zeros, 4 ones ... const uint64_t m8 = 0x00ff00ff00ff00ff; //binary: 8 zeros, 8 ones ... const uint64_t m16 = 0x0000ffff0000ffff; //binary: 16 zeros, 16 ones ... const uint64_t m32 = 0x00000000ffffffff; //binary: 32 zeros, 32 ones const uint64_t hff = 0xffffffffffffffff; //binary: all ones const uint64_t h01 = 0x0101010101010101; //the sum of 256 to the power of 0,1,2,3... //This is a naive implementation, shown for comparison, //and to help in understanding the better functions. //This algorithm uses 24 arithmetic operations (shift, add, and). int popcount64a(uint64_t x) { x = (x & m1 ) + ((x >> 1) & m1 ); //put count of each 2 bits into those 2 bits x = (x & m2 ) + ((x >> 2) & m2 ); //put count of each 4 bits into those 4 bits x = (x & m4 ) + ((x >> 4) & m4 ); //put count of each 8 bits into those 8 bits x = (x & m8 ) + ((x >> 8) & m8 ); //put count of each 16 bits into those 16 bits x = (x & m16) + ((x >> 16) & m16); //put count of each 32 bits into those 32 bits x = (x & m32) + ((x >> 32) & m32); //put count of each 64 bits into those 64 bits return x; } //This uses fewer arithmetic operations than any other known //implementation on machines with slow multiplication. //This algorithm uses 17 arithmetic operations. int popcount64b(uint64_t x) { x -= (x >> 1) & m1; //put count of each 2 bits into those 2 bits x = (x & m2) + ((x >> 2) & m2); //put count of each 4 bits into those 4 bits x = (x + (x >> 4)) & m4; //put count of each 8 bits into those 8 bits x += x >> 8; //put count of each 16 bits into their lowest 8 bits x += x >> 16; //put count of each 32 bits into their lowest 8 bits x += x >> 32; //put count of each 64 bits into their lowest 8 bits return x & 0x7f; } //This uses fewer arithmetic operations than any other known //implementation on machines with fast multiplication. //This algorithm uses 12 arithmetic operations, one of which is a multiply. int popcount64c(uint64_t x) { x -= (x >> 1) & m1; //put count of each 2 bits into those 2 bits x = (x & m2) + ((x >> 2) & m2); //put count of each 4 bits into those 4 bits x = (x + (x >> 4)) & m4; //put count of each 8 bits into those 8 bits return (x * h01) >> 56; //returns left 8 bits of x + (x<<8) + (x<<16) + (x<<24) + ... } // returns an entry with hamming set to 255 if no decode was found. static void quick_decode_codeword(apriltag_family_t *tf, uint64_t rcode, struct quick_decode_entry *entry) { int threshold = imax(tf->h - tf->d - 1, 0); 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 = 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; } } rcode = rotate90(rcode, tf->d); } entry->rcode = 0; 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) { apriltag_detection_t *a = *(apriltag_detection_t**) _a; apriltag_detection_t *b = *(apriltag_detection_t**) _b; return a->id - b->id; } void apriltag_detector_remove_family(apriltag_detector_t *td, apriltag_family_t *fam) { zarray_remove_value(td->tag_families, &fam, 0); } void apriltag_detector_add_family_bits(apriltag_detector_t *td, apriltag_family_t *fam, int bits_corrected) { zarray_add(td->tag_families, &fam); } void apriltag_detector_clear_families(apriltag_detector_t *td) { zarray_clear(td->tag_families); } apriltag_detector_t *apriltag_detector_create() { apriltag_detector_t *td = (apriltag_detector_t*) calloc(1, sizeof(apriltag_detector_t)); td->qtp.max_nmaxima = 10; td->qtp.min_cluster_pixels = 5; td->qtp.max_line_fit_mse = 10.0; td->qtp.critical_rad = 10 * M_PI / 180; td->qtp.deglitch = 0; td->qtp.min_white_black_diff = 5; td->tag_families = zarray_create(sizeof(apriltag_family_t*)); td->refine_edges = 1; td->refine_pose = 0; td->refine_decode = 0; return td; } void apriltag_detector_destroy(apriltag_detector_t *td) { apriltag_detector_clear_families(td); zarray_destroy(td->tag_families); free(td); } struct evaluate_quad_ret { int64_t rcode; float score; matd_t *H, *Hinv; int decode_status; struct quick_decode_entry e; }; // returns non-zero if an error occurs (i.e., H has no inverse) int quad_update_homographies(struct quad *quad) { zarray_t *correspondences = zarray_create(sizeof(float[4])); for (int i = 0; i < 4; i++) { float corr[4]; // At this stage of the pipeline, we have not attempted to decode the // quad into an oriented tag. Thus, just act as if the quad is facing // "up" with respect to our desired corners. We'll fix the rotation // later. // [-1, -1], [1, -1], [1, 1], [-1, 1] corr[0] = (i==0 || i==3) ? -1 : 1; corr[1] = (i==0 || i==1) ? -1 : 1; corr[2] = quad->p[i][0]; corr[3] = quad->p[i][1]; zarray_add(correspondences, &corr); } if (quad->H) matd_destroy(quad->H); if (quad->Hinv) matd_destroy(quad->Hinv); // XXX Tunable quad->H = homography_compute(correspondences, HOMOGRAPHY_COMPUTE_FLAG_SVD); quad->Hinv = matd_inverse(quad->H); zarray_destroy(correspondences); if (quad->H && quad->Hinv) return 0; return -1; } // compute a "score" for a quad that is independent of tag family // encoding (but dependent upon the tag geometry) by considering the // contrast around the exterior of the tag. float quad_goodness(apriltag_family_t *family, image_u8_t *im, struct quad *quad) { // when sampling from the white border, how much white border do // we actually consider valid, measured in bit-cell units? (the // outside portions are often intruded upon, so it could be advantageous to use // less than the "nominal" 1.0. (Less than 1.0 not well tested.) // XXX Tunable float white_border = 1; // in tag coordinates, how big is each bit cell? float bit_size = 2.0 / (2*family->black_border + family->d); // float inv_bit_size = 1.0 / bit_size; int32_t xmin = INT32_MAX, xmax = 0, ymin = INT32_MAX, ymax = 0; for (int i = 0; i < 4; i++) { float tx = (i == 0 || i == 3) ? -1 - bit_size : 1 + bit_size; float ty = (i == 0 || i == 1) ? -1 - bit_size : 1 + bit_size; float x, y; homography_project(quad->H, tx, ty, &x, &y); xmin = imin(xmin, x); xmax = imax(xmax, x); ymin = imin(ymin, y); ymax = imax(ymax, y); } // clamp bounding box to image dimensions xmin = imax(0, xmin); xmax = imin(im->width-1, xmax); ymin = imax(0, ymin); ymax = imin(im->height-1, ymax); // int nbits = family->d * family->d; int32_t W1 = 0, B1 = 0, Wn = 0, Bn = 0; // int64_t W1 = 0, B1 = 0, Wn = 0, Bn = 0; float wsz = bit_size*white_border; float bsz = bit_size*family->black_border; matd_t *Hinv = quad->Hinv; // matd_t *H = quad->H; // iterate over all the pixels in the tag. (Iterating in pixel space) for (int y = ymin; y <= ymax; y++) { // we'll incrementally compute the homography // projections. Begin by evaluating the homogeneous position // [(xmin - .5f), y, 1]. Then, we'll update as we stride in // the +x direction. float Hx = MATD_EL(Hinv, 0, 0) * (.5 + (int) xmin) + MATD_EL(Hinv, 0, 1) * (y + .5) + MATD_EL(Hinv, 0, 2); float Hy = MATD_EL(Hinv, 1, 0) * (.5 + (int) xmin) + MATD_EL(Hinv, 1, 1) * (y + .5) + MATD_EL(Hinv, 1, 2); float Hh = MATD_EL(Hinv, 2, 0) * (.5 + (int) xmin) + MATD_EL(Hinv, 2, 1) * (y + .5) + MATD_EL(Hinv, 2, 2); for (int x = xmin; x <= xmax; x++) { // project the pixel center. float tx, ty; // divide by homogeneous coordinate tx = Hx / Hh; ty = Hy / Hh; // if we move x one pixel to the right, here's what // happens to our three pre-normalized coordinates. Hx += MATD_EL(Hinv, 0, 0); Hy += MATD_EL(Hinv, 1, 0); Hh += MATD_EL(Hinv, 2, 0); float txa = fabsf((float) tx), tya = fabsf((float) ty); float xymax = fmaxf(txa, tya); // if (txa >= 1 + wsz || tya >= 1 + wsz) if (xymax >= 1 + wsz) continue; uint8_t v = im->buf[y*im->stride + x]; // it's within the white border? // if (txa >= 1 || tya >= 1) { if (xymax >= 1) { W1 += v; Wn ++; continue; } // it's within the black border? // if (txa >= 1 - bsz || tya >= 1 - bsz) { if (xymax >= 1 - bsz) { B1 += v; Bn ++; continue; } // it must be a data bit. We don't do anything with these. continue; } } // score = average margin between white and black pixels near border. float margin = 1.0 * W1 / Wn - 1.0 * B1 / Bn; // printf("margin %f: W1 %f, B1 %f\n", margin, W1, B1); return margin; } // returns the decision margin. Return < 0 if the detection should be rejected. float quad_decode(apriltag_family_t *family, image_u8_t *im, struct quad *quad, struct quick_decode_entry *entry, image_u8_t *im_samples) { // decode the tag binary contents by sampling the pixel // closest to the center of each bit cell. int64_t rcode = 0; // how wide do we assume the white border is? float white_border = 1.0; // We will compute a threshold by sampling known white/black cells around this tag. // This sampling is achieved by considering a set of samples along lines. // // coordinates are given in bit coordinates. ([0, fam->d]). // // { initial x, initial y, delta x, delta y, WHITE=1 } float patterns[] = { // left white column 0 - white_border / 2.0, 0.5, 0, 1, 1, // left black column 0 + family->black_border / 2.0, 0.5, 0, 1, 0, // right white column 2*family->black_border + family->d + white_border / 2.0, .5, 0, 1, 1, // right black column 2*family->black_border + family->d - family->black_border / 2.0, .5, 0, 1, 0, // top white row 0.5, -white_border / 2.0, 1, 0, 1, // top black row 0.5, family->black_border / 2.0, 1, 0, 0, // bottom white row 0.5, 2*family->black_border + family->d + white_border / 2.0, 1, 0, 1, // bottom black row 0.5, 2*family->black_border + family->d - family->black_border / 2.0, 1, 0, 0 // XXX float-counts the corners. }; struct graymodel whitemodel, blackmodel; graymodel_init(&whitemodel); graymodel_init(&blackmodel); for (int pattern_idx = 0; pattern_idx < sizeof(patterns)/(5*sizeof(float)); pattern_idx ++) { float *pattern = &patterns[pattern_idx * 5]; int is_white = pattern[4]; for (int i = 0; i < 2*family->black_border + family->d; i++) { float tagx01 = (pattern[0] + i*pattern[2]) / (2*family->black_border + family->d); float tagy01 = (pattern[1] + i*pattern[3]) / (2*family->black_border + family->d); float tagx = 2*(tagx01-0.5); float tagy = 2*(tagy01-0.5); float px, py; homography_project(quad->H, tagx, tagy, &px, &py); // don't round int ix = px; int iy = py; if (ix < 0 || iy < 0 || ix >= im->width || iy >= im->height) continue; int v = im->buf[iy*im->stride + ix]; if (im_samples) { im_samples->buf[iy*im_samples->stride + ix] = (1-is_white)*255; } if (is_white) graymodel_add(&whitemodel, tagx, tagy, v); else graymodel_add(&blackmodel, tagx, tagy, v); } } graymodel_solve(&whitemodel); graymodel_solve(&blackmodel); // XXX Tunable if (graymodel_interpolate(&whitemodel, 0, 0) - graymodel_interpolate(&blackmodel, 0, 0) < 0) return -1; // compute the average decision margin (how far was each bit from // the decision boundary? // // we score this separately for white and black pixels and return // the minimum average threshold for black/white pixels. This is // to penalize thresholds that are too close to an extreme. float black_score = 0, white_score = 0; float black_score_count = 1, white_score_count = 1; for (int bitidx = 0; bitidx < family->d * family->d; bitidx++) { int bitx = bitidx % family->d; int bity = bitidx / family->d; float tagx01 = (family->black_border + bitx + 0.5) / (2*family->black_border + family->d); float tagy01 = (family->black_border + bity + 0.5) / (2*family->black_border + family->d); // scale to [-1, 1] float tagx = 2*(tagx01-0.5); float tagy = 2*(tagy01-0.5); float px, py; homography_project(quad->H, tagx, tagy, &px, &py); rcode = (rcode << 1); // don't round. int ix = px; int iy = py; if (ix < 0 || iy < 0 || ix >= im->width || iy >= im->height) continue; int v = im->buf[iy*im->stride + ix]; float thresh = (graymodel_interpolate(&blackmodel, tagx, tagy) + graymodel_interpolate(&whitemodel, tagx, tagy)) / 2.0; if (v > thresh) { white_score += (v - thresh); white_score_count ++; rcode |= 1; } else { black_score += (thresh - v); black_score_count ++; } if (im_samples) im_samples->buf[iy*im_samples->stride + ix] = (1 - (rcode & 1)) * 255; } quick_decode_codeword(family, rcode, entry); return fmin(white_score / white_score_count, black_score / black_score_count); } float score_goodness(apriltag_family_t *family, image_u8_t *im, struct quad *quad, void *user) { return quad_goodness(family, im, quad); } float score_decodability(apriltag_family_t *family, image_u8_t *im, struct quad *quad, void *user) { struct quick_decode_entry entry; float decision_margin = quad_decode(family, im, quad, &entry, NULL); // hamming trumps decision margin; maximum value for decision_margin is 255. return decision_margin - entry.hamming*1000; } // returns score of best quad float optimize_quad_generic(apriltag_family_t *family, image_u8_t *im, struct quad *quad0, float *stepsizes, int nstepsizes, float (*score)(apriltag_family_t *family, image_u8_t *im, struct quad *quad, void *user), void *user) { struct quad *best_quad = quad_copy(quad0); float best_score = score(family, im, best_quad, user); for (int stepsize_idx = 0; stepsize_idx < nstepsizes; stepsize_idx++) { int improved = 1; // when we make progress with a particular step size, how many // times will we try to perform that same step size again? // (max_repeat = 0 means ("don't repeat--- just move to the // next step size"). // XXX Tunable int max_repeat = 1; for (int repeat = 0; repeat <= max_repeat && improved; repeat++) { improved = 0; // wiggle point i for (int i = 0; i < 4; i++) { float stepsize = stepsizes[stepsize_idx]; // XXX Tunable (really 1 makes the best sense since) int nsteps = 1; struct quad *this_best_quad = NULL; float this_best_score = best_score; for (int sx = -nsteps; sx <= nsteps; sx++) { for (int sy = -nsteps; sy <= nsteps; sy++) { if (sx==0 && sy==0) continue; struct quad *this_quad = quad_copy(best_quad); this_quad->p[i][0] = best_quad->p[i][0] + sx*stepsize; this_quad->p[i][1] = best_quad->p[i][1] + sy*stepsize; if (quad_update_homographies(this_quad)) continue; float this_score = score(family, im, this_quad, user); if (this_score > this_best_score) { quad_destroy(this_best_quad); this_best_quad = this_quad; this_best_score = this_score; } else { quad_destroy(this_quad); } } } if (this_best_score > best_score) { quad_destroy(best_quad); best_quad = this_best_quad; best_score = this_best_score; improved = 1; } } } } matd_destroy(quad0->H); matd_destroy(quad0->Hinv); memcpy(quad0, best_quad, sizeof(struct quad)); // copy pointers free(best_quad); return best_score; } static void refine_edges(apriltag_detector_t *td, image_u8_t *im_orig, struct quad *quad) { float lines[4][4]; // for each line, [Ex Ey nx ny] for (int edge = 0; edge < 4; edge++) { int a = edge, b = (edge + 1) & 3; // indices of the end points. // compute the normal to the current line estimate float nx = quad->p[b][1] - quad->p[a][1]; float ny = -quad->p[b][0] + quad->p[a][0]; float mag = sqrt(nx*nx + ny*ny); nx /= mag; ny /= mag; // we will now fit a NEW line by sampling points near // our original line that have large gradients. On really big tags, // we're willing to sample more to get an even better estimate. int nsamples = imax(16, mag / 8); // XXX tunable // stats for fitting a line... float Mx = 0, My = 0, Mxx = 0, Mxy = 0, Myy = 0, N = 0; for (int s = 0; s < nsamples; s++) { // compute a point along the line... Note, we're avoiding // sampling *right* at the corners, since those points are // the least reliable. float alpha = (1.0 + s) / (nsamples + 1); float x0 = alpha*quad->p[a][0] + (1-alpha)*quad->p[b][0]; float y0 = alpha*quad->p[a][1] + (1-alpha)*quad->p[b][1]; // search along the normal to this line, looking at the // gradients along the way. We're looking for a strong // response. float Mn = 0; float Mcount = 0; // XXX tunable: how far to search? We want to search far // enough that we find the best edge, but not so far that // we hit other edges that aren't part of the tag. We // shouldn't ever have to search more than quad_decimate, // since otherwise we would (ideally) have started our // search on another pixel in the first place. Likewise, // for very small tags, we don't want the range to be too // big. float range = 1.0 + 1; // XXX tunable step size. for (float n = -range; n <= range; n += 0.25) { // Because of the guaranteed winding order of the // points in the quad, we will start inside the white // portion of the quad and work our way outward. // // sample to points (x1,y1) and (x2,y2) XXX tunable: // how far +/- to look? Small values compute the // gradient more precisely, but are more sensitive to // noise. float grange = 1; int x1 = x0 + (n + grange)*nx; int y1 = y0 + (n + grange)*ny; if (x1 < 0 || x1 >= im_orig->width || y1 < 0 || y1 >= im_orig->height) continue; int x2 = x0 + (n - grange)*nx; int y2 = y0 + (n - grange)*ny; if (x2 < 0 || x2 >= im_orig->width || y2 < 0 || y2 >= im_orig->height) continue; int g1 = im_orig->buf[y1*im_orig->stride + x1]; int g2 = im_orig->buf[y2*im_orig->stride + x2]; if (g1 < g2) // reject points whose gradient is "backwards". They can only hurt us. continue; float weight = (g2 - g1)*(g2 - g1); // XXX tunable. What shape for weight=f(g2-g1)? // compute weighted average of the gradient at this point. Mn += weight*n; Mcount += weight; } // what was the average point along the line? if (Mcount == 0) continue; float n0 = Mn / Mcount; // where is the point along the line? float bestx = x0 + n0*nx; float besty = y0 + n0*ny; // update our line fit statistics Mx += bestx; My += besty; Mxx += bestx*bestx; Mxy += bestx*besty; Myy += besty*besty; N++; } // fit a line float Ex = Mx / N, Ey = My / N; float Cxx = Mxx / N - Ex*Ex; float Cxy = Mxy / N - Ex*Ey; float Cyy = Myy / N - Ey*Ey; float normal_theta = .5 * atan2f(-2*Cxy, (Cyy - Cxx)); nx = cosf(normal_theta); ny = sinf(normal_theta); lines[edge][0] = Ex; lines[edge][1] = Ey; lines[edge][2] = nx; lines[edge][3] = ny; } // now refit the corners of the quad for (int i = 0; i < 4; i++) { // solve for the intersection of lines (i) and (i+1)&3. float A00 = lines[i][3], A01 = -lines[(i+1)&3][3]; float A10 = -lines[i][2], A11 = lines[(i+1)&3][2]; float B0 = -lines[i][0] + lines[(i+1)&3][0]; float B1 = -lines[i][1] + lines[(i+1)&3][1]; float det = A00 * A11 - A10 * A01; // inverse. if (fabs(det) > 0.001) { // solve float W00 = A11 / det, W01 = -A01 / det; float L0 = W00*B0 + W01*B1; // compute intersection quad->p[i][0] = lines[i][0] + L0*A00; quad->p[i][1] = lines[i][1] + L0*A10; } else { // this is a bad sign. We'll just keep the corner we had. // printf("bad det: %15f %15f %15f %15f %15f\n", A00, A11, A10, A01, det); } } } void apriltag_detection_destroy(apriltag_detection_t *det) { if (det == NULL) return; matd_destroy(det->H); free(det); } int prefer_smaller(int pref, float q0, float q1) { if (pref) // already prefer something? exit. return pref; if (q0 < q1) return -1; // we now prefer q0 if (q1 < q0) return 1; // we now prefer q1 // no preference return 0; } zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig) { if (zarray_size(td->tag_families) == 0) { zarray_t *s = zarray_create(sizeof(apriltag_detection_t*)); printf("apriltag.c: No tag families enabled."); return s; } /////////////////////////////////////////////////////////// // Step 1. Detect quads according to requested image decimation // and blurring parameters. // zarray_t *quads = apriltag_quad_gradient(td, im_orig); zarray_t *quads = apriltag_quad_thresh(td, im_orig, false); zarray_t *detections = zarray_create(sizeof(apriltag_detection_t*)); td->nquads = zarray_size(quads); //////////////////////////////////////////////////////////////// // Step 2. Decode tags from each quad. if (1) { for (int i = 0; i < zarray_size(quads); i++) { struct quad *quad_original; zarray_get_volatile(quads, i, &quad_original); // refine edges is not dependent upon the tag family, thus // apply this optimization BEFORE the other work. //if (td->quad_decimate > 1 && td->refine_edges) { if (td->refine_edges) { refine_edges(td, im_orig, quad_original); } // make sure the homographies are computed... if (quad_update_homographies(quad_original)) continue; for (int famidx = 0; famidx < zarray_size(td->tag_families); famidx++) { apriltag_family_t *family; zarray_get(td->tag_families, famidx, &family); float goodness = 0; // since the geometry of tag families can vary, start any // optimization process over with the original quad. struct quad *quad = quad_copy(quad_original); // improve the quad corner positions by minimizing the // variance within each intra-bit area. if (td->refine_pose) { // NB: We potentially step an integer // number of times in each direction. To make each // sample as useful as possible, the step sizes should // not be integer multiples of each other. (I.e., // probably don't use 1, 0.5, 0.25, etc.) // XXX Tunable float stepsizes[] = { 1, .4, .16, .064 }; int nstepsizes = sizeof(stepsizes)/sizeof(float); goodness = optimize_quad_generic(family, im_orig, quad, stepsizes, nstepsizes, score_goodness, NULL); } if (td->refine_decode) { // this optimizes decodability, but we don't report // that value to the user. (so discard return value.) // XXX Tunable float stepsizes[] = { .4 }; int nstepsizes = sizeof(stepsizes)/sizeof(float); optimize_quad_generic(family, im_orig, quad, stepsizes, nstepsizes, score_decodability, NULL); } struct quick_decode_entry entry; float decision_margin = quad_decode(family, im_orig, quad, &entry, NULL); if (entry.hamming < 255 && decision_margin >= 0) { apriltag_detection_t *det = calloc(1, sizeof(apriltag_detection_t)); det->family = family; det->id = entry.id; det->hamming = entry.hamming; det->goodness = goodness; det->decision_margin = decision_margin; float theta = -entry.rotation * M_PI / 2.0; float c = cos(theta), s = sin(theta); // Fix the rotation of our homography to properly orient the tag matd_t *R = matd_create(3,3); MATD_EL(R, 0, 0) = c; MATD_EL(R, 0, 1) = -s; MATD_EL(R, 1, 0) = s; MATD_EL(R, 1, 1) = c; MATD_EL(R, 2, 2) = 1; 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]); // [-1, -1], [1, -1], [1, 1], [-1, 1], Desired points // [-1, 1], [1, 1], [1, -1], [-1, -1], FLIP Y // adjust the points in det->p so that they correspond to // counter-clockwise around the quad, starting at -1,-1. for (int i = 0; i < 4; i++) { int tcx = (i == 1 || i == 2) ? 1 : -1; int tcy = (i < 2) ? 1 : -1; float p[2]; homography_project(det->H, tcx, tcy, &p[0], &p[1]); det->p[i][0] = p[0]; det->p[i][1] = p[1]; } zarray_add(detections, &det); } quad_destroy(quad); } } } //////////////////////////////////////////////////////////////// // Step 3. Reconcile detections--- don't report the same tag more // than once. (Allow non-overlapping duplicate detections.) if (1) { zarray_t *poly0 = g2d_polygon_create_zeros(4); zarray_t *poly1 = g2d_polygon_create_zeros(4); for (int i0 = 0; i0 < zarray_size(detections); i0++) { apriltag_detection_t *det0; zarray_get(detections, i0, &det0); for (int k = 0; k < 4; k++) zarray_set(poly0, k, det0->p[k], NULL); for (int i1 = i0+1; i1 < zarray_size(detections); i1++) { apriltag_detection_t *det1; zarray_get(detections, i1, &det1); if (det0->id != det1->id || det0->family != det1->family) continue; for (int k = 0; k < 4; k++) zarray_set(poly1, k, det1->p[k], NULL); if (g2d_polygon_overlaps_polygon(poly0, poly1)) { // the tags overlap. Delete one, keep the other. int pref = 0; // 0 means undecided which one we'll keep. pref = prefer_smaller(pref, det0->hamming, det1->hamming); // want small hamming pref = prefer_smaller(pref, -det0->decision_margin, -det1->decision_margin); // want bigger margins pref = prefer_smaller(pref, -det0->goodness, -det1->goodness); // want bigger goodness // if we STILL don't prefer one detection over the other, then pick // any deterministic criterion. for (int i = 0; i < 4; i++) { pref = prefer_smaller(pref, det0->p[i][0], det1->p[i][0]); pref = prefer_smaller(pref, det0->p[i][1], det1->p[i][1]); } if (pref == 0) { // at this point, we should only be undecided if the tag detections // are *exactly* the same. How would that happen? // printf("uh oh, no preference for overlappingdetection\n"); } if (pref < 0) { // keep det0, destroy det1 apriltag_detection_destroy(det1); zarray_remove_index(detections, i1, 1); i1--; // retry the same index goto retry1; } else { // keep det1, destroy det0 apriltag_detection_destroy(det0); zarray_remove_index(detections, i0, 1); i0--; // retry the same index. goto retry0; } } retry1: ; } retry0: ; } zarray_destroy(poly0); zarray_destroy(poly1); } for (int i = 0; i < zarray_size(quads); i++) { struct quad *quad; zarray_get_volatile(quads, i, &quad); matd_destroy(quad->H); matd_destroy(quad->Hinv); } zarray_destroy(quads); zarray_sort(detections, detection_compare_function); return detections; } // Call this method on each of the tags returned by apriltag_detector_detect void apriltag_detections_destroy(zarray_t *detections) { for (int i = 0; i < zarray_size(detections); i++) { apriltag_detection_t *det; zarray_get(detections, i, &det); apriltag_detection_destroy(det); } zarray_destroy(detections); } //////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////// void imlib_find_apriltags(list_t *out, image_t *ptr, rectangle_t *roi, apriltag_families_t families, float fx, float fy, float cx, float cy) { // Frame Buffer Memory Usage... // -> GRAYSCALE Input Image = w*h*1 // -> GRAYSCALE Threhsolded Image = w*h*1 // -> UnionFind = w*h*2 (+w*h*1 for hash table) size_t resolution = roi->w * roi->h; size_t fb_alloc_need = resolution * (1 + 1 + 2 + 1); // read above... umm_init_x(((fb_avail() - fb_alloc_need) / resolution) * resolution); apriltag_detector_t *td = apriltag_detector_create(); #ifdef IMLIB_ENABLE_APRILTAGS_TAG16H5 if (families & TAG16H5) { apriltag_detector_add_family(td, (apriltag_family_t *) &tag16h5); } #endif #ifdef IMLIB_ENABLE_APRILTAGS_TAG25H7 if (families & TAG25H7) { apriltag_detector_add_family(td, (apriltag_family_t *) &tag25h7); } #endif #ifdef IMLIB_ENABLE_APRILTAGS_TAG25H9 if (families & TAG25H9) { apriltag_detector_add_family(td, (apriltag_family_t *) &tag25h9); } #endif #ifdef IMLIB_ENABLE_APRILTAGS_TAG36H10 if (families & TAG36H10) { apriltag_detector_add_family(td, (apriltag_family_t *) &tag36h10); } #endif #ifdef IMLIB_ENABLE_APRILTAGS_TAG36H11 if (families & TAG36H11) { apriltag_detector_add_family(td, (apriltag_family_t *) &tag36h11); } #endif #ifdef IMLIB_ENABLE_APRILTAGS_ARTOOLKIT if (families & ARTOOLKIT) { apriltag_detector_add_family(td, (apriltag_family_t *) &artoolkit); } #endif image_t img; img.w = roi->w; img.h = roi->h; img.pixfmt = PIXFORMAT_GRAYSCALE; img.data = fb_alloc(image_size(&img), FB_ALLOC_NO_HINT); imlib_draw_image(&img, ptr, 0, 0, 1.f, 1.f, roi, -1, 255, NULL, NULL, 0, NULL, NULL, NULL, NULL); image_u8_t im; im.width = roi->w; im.height = roi->h; im.stride = roi->w; im.buf = img.data; zarray_t *detections = apriltag_detector_detect(td, &im); list_init(out, sizeof(find_apriltags_list_lnk_data_t)); for (int i = 0, j = zarray_size(detections); i < j; i++) { apriltag_detection_t *det; zarray_get(detections, i, &det); find_apriltags_list_lnk_data_t lnk_data; rectangle_init(&(lnk_data.rect), fast_roundf(det->p[0][0]) + roi->x, fast_roundf(det->p[0][1]) + roi->y, 0, 0); for (size_t k = 1, l = (sizeof(det->p) / sizeof(det->p[0])); k < l; k++) { rectangle_t temp; rectangle_init(&temp, fast_roundf(det->p[k][0]) + roi->x, fast_roundf(det->p[k][1]) + roi->y, 0, 0); rectangle_united(&(lnk_data.rect), &temp); } // Add corners... lnk_data.corners[0].x = fast_roundf(det->p[3][0]) + roi->x; // top-left lnk_data.corners[0].y = fast_roundf(det->p[3][1]) + roi->y; // top-left lnk_data.corners[1].x = fast_roundf(det->p[2][0]) + roi->x; // top-right lnk_data.corners[1].y = fast_roundf(det->p[2][1]) + roi->y; // top-right lnk_data.corners[2].x = fast_roundf(det->p[1][0]) + roi->x; // bottom-right lnk_data.corners[2].y = fast_roundf(det->p[1][1]) + roi->y; // bottom-right lnk_data.corners[3].x = fast_roundf(det->p[0][0]) + roi->x; // bottom-left lnk_data.corners[3].y = fast_roundf(det->p[0][1]) + roi->y; // bottom-left lnk_data.id = det->id; lnk_data.family = 0; #ifdef IMLIB_ENABLE_APRILTAGS_TAG16H5 if(det->family == &tag16h5) { lnk_data.family |= TAG16H5; } #endif #ifdef IMLIB_ENABLE_APRILTAGS_TAG25H7 if(det->family == &tag25h7) { lnk_data.family |= TAG25H7; } #endif #ifdef IMLIB_ENABLE_APRILTAGS_TAG25H9 if(det->family == &tag25h9) { lnk_data.family |= TAG25H9; } #endif #ifdef IMLIB_ENABLE_APRILTAGS_TAG36H10 if(det->family == &tag36h10) { lnk_data.family |= TAG36H10; } #endif #ifdef IMLIB_ENABLE_APRILTAGS_TAG36H11 if(det->family == &tag36h11) { lnk_data.family |= TAG36H11; } #endif #ifdef IMLIB_ENABLE_APRILTAGS_ARTOOLKIT if(det->family == &artoolkit) { lnk_data.family |= ARTOOLKIT; } #endif lnk_data.hamming = det->hamming; lnk_data.centroid_x = det->c[0] + roi->x; lnk_data.centroid_y = det->c[1] + roi->y; lnk_data.goodness = det->goodness / 255.0; // scale to [0:1] lnk_data.decision_margin = det->decision_margin / 255.0; // scale to [0:1] matd_t *pose = homography_to_pose(det->H, -fx, fy, cx, cy); lnk_data.x_translation = MATD_EL(pose, 0, 3); lnk_data.y_translation = MATD_EL(pose, 1, 3); lnk_data.z_translation = MATD_EL(pose, 2, 3); lnk_data.x_rotation = fast_atan2f(MATD_EL(pose, 2, 1), MATD_EL(pose, 2, 2)); lnk_data.y_rotation = fast_atan2f(-MATD_EL(pose, 2, 0), fast_sqrtf(sq(MATD_EL(pose, 2, 1)) + sq(MATD_EL(pose, 2, 2)))); lnk_data.z_rotation = fast_atan2f(MATD_EL(pose, 1, 0), MATD_EL(pose, 0, 0)); matd_destroy(pose); list_push_back(out, &lnk_data); } apriltag_detections_destroy(detections); fb_free(); // grayscale_image; apriltag_detector_destroy(td); fb_free(); // umm_init_x(); } #ifdef IMLIB_ENABLE_FIND_RECTS void imlib_find_rects(list_t *out, image_t *ptr, rectangle_t *roi, uint32_t threshold) { // Frame Buffer Memory Usage... // -> GRAYSCALE Input Image = w*h*1 // -> GRAYSCALE Threhsolded Image = w*h*1 // -> UnionFind = w*h*2 (+w*h*1 for hash table) size_t resolution = roi->w * roi->h; size_t fb_alloc_need = resolution * (1 + 1 + 2 + 2); // read above... umm_init_x(((fb_avail() - fb_alloc_need) / resolution) * resolution); apriltag_detector_t *td = apriltag_detector_create(); image_t img; img.w = roi->w; img.h = roi->h; img.pixfmt = PIXFORMAT_GRAYSCALE; img.data = fb_alloc(image_size(&img), FB_ALLOC_NO_HINT); imlib_draw_image(&img, ptr, 0, 0, 1.f, 1.f, roi, -1, 255, NULL, NULL, 0, NULL, NULL, NULL, NULL); image_u8_t im; im.width = roi->w; im.height = roi->h; im.stride = roi->w; im.buf = img.data; /////////////////////////////////////////////////////////// // Detect quads according to requested image decimation // and blurring parameters. // zarray_t *detections = apriltag_quad_gradient(td, &im, true); zarray_t *detections = apriltag_quad_thresh(td, &im, true); td->nquads = zarray_size(detections); //////////////////////////////////////////////////////////////// // Decode tags from each quad. if (1) { for (int i = 0; i < zarray_size(detections); i++) { struct quad *quad_original; zarray_get_volatile(detections, i, &quad_original); // refine edges is not dependent upon the tag family, thus // apply this optimization BEFORE the other work. //if (td->quad_decimate > 1 && td->refine_edges) { if (td->refine_edges) { refine_edges(td, &im, quad_original); } // make sure the homographies are computed... if (quad_update_homographies(quad_original)) continue; } } //////////////////////////////////////////////////////////////// // Reconcile detections--- don't report the same tag more // than once. (Allow non-overlapping duplicate detections.) if (1) { zarray_t *poly0 = g2d_polygon_create_zeros(4); zarray_t *poly1 = g2d_polygon_create_zeros(4); for (int i0 = 0; i0 < zarray_size(detections); i0++) { struct quad *det0; zarray_get_volatile(detections, i0, &det0); for (int k = 0; k < 4; k++) zarray_set(poly0, k, det0->p[k], NULL); for (int i1 = i0+1; i1 < zarray_size(detections); i1++) { struct quad *det1; zarray_get_volatile(detections, i1, &det1); for (int k = 0; k < 4; k++) zarray_set(poly1, k, det1->p[k], NULL); if (g2d_polygon_overlaps_polygon(poly0, poly1)) { // the tags overlap. Delete one, keep the other. int pref = 0; // 0 means undecided which one we'll keep. // if we STILL don't prefer one detection over the other, then pick // any deterministic criterion. for (int i = 0; i < 4; i++) { pref = prefer_smaller(pref, det0->p[i][0], det1->p[i][0]); pref = prefer_smaller(pref, det0->p[i][1], det1->p[i][1]); } if (pref == 0) { // at this point, we should only be undecided if the tag detections // are *exactly* the same. How would that happen? // printf("uh oh, no preference for overlappingdetection\n"); } if (pref < 0) { // keep det0, destroy det1 matd_destroy(det1->H); matd_destroy(det1->Hinv); zarray_remove_index(detections, i1, 1); i1--; // retry the same index goto retry1; } else { // keep det1, destroy det0 matd_destroy(det0->H); matd_destroy(det0->Hinv); zarray_remove_index(detections, i0, 1); i0--; // retry the same index. goto retry0; } } retry1: ; } retry0: ; } zarray_destroy(poly0); zarray_destroy(poly1); } list_init(out, sizeof(find_rects_list_lnk_data_t)); const int r_diag_len = fast_roundf(fast_sqrtf((roi->w * roi->w) + (roi->h * roi->h))) * 2; int *theta_buffer = fb_alloc(sizeof(int) * r_diag_len, FB_ALLOC_NO_HINT); uint32_t *mag_buffer = fb_alloc(sizeof(uint32_t) * r_diag_len, FB_ALLOC_NO_HINT); point_t *point_buffer = fb_alloc(sizeof(point_t) * r_diag_len, FB_ALLOC_NO_HINT); for (int i = 0, j = zarray_size(detections); i < j; i++) { struct quad *det; zarray_get_volatile(detections, i, &det); line_t lines[4]; lines[0].x1 = fast_roundf(det->p[0][0]) + roi->x; lines[0].y1 = fast_roundf(det->p[0][1]) + roi->y; lines[0].x2 = fast_roundf(det->p[1][0]) + roi->x; lines[0].y2 = fast_roundf(det->p[1][1]) + roi->y; lines[1].x1 = fast_roundf(det->p[1][0]) + roi->x; lines[1].y1 = fast_roundf(det->p[1][1]) + roi->y; lines[1].x2 = fast_roundf(det->p[2][0]) + roi->x; lines[1].y2 = fast_roundf(det->p[2][1]) + roi->y; lines[2].x1 = fast_roundf(det->p[2][0]) + roi->x; lines[2].y1 = fast_roundf(det->p[2][1]) + roi->y; lines[2].x2 = fast_roundf(det->p[3][0]) + roi->x; lines[2].y2 = fast_roundf(det->p[3][1]) + roi->y; lines[3].x1 = fast_roundf(det->p[3][0]) + roi->x; lines[3].y1 = fast_roundf(det->p[3][1]) + roi->y; lines[3].x2 = fast_roundf(det->p[0][0]) + roi->x; lines[3].y2 = fast_roundf(det->p[0][1]) + roi->y; uint32_t magnitude = 0; for (int i = 0; i < 4; i++) { if(!lb_clip_line(&lines[i], 0, 0, ptr->w, ptr->h)) { continue; } size_t index = trace_line(ptr, &lines[i], theta_buffer, mag_buffer, point_buffer); for (int j = 0; j < index; j++) { magnitude += mag_buffer[j]; } } if (magnitude < threshold) { continue; } find_rects_list_lnk_data_t lnk_data; rectangle_init(&(lnk_data.rect), fast_roundf(det->p[0][0]) + roi->x, fast_roundf(det->p[0][1]) + roi->y, 0, 0); for (size_t k = 1, l = (sizeof(det->p) / sizeof(det->p[0])); k < l; k++) { rectangle_t temp; rectangle_init(&temp, fast_roundf(det->p[k][0]) + roi->x, fast_roundf(det->p[k][1]) + roi->y, 0, 0); rectangle_united(&(lnk_data.rect), &temp); } // Add corners... lnk_data.corners[0].x = fast_roundf(det->p[3][0]) + roi->x; // top-left lnk_data.corners[0].y = fast_roundf(det->p[3][1]) + roi->y; // top-left lnk_data.corners[1].x = fast_roundf(det->p[2][0]) + roi->x; // top-right lnk_data.corners[1].y = fast_roundf(det->p[2][1]) + roi->y; // top-right lnk_data.corners[2].x = fast_roundf(det->p[1][0]) + roi->x; // bottom-right lnk_data.corners[2].y = fast_roundf(det->p[1][1]) + roi->y; // bottom-right lnk_data.corners[3].x = fast_roundf(det->p[0][0]) + roi->x; // bottom-left lnk_data.corners[3].y = fast_roundf(det->p[0][1]) + roi->y; // bottom-left lnk_data.magnitude = magnitude; list_push_back(out, &lnk_data); } fb_free(); // point_buffer fb_free(); // mag_buffer fb_free(); // theta_buffer zarray_destroy(detections); fb_free(); // grayscale_image; apriltag_detector_destroy(td); fb_free(); // umm_init_x(); } #endif //IMLIB_ENABLE_FIND_RECTS #ifdef IMLIB_ENABLE_ROTATION_CORR // http://jepsonsblog.blogspot.com/2012/11/rotation-in-3d-using-opencvs.html void imlib_rotation_corr(image_t *img, float x_rotation, float y_rotation, float z_rotation, float x_translation, float y_translation, float zoom, float fov, float *corners) { // Create a tmp copy of the image to pull pixels from. size_t size = image_size(img); void *data = fb_alloc(size, FB_ALLOC_NO_HINT); memcpy(data, img->data, size); memset(img->data, 0, size); umm_init_x(fb_avail()); int w = img->w; int h = img->h; float z = (fast_sqrtf((w * w) + (h * h)) / 2) / tanf(fov / 2); float z_z = z * zoom; matd_t *A1 = matd_create(4, 3); MATD_EL(A1, 0, 0) = 1; MATD_EL(A1, 0, 1) = 0; MATD_EL(A1, 0, 2) = -w / 2; MATD_EL(A1, 1, 0) = 0; MATD_EL(A1, 1, 1) = 1; MATD_EL(A1, 1, 2) = -h / 2; MATD_EL(A1, 2, 0) = 0; MATD_EL(A1, 2, 1) = 0; MATD_EL(A1, 2, 2) = 0; MATD_EL(A1, 3, 0) = 0; MATD_EL(A1, 3, 1) = 0; MATD_EL(A1, 3, 2) = 1; // needed for z translation matd_t *RX = matd_create(4, 4); MATD_EL(RX, 0, 0) = 1; MATD_EL(RX, 0, 1) = 0; MATD_EL(RX, 0, 2) = 0; MATD_EL(RX, 0, 3) = 0; MATD_EL(RX, 1, 0) = 0; MATD_EL(RX, 1, 1) = +cosf(x_rotation); MATD_EL(RX, 1, 2) = -sinf(x_rotation); MATD_EL(RX, 1, 3) = 0; MATD_EL(RX, 2, 0) = 0; MATD_EL(RX, 2, 1) = +sinf(x_rotation); MATD_EL(RX, 2, 2) = +cosf(x_rotation); MATD_EL(RX, 2, 3) = 0; MATD_EL(RX, 3, 0) = 0; MATD_EL(RX, 3, 1) = 0; MATD_EL(RX, 3, 2) = 0; MATD_EL(RX, 3, 3) = 1; matd_t *RY = matd_create(4, 4); MATD_EL(RY, 0, 0) = +cosf(y_rotation); MATD_EL(RY, 0, 1) = 0; MATD_EL(RY, 0, 2) = -sinf(y_rotation); MATD_EL(RY, 0, 3) = 0; MATD_EL(RY, 1, 0) = 0; MATD_EL(RY, 1, 1) = 1; MATD_EL(RY, 1, 2) = 0; MATD_EL(RY, 1, 3) = 0; MATD_EL(RY, 2, 0) = +sinf(y_rotation); MATD_EL(RY, 2, 1) = 0; MATD_EL(RY, 2, 2) = +cosf(y_rotation); MATD_EL(RY, 2, 3) = 0; MATD_EL(RY, 3, 0) = 0; MATD_EL(RY, 3, 1) = 0; MATD_EL(RY, 3, 2) = 0; MATD_EL(RY, 3, 3) = 1; matd_t *RZ = matd_create(4, 4); MATD_EL(RZ, 0, 0) = +cosf(z_rotation); MATD_EL(RZ, 0, 1) = -sinf(z_rotation); MATD_EL(RZ, 0, 2) = 0; MATD_EL(RZ, 0, 3) = 0; MATD_EL(RZ, 1, 0) = +sinf(z_rotation); MATD_EL(RZ, 1, 1) = +cosf(z_rotation); MATD_EL(RZ, 1, 2) = 0; MATD_EL(RZ, 1, 3) = 0; MATD_EL(RZ, 2, 0) = 0; MATD_EL(RZ, 2, 1) = 0; MATD_EL(RZ, 2, 2) = 1; MATD_EL(RZ, 2, 3) = 0; MATD_EL(RZ, 3, 0) = 0; MATD_EL(RZ, 3, 1) = 0; MATD_EL(RZ, 3, 2) = 0; MATD_EL(RZ, 3, 3) = 1; matd_t *R = matd_op("M*M*M", RX, RY, RZ); matd_t *T = matd_create(4, 4); MATD_EL(T, 0, 0) = 1; MATD_EL(T, 0, 1) = 0; MATD_EL(T, 0, 2) = 0; MATD_EL(T, 0, 3) = x_translation; MATD_EL(T, 1, 0) = 0; MATD_EL(T, 1, 1) = 1; MATD_EL(T, 1, 2) = 0; MATD_EL(T, 1, 3) = y_translation; MATD_EL(T, 2, 0) = 0; MATD_EL(T, 2, 1) = 0; MATD_EL(T, 2, 2) = 1; MATD_EL(T, 2, 3) = z; MATD_EL(T, 3, 0) = 0; MATD_EL(T, 3, 1) = 0; MATD_EL(T, 3, 2) = 0; MATD_EL(T, 3, 3) = 1; matd_t *A2 = matd_create(3, 4); MATD_EL(A2, 0, 0) = z_z; MATD_EL(A2, 0, 1) = 0; MATD_EL(A2, 0, 2) = w / 2; MATD_EL(A2, 0, 3) = 0; MATD_EL(A2, 1, 0) = 0; MATD_EL(A2, 1, 1) = z_z; MATD_EL(A2, 1, 2) = h / 2; MATD_EL(A2, 1, 3) = 0; MATD_EL(A2, 2, 0) = 0; MATD_EL(A2, 2, 1) = 0; MATD_EL(A2, 2, 2) = 1; MATD_EL(A2, 2, 3) = 0; matd_t *T1 = matd_op("M*M", R, A1); matd_t *T2 = matd_op("M*M", T, T1); matd_t *T3 = matd_op("M*M", A2, T2); matd_t *T4 = matd_inverse(T3); if (T4 && corners) { float corr[4]; zarray_t *correspondences = zarray_create(sizeof(float[4])); corr[0] = 0; corr[1] = 0; corr[2] = corners[0]; corr[3] = corners[1]; zarray_add(correspondences, &corr); corr[0] = w - 1; corr[1] = 0; corr[2] = corners[2]; corr[3] = corners[3]; zarray_add(correspondences, &corr); corr[0] = w - 1; corr[1] = h - 1; corr[2] = corners[4]; corr[3] = corners[5]; zarray_add(correspondences, &corr); corr[0] = 0; corr[1] = h - 1; corr[2] = corners[6]; corr[3] = corners[7]; zarray_add(correspondences, &corr); matd_t *H = homography_compute(correspondences, HOMOGRAPHY_COMPUTE_FLAG_INVERSE); if (!H) { // try again... H = homography_compute(correspondences, HOMOGRAPHY_COMPUTE_FLAG_SVD); } if (H) { matd_t *T5 = matd_op("M*M", H, T4); matd_destroy(H); matd_destroy(T4); T4 = T5; } zarray_destroy(correspondences); } if (T4) { float T4_00 = MATD_EL(T4, 0, 0), T4_01 = MATD_EL(T4, 0, 1), T4_02 = MATD_EL(T4, 0, 2); float T4_10 = MATD_EL(T4, 1, 0), T4_11 = MATD_EL(T4, 1, 1), T4_12 = MATD_EL(T4, 1, 2); float T4_20 = MATD_EL(T4, 2, 0), T4_21 = MATD_EL(T4, 2, 1), T4_22 = MATD_EL(T4, 2, 2); if ((fast_fabsf(T4_20) < MATD_EPS) && (fast_fabsf(T4_21) < MATD_EPS)) { // warp affine T4_00 /= T4_22; T4_01 /= T4_22; T4_02 /= T4_22; T4_10 /= T4_22; T4_11 /= T4_22; T4_12 /= T4_22; switch (img->pixfmt) { case PIXFORMAT_BINARY: { uint32_t *tmp = (uint32_t *) data; for (int y = 0, yy = h; y < yy; y++) { uint32_t *row_ptr = IMAGE_COMPUTE_BINARY_PIXEL_ROW_PTR(img, y); for (int x = 0, xx = w; x < xx; x++) { int sourceX = fast_roundf(T4_00*x + T4_01*y + T4_02); int sourceY = fast_roundf(T4_10*x + T4_11*y + T4_12); if ((0 <= sourceX) && (sourceX < w) && (0 <= sourceY) && (sourceY < h)) { uint32_t *ptr = tmp + (((w + UINT32_T_MASK) >> UINT32_T_SHIFT) * sourceY); int pixel = IMAGE_GET_BINARY_PIXEL_FAST(ptr, sourceX); IMAGE_PUT_BINARY_PIXEL_FAST(row_ptr, x, pixel); } } } break; } case PIXFORMAT_GRAYSCALE: { uint8_t *tmp = (uint8_t *) data; for (int y = 0, yy = h; y < yy; y++) { uint8_t *row_ptr = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(img, y); for (int x = 0, xx = w; x < xx; x++) { int sourceX = fast_roundf(T4_00*x + T4_01*y + T4_02); int sourceY = fast_roundf(T4_10*x + T4_11*y + T4_12); if ((0 <= sourceX) && (sourceX < w) && (0 <= sourceY) && (sourceY < h)) { uint8_t *ptr = tmp + (w * sourceY); int pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(ptr, sourceX); IMAGE_PUT_GRAYSCALE_PIXEL_FAST(row_ptr, x, pixel); } } } break; } case PIXFORMAT_RGB565: { uint16_t *tmp = (uint16_t *) data; for (int y = 0, yy = h; y < yy; y++) { uint16_t *row_ptr = IMAGE_COMPUTE_RGB565_PIXEL_ROW_PTR(img, y); for (int x = 0, xx = w; x < xx; x++) { int sourceX = fast_roundf(T4_00*x + T4_01*y + T4_02); int sourceY = fast_roundf(T4_10*x + T4_11*y + T4_12); if ((0 <= sourceX) && (sourceX < w) && (0 <= sourceY) && (sourceY < h)) { uint16_t *ptr = tmp + (w * sourceY); int pixel = IMAGE_GET_RGB565_PIXEL_FAST(ptr, sourceX); IMAGE_PUT_RGB565_PIXEL_FAST(row_ptr, x, pixel); } } } break; } default: { break; } } } else { // warp perspective switch (img->pixfmt) { case PIXFORMAT_BINARY: { uint32_t *tmp = (uint32_t *) data; for (int y = 0, yy = h; y < yy; y++) { uint32_t *row_ptr = IMAGE_COMPUTE_BINARY_PIXEL_ROW_PTR(img, y); for (int x = 0, xx = w; x < xx; x++) { float xxx = T4_00*x + T4_01*y + T4_02; float yyy = T4_10*x + T4_11*y + T4_12; float zzz = T4_20*x + T4_21*y + T4_22; int sourceX = fast_roundf(xxx / zzz); int sourceY = fast_roundf(yyy / zzz); if ((0 <= sourceX) && (sourceX < w) && (0 <= sourceY) && (sourceY < h)) { uint32_t *ptr = tmp + (((w + UINT32_T_MASK) >> UINT32_T_SHIFT) * sourceY); int pixel = IMAGE_GET_BINARY_PIXEL_FAST(ptr, sourceX); IMAGE_PUT_BINARY_PIXEL_FAST(row_ptr, x, pixel); } } } break; } case PIXFORMAT_GRAYSCALE: { uint8_t *tmp = (uint8_t *) data; for (int y = 0, yy = h; y < yy; y++) { uint8_t *row_ptr = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(img, y); for (int x = 0, xx = w; x < xx; x++) { float xxx = T4_00*x + T4_01*y + T4_02; float yyy = T4_10*x + T4_11*y + T4_12; float zzz = T4_20*x + T4_21*y + T4_22; int sourceX = fast_roundf(xxx / zzz); int sourceY = fast_roundf(yyy / zzz); if ((0 <= sourceX) && (sourceX < w) && (0 <= sourceY) && (sourceY < h)) { uint8_t *ptr = tmp + (w * sourceY); int pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(ptr, sourceX); IMAGE_PUT_GRAYSCALE_PIXEL_FAST(row_ptr, x, pixel); } } } break; } case PIXFORMAT_RGB565: { uint16_t *tmp = (uint16_t *) data; for (int y = 0, yy = h; y < yy; y++) { uint16_t *row_ptr = IMAGE_COMPUTE_RGB565_PIXEL_ROW_PTR(img, y); for (int x = 0, xx = w; x < xx; x++) { float xxx = T4_00*x + T4_01*y + T4_02; float yyy = T4_10*x + T4_11*y + T4_12; float zzz = T4_20*x + T4_21*y + T4_22; int sourceX = fast_roundf(xxx / zzz); int sourceY = fast_roundf(yyy / zzz); if ((0 <= sourceX) && (sourceX < w) && (0 <= sourceY) && (sourceY < h)) { uint16_t *ptr = tmp + (w * sourceY); int pixel = IMAGE_GET_RGB565_PIXEL_FAST(ptr, sourceX); IMAGE_PUT_RGB565_PIXEL_FAST(row_ptr, x, pixel); } } } break; } default: { break; } } } matd_destroy(T4); } matd_destroy(T3); matd_destroy(T2); matd_destroy(T1); matd_destroy(A2); matd_destroy(T); matd_destroy(R); matd_destroy(RZ); matd_destroy(RY); matd_destroy(RX); matd_destroy(A1); fb_free(); // umm_init_x(); fb_free(); } #endif //IMLIB_ENABLE_ROTATION_CORR *INDENT-ON* #pragma GCC diagnostic pop