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/timeprofile.h b/common/timeprofile.h
new file mode 100644
index 0000000..8016386
--- /dev/null
+++ b/common/timeprofile.h
@@ -0,0 +1,120 @@
+/* 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.
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdio.h>
+#include <string.h>
+#include <stdint.h>
+
+#include "time_util.h"
+#include "zarray.h"
+
+struct timeprofile_entry
+{
+ char name[32];
+ int64_t utime;
+};
+
+typedef struct timeprofile timeprofile_t;
+struct timeprofile
+{
+ int64_t utime;
+ zarray_t *stamps;
+};
+
+static inline timeprofile_t *timeprofile_create()
+{
+ timeprofile_t *tp = (timeprofile_t*) calloc(1, sizeof(timeprofile_t));
+ tp->stamps = zarray_create(sizeof(struct timeprofile_entry));
+
+ tp->utime = utime_now();
+
+ return tp;
+}
+
+static inline void timeprofile_destroy(timeprofile_t *tp)
+{
+ zarray_destroy(tp->stamps);
+ free(tp);
+}
+
+static inline void timeprofile_clear(timeprofile_t *tp)
+{
+ zarray_clear(tp->stamps);
+ tp->utime = utime_now();
+}
+
+static inline void timeprofile_stamp(timeprofile_t *tp, const char *name)
+{
+ struct timeprofile_entry tpe;
+
+ strncpy(tpe.name, name, sizeof(tpe.name));
+ tpe.name[sizeof(tpe.name)-1] = 0;
+ tpe.utime = utime_now();
+
+ zarray_add(tp->stamps, &tpe);
+}
+
+static inline void timeprofile_display(timeprofile_t *tp)
+{
+ int64_t lastutime = tp->utime;
+
+ for (int i = 0; i < zarray_size(tp->stamps); i++) {
+ struct timeprofile_entry *stamp;
+
+ zarray_get_volatile(tp->stamps, i, &stamp);
+
+ double cumtime = (stamp->utime - tp->utime)/1000000.0;
+
+ double parttime = (stamp->utime - lastutime)/1000000.0;
+
+ printf("%2d %32s %15f ms %15f ms\n", i, stamp->name, parttime*1000, cumtime*1000);
+
+ lastutime = stamp->utime;
+ }
+}
+
+static inline uint64_t timeprofile_total_utime(timeprofile_t *tp)
+{
+ if (zarray_size(tp->stamps) == 0)
+ return 0;
+
+ struct timeprofile_entry *first, *last;
+ zarray_get_volatile(tp->stamps, 0, &first);
+ zarray_get_volatile(tp->stamps, zarray_size(tp->stamps) - 1, &last);
+
+ return last->utime - first->utime;
+}
+
+#ifdef __cplusplus
+}
+#endif