Squashed 'third_party/apriltag/' content from commit 3e8e974d0
git-subtree-dir: third_party/apriltag
git-subtree-split: 3e8e974d0d8d6ab318abf56d87506d15d7f2cc35
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
Change-Id: I04ba3cb2106b6813a1013d57aa8074c26f856598
diff --git a/common/zmaxheap.c b/common/zmaxheap.c
new file mode 100644
index 0000000..e04d03e
--- /dev/null
+++ b/common/zmaxheap.c
@@ -0,0 +1,422 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson@umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+#include <assert.h>
+#include <stdint.h>
+
+#include "zmaxheap.h"
+#include "debug_print.h"
+
+#ifdef _WIN32
+static inline long int random(void)
+{
+ return rand();
+}
+#endif
+
+// 0
+// 1 2
+// 3 4 5 6
+// 7 8 9 10 11 12 13 14
+//
+// Children of node i: 2*i+1, 2*i+2
+// Parent of node i: (i-1) / 2
+//
+// Heap property: a parent is greater than (or equal to) its children.
+
+#define MIN_CAPACITY 16
+
+struct zmaxheap
+{
+ size_t el_sz;
+
+ int size;
+ int alloc;
+
+ float *values;
+ char *data;
+
+ void (*swap)(zmaxheap_t *heap, int a, int b);
+};
+
+static inline void swap_default(zmaxheap_t *heap, int a, int b)
+{
+ float t = heap->values[a];
+ heap->values[a] = heap->values[b];
+ heap->values[b] = t;
+
+ char *tmp = malloc(sizeof(char)*heap->el_sz);
+ memcpy(tmp, &heap->data[a*heap->el_sz], heap->el_sz);
+ memcpy(&heap->data[a*heap->el_sz], &heap->data[b*heap->el_sz], heap->el_sz);
+ memcpy(&heap->data[b*heap->el_sz], tmp, heap->el_sz);
+ free(tmp);
+}
+
+static inline void swap_pointer(zmaxheap_t *heap, int a, int b)
+{
+ float t = heap->values[a];
+ heap->values[a] = heap->values[b];
+ heap->values[b] = t;
+
+ void **pp = (void**) heap->data;
+ void *tmp = pp[a];
+ pp[a] = pp[b];
+ pp[b] = tmp;
+}
+
+
+zmaxheap_t *zmaxheap_create(size_t el_sz)
+{
+ zmaxheap_t *heap = calloc(1, sizeof(zmaxheap_t));
+ heap->el_sz = el_sz;
+
+ heap->swap = swap_default;
+
+ if (el_sz == sizeof(void*))
+ heap->swap = swap_pointer;
+
+ return heap;
+}
+
+void zmaxheap_destroy(zmaxheap_t *heap)
+{
+ free(heap->values);
+ free(heap->data);
+ memset(heap, 0, sizeof(zmaxheap_t));
+ free(heap);
+}
+
+int zmaxheap_size(zmaxheap_t *heap)
+{
+ return heap->size;
+}
+
+void zmaxheap_ensure_capacity(zmaxheap_t *heap, int capacity)
+{
+ if (heap->alloc >= capacity)
+ return;
+
+ int newcap = heap->alloc;
+
+ while (newcap < capacity) {
+ if (newcap < MIN_CAPACITY) {
+ newcap = MIN_CAPACITY;
+ continue;
+ }
+
+ newcap *= 2;
+ }
+
+ heap->values = realloc(heap->values, newcap * sizeof(float));
+ heap->data = realloc(heap->data, newcap * heap->el_sz);
+ heap->alloc = newcap;
+}
+
+void zmaxheap_add(zmaxheap_t *heap, void *p, float v)
+{
+
+ assert (isfinite(v) && "zmaxheap_add: Trying to add non-finite number to heap. NaN's prohibited, could allow INF with testing");
+ zmaxheap_ensure_capacity(heap, heap->size + 1);
+
+ int idx = heap->size;
+
+ heap->values[idx] = v;
+ memcpy(&heap->data[idx*heap->el_sz], p, heap->el_sz);
+
+ heap->size++;
+
+ while (idx > 0) {
+
+ int parent = (idx - 1) / 2;
+
+ // we're done!
+ if (heap->values[parent] >= v)
+ break;
+
+ // else, swap and recurse upwards.
+ heap->swap(heap, idx, parent);
+ idx = parent;
+ }
+}
+
+void zmaxheap_vmap(zmaxheap_t *heap, void (*f)())
+{
+ assert(heap != NULL);
+ assert(f != NULL);
+ assert(heap->el_sz == sizeof(void*));
+
+ for (int idx = 0; idx < heap->size; idx++) {
+ void *p = NULL;
+ memcpy(&p, &heap->data[idx*heap->el_sz], heap->el_sz);
+ if (p == NULL) {
+ debug_print("Warning: zmaxheap_vmap item %d is NULL\n", idx);
+ }
+ f(p);
+ }
+}
+
+// Removes the item in the heap at the given index. Returns 1 if the
+// item existed. 0 Indicates an invalid idx (heap is smaller than
+// idx). This is mostly intended to be used by zmaxheap_remove_max.
+int zmaxheap_remove_index(zmaxheap_t *heap, int idx, void *p, float *v)
+{
+ if (idx >= heap->size)
+ return 0;
+
+ // copy out the requested element from the heap.
+ if (v != NULL)
+ *v = heap->values[idx];
+ if (p != NULL)
+ memcpy(p, &heap->data[idx*heap->el_sz], heap->el_sz);
+
+ heap->size--;
+
+ // If this element is already the last one, then there's nothing
+ // for us to do.
+ if (idx == heap->size)
+ return 1;
+
+ // copy last element to first element. (which probably upsets
+ // the heap property).
+ heap->values[idx] = heap->values[heap->size];
+ memcpy(&heap->data[idx*heap->el_sz], &heap->data[heap->el_sz * heap->size], heap->el_sz);
+
+ // now fix the heap. Note, as we descend, we're "pushing down"
+ // the same node the entire time. Thus, while the index of the
+ // parent might change, the parent_score doesn't.
+ int parent = idx;
+ float parent_score = heap->values[idx];
+
+ // descend, fixing the heap.
+ while (parent < heap->size) {
+
+ int left = 2*parent + 1;
+ int right = left + 1;
+
+// assert(parent_score == heap->values[parent]);
+
+ float left_score = (left < heap->size) ? heap->values[left] : -INFINITY;
+ float right_score = (right < heap->size) ? heap->values[right] : -INFINITY;
+
+ // put the biggest of (parent, left, right) as the parent.
+
+ // already okay?
+ if (parent_score >= left_score && parent_score >= right_score)
+ break;
+
+ // if we got here, then one of the children is bigger than the parent.
+ if (left_score >= right_score) {
+ assert(left < heap->size);
+ heap->swap(heap, parent, left);
+ parent = left;
+ } else {
+ // right_score can't be less than left_score if right_score is -INFINITY.
+ assert(right < heap->size);
+ heap->swap(heap, parent, right);
+ parent = right;
+ }
+ }
+
+ return 1;
+}
+
+int zmaxheap_remove_max(zmaxheap_t *heap, void *p, float *v)
+{
+ return zmaxheap_remove_index(heap, 0, p, v);
+}
+
+void zmaxheap_iterator_init(zmaxheap_t *heap, zmaxheap_iterator_t *it)
+{
+ memset(it, 0, sizeof(zmaxheap_iterator_t));
+ it->heap = heap;
+ it->in = 0;
+ it->out = 0;
+}
+
+int zmaxheap_iterator_next(zmaxheap_iterator_t *it, void *p, float *v)
+{
+ zmaxheap_t *heap = it->heap;
+
+ if (it->in >= zmaxheap_size(heap))
+ return 0;
+
+ *v = heap->values[it->in];
+ memcpy(p, &heap->data[it->in*heap->el_sz], heap->el_sz);
+
+ if (it->in != it->out) {
+ heap->values[it->out] = heap->values[it->in];
+ memcpy(&heap->data[it->out*heap->el_sz], &heap->data[it->in*heap->el_sz], heap->el_sz);
+ }
+
+ it->in++;
+ it->out++;
+ return 1;
+}
+
+int zmaxheap_iterator_next_volatile(zmaxheap_iterator_t *it, void *p, float *v)
+{
+ zmaxheap_t *heap = it->heap;
+
+ if (it->in >= zmaxheap_size(heap))
+ return 0;
+
+ *v = heap->values[it->in];
+ *((void**) p) = &heap->data[it->in*heap->el_sz];
+
+ if (it->in != it->out) {
+ heap->values[it->out] = heap->values[it->in];
+ memcpy(&heap->data[it->out*heap->el_sz], &heap->data[it->in*heap->el_sz], heap->el_sz);
+ }
+
+ it->in++;
+ it->out++;
+ return 1;
+}
+
+void zmaxheap_iterator_remove(zmaxheap_iterator_t *it)
+{
+ it->out--;
+}
+
+static void maxheapify(zmaxheap_t *heap, int parent)
+{
+ int left = 2*parent + 1;
+ int right = 2*parent + 2;
+
+ int betterchild = parent;
+
+ if (left < heap->size && heap->values[left] > heap->values[betterchild])
+ betterchild = left;
+ if (right < heap->size && heap->values[right] > heap->values[betterchild])
+ betterchild = right;
+
+ if (betterchild != parent) {
+ heap->swap(heap, parent, betterchild);
+ return maxheapify(heap, betterchild);
+ }
+}
+
+#if 0 //won't compile if defined but not used
+// test the heap property
+static void validate(zmaxheap_t *heap)
+{
+ for (int parent = 0; parent < heap->size; parent++) {
+ int left = 2*parent + 1;
+ int right = 2*parent + 2;
+
+ if (left < heap->size) {
+ assert(heap->values[parent] > heap->values[left]);
+ }
+
+ if (right < heap->size) {
+ assert(heap->values[parent] > heap->values[right]);
+ }
+ }
+}
+#endif
+void zmaxheap_iterator_finish(zmaxheap_iterator_t *it)
+{
+ // if nothing was removed, no work to do.
+ if (it->in == it->out)
+ return;
+
+ zmaxheap_t *heap = it->heap;
+
+ heap->size = it->out;
+
+ // restore heap property
+ for (int i = heap->size/2 - 1; i >= 0; i--)
+ maxheapify(heap, i);
+}
+
+void zmaxheap_test()
+{
+ int cap = 10000;
+ int sz = 0;
+ int32_t *vals = calloc(sizeof(int32_t), cap);
+
+ zmaxheap_t *heap = zmaxheap_create(sizeof(int32_t));
+
+ int maxsz = 0;
+ int zcnt = 0;
+
+ for (int iter = 0; iter < 5000000; iter++) {
+ assert(sz == heap->size);
+
+ if ((random() & 1) == 0 && sz < cap) {
+ // add a value
+ int32_t v = (int32_t) (random() / 1000);
+ float fv = v;
+ assert(v == fv);
+
+ vals[sz] = v;
+ zmaxheap_add(heap, &v, fv);
+ sz++;
+
+// printf("add %d %f\n", v, fv);
+ } else {
+ // remove a value
+ int maxv = -1, maxi = -1;
+
+ for (int i = 0; i < sz; i++) {
+ if (vals[i] > maxv) {
+ maxv = vals[i];
+ maxi = i;
+ }
+ }
+
+
+ int32_t outv;
+ float outfv;
+ int res = zmaxheap_remove_max(heap, &outv, &outfv);
+ if (sz == 0) {
+ assert(res == 0);
+ } else {
+// printf("%d %d %d %f\n", sz, maxv, outv, outfv);
+ assert(outv == outfv);
+ assert(maxv == outv);
+
+ // shuffle erase the maximum from our list.
+ vals[maxi] = vals[sz - 1];
+ sz--;
+ }
+ }
+
+ if (sz > maxsz)
+ maxsz = sz;
+
+ if (maxsz > 0 && sz == 0)
+ zcnt++;
+ }
+
+ printf("max size: %d, zcount %d\n", maxsz, zcnt);
+ free (vals);
+}